UASControlParameters.cpp 6.04 KB
Newer Older
Mariano Lizarraga's avatar
Mariano Lizarraga committed
1
#include "UASControlParameters.h"
2 3 4 5
#include "ui_UASControlParameters.h"

#define CONTROL_MODE_LOCKED "MODE LOCKED"
#define CONTROL_MODE_MANUAL "MODE MANUAL"
6 7

#ifdef MAVLINK_ENABLED_SLUGS
8 9 10 11
#define CONTROL_MODE_GUIDED "MODE MID-L CMDS"
#define CONTROL_MODE_AUTO   "MODE WAYPOINT"
#define CONTROL_MODE_TEST1  "MODE PASST"
#define CONTROL_MODE_TEST2  "MODE SEL PT"
12
#else
13 14 15 16
#define CONTROL_MODE_GUIDED "MODE GUIDED"
#define CONTROL_MODE_AUTO   "MODE AUTO"
#define CONTROL_MODE_TEST1  "MODE TEST1"
#define CONTROL_MODE_TEST2  "MODE TEST2"
17 18
#endif

19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37
#define CONTROL_MODE_READY  "MODE TEST3"
#define CONTROL_MODE_RC_TRAINING  "RC SIMULATION"

#define CONTROL_MODE_LOCKED_INDEX 1
#define CONTROL_MODE_MANUAL_INDEX 2
#define CONTROL_MODE_GUIDED_INDEX 3
#define CONTROL_MODE_AUTO_INDEX   4
#define CONTROL_MODE_TEST1_INDEX  5
#define CONTROL_MODE_TEST2_INDEX  6
#define CONTROL_MODE_TEST3_INDEX  7
#define CONTROL_MODE_READY_INDEX  8
#define CONTROL_MODE_RC_TRAINING_INDEX  9

UASControlParameters::UASControlParameters(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::UASControlParameters)
{
    ui->setupUi(this);

38 39
    ui->btSetCtrl->setStatusTip(tr("Set Passthrough"));

40 41
    connect(ui->btGetCommands, SIGNAL(clicked()), this, SLOT(getCommands()));

42
    connect(ui->btSetCtrl, SIGNAL(clicked()), this, SLOT(setPassthrough()));
43 44 45 46 47 48 49 50 51 52 53
}

UASControlParameters::~UASControlParameters()
{
    delete ui;
}

void UASControlParameters::changedMode(int mode)
{
    QString modeTemp;

54
    switch (mode) {
55
    case (uint8_t)MAV_MODE_PREFLIGHT:
56 57
        modeTemp = "LOCKED MODE";
        break;
lm's avatar
lm committed
58 59 60 61 62
    case (uint8_t)MAV_MODE_MANUAL_ARMED:
        modeTemp = "A/MANUAL MODE";
        break;
    case (uint8_t)MAV_MODE_MANUAL_DISARMED:
        modeTemp = "D/MANUAL MODE";
63
        break;
64
#ifdef MAVLINK_ENABLED_SLUGS
65
    case (uint8_t)MAV_MODE_AUTO:
66
        modeTemp = "WAYPOINT MODE";
67 68
        break;
    case (uint8_t)MAV_MODE_GUIDED:
69
        modeTemp = "MID-L CMDS MODE";
70
        break;
71 72 73 74 75 76

    case (uint8_t)MAV_MODE_TEST1:
        modeTemp = "PASST MODE";
        break;
    case (uint8_t)MAV_MODE_TEST2:
        modeTemp = "SEL PT MODE";
77
        break;
78
#endif
79
    default:
lm's avatar
lm committed
80
        modeTemp = "UNKNOWN MODE";
81
        break;
82 83
    }

84

85
    if(modeTemp != this->mode) {
86
        ui->lbMode->setStyleSheet("background-color: rgb(165, 42, 42)");
87
    } else {
88
        ui->lbMode->setStyleSheet("background-color: rgb(85, 107, 47)");
89 90 91 92 93
    }
}

void UASControlParameters::activeUasSet(UASInterface *uas)
{
94
    if(uas) {
95
        connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
96
        connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(speedChanged(UASInterface*,double,double,double,quint64)));
97 98 99
        connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
        connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
        connect(uas, SIGNAL(thrustChanged(UASInterface*,double)), this, SLOT(thrustChanged(UASInterface*,double)) );
100

101 102
        activeUAS= uas;
    }
103 104 105 106
}

void UASControlParameters::updateGlobalPosition(UASInterface * a, double b, double c, double aa, quint64 ab)
{
107 108 109 110
    Q_UNUSED(a);
    Q_UNUSED(b);
    Q_UNUSED(c);
    Q_UNUSED(ab);
111 112 113 114 115
    this->altitude=aa;
}

void UASControlParameters::speedChanged(UASInterface* uas, double vx, double vy, double vz, quint64 time)
{
116 117
    Q_UNUSED(time);
    Q_UNUSED(uas);
118 119 120 121 122 123 124
    this->speed = sqrt(pow(vx, 2.0) + pow(vy, 2.0) + pow(vz, 2.0));
    //ui->sbAirSpeed->setValue(speed);
}

void UASControlParameters::updateAttitude(UASInterface *uas, double roll, double pitch, double yaw, quint64 time)
{
    Q_UNUSED(uas);
125 126
    Q_UNUSED(pitch);
    Q_UNUSED(yaw);
127 128 129 130 131 132
    Q_UNUSED(time);
    //ui->sbTurnRate->setValue(roll);
    this->roll = roll;
}

void UASControlParameters::setCommands()
133
{
134 135
#ifdef MAVLINK_ENABLED_SLUGS
    if(this->activeUAS) {
136
        UAS* myUas= static_cast<UAS*>(this->activeUAS);
137

138
        mavlink_message_t msg;
139

140 141 142
        tempCmds.uCommand = ui->sbAirSpeed->value();
        tempCmds.hCommand = ui->sbHeight->value();
        tempCmds.rCommand = ui->sbTurnRate->value();
143

144 145 146
        mavlink_msg_mid_lvl_cmds_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &this->tempCmds);
        myUas->sendMessage(msg);
    }
147
#endif
148
}
149 150 151 152 153 154 155 156

void UASControlParameters::getCommands()
{
    ui->sbAirSpeed->setValue(this->speed);
    ui->sbHeight->setValue(this->altitude);
    ui->sbTurnRate->setValue(this->roll);
}

157 158
void UASControlParameters::setPassthrough()
{
159 160
#ifdef MAVLINK_ENABLED_SLUGS
    if(this->activeUAS) {
161
        UAS* myUas= static_cast<UAS*>(this->activeUAS);
162

163
        mavlink_message_t msg;
164

165
        int8_t tmpBit=0;
166

167
        if(ui->cxdle_c->isChecked()) { //left elevator command
168 169
            tmpBit+=8;
        }
170
        if(ui->cxdr_c->isChecked()) { //rudder command
171 172
            tmpBit+=16;
        }
173

174
        if(ui->cxdla_c->isChecked()) { //left aileron command
175 176
            tmpBit+=64;
        }
177
        if(ui->cxdt_c->isChecked()) { //throttle command
178 179
            tmpBit+=128;
        }
180

181 182 183
        generic_16bit r;
        r.b[1] = 0;
        r.b[0] = tmpBit;//255;
184

185 186 187 188 189 190 191
        tempCtrl.target= this->activeUAS->getUASID();
        tempCtrl.bitfieldPt= (uint16_t)r.s;

        mavlink_msg_ctrl_srfc_pt_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &this->tempCtrl);
        myUas->sendMessage(msg);
        //qDebug()<<tempCtrl.bitfieldPt;
    }
192
#endif
193 194
}

195 196 197 198 199 200
void UASControlParameters::updateMode(int uas,QString mode,QString description)
{
    Q_UNUSED(uas);
    Q_UNUSED(description);
    this->mode = mode;
    ui->lbMode->setText(this->mode);
201

202
    ui->lbMode->setStyleSheet("background-color: rgb(85, 107, 47)");
203 204 205 206 207 208 209
}

void UASControlParameters::thrustChanged(UASInterface *uas, double throttle)
{
    Q_UNUSED(uas);
    this->throttle= throttle;
}