UAS.cc 53.2 KB
Newer Older
pixhawk's avatar
pixhawk committed
1 2
/*=====================================================================

lm's avatar
lm committed
3
QGroundControl Open Source Ground Control Station
pixhawk's avatar
pixhawk committed
4

lm's avatar
lm committed
5
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
pixhawk's avatar
pixhawk committed
6

lm's avatar
lm committed
7
This file is part of the QGROUNDCONTROL project
pixhawk's avatar
pixhawk committed
8

lm's avatar
lm committed
9
    QGROUNDCONTROL is free software: you can redistribute it and/or modify
pixhawk's avatar
pixhawk committed
10 11 12 13
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

lm's avatar
lm committed
14
    QGROUNDCONTROL is distributed in the hope that it will be useful,
pixhawk's avatar
pixhawk committed
15 16 17 18 19
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
lm's avatar
lm committed
20
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
pixhawk's avatar
pixhawk committed
21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41

======================================================================*/

/**
 * @file
 *   @brief Represents one unmanned aerial vehicle
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QList>
#include <QMessageBox>
#include <QTimer>
#include <iostream>
#include <QDebug>
#include <cmath>
#include "UAS.h"
#include "LinkInterface.h"
#include "UASManager.h"
#include "MG.h"
42
#include "QGC.h"
pixhawk's avatar
pixhawk committed
43
#include "GAudioOutput.h"
44 45
#include "MAVLinkProtocol.h"
#include <mavlink.h>
pixhawk's avatar
pixhawk committed
46

47
UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
48
        uasId(id),
pixhawk's avatar
pixhawk committed
49 50 51 52
        startTime(MG::TIME::getGroundTimeNow()),
        commStatus(COMM_DISCONNECTED),
        name(""),
        links(new QList<LinkInterface*>()),
53
        unknownPackets(),
54
        mavlink(protocol),
55
        waypointManager(*this),
pixhawk's avatar
pixhawk committed
56
        thrustSum(0),
pixhawk's avatar
pixhawk committed
57
        thrustMax(10),
pixhawk's avatar
pixhawk committed
58
        startVoltage(0),
59 60
        currentVoltage(12.0f),
        lpVoltage(12.0f),
pixhawk's avatar
pixhawk committed
61 62 63
        mode(MAV_MODE_UNINIT),
        status(MAV_STATE_UNINIT),
        onboardTimeOffset(0),
pixhawk's avatar
pixhawk committed
64 65 66 67
        controlRollManual(true),
        controlPitchManual(true),
        controlYawManual(true),
        controlThrustManual(true),
pixhawk's avatar
pixhawk committed
68 69 70
        manualRollAngle(0),
        manualPitchAngle(0),
        manualYawAngle(0),
lm's avatar
lm committed
71 72
        manualThrust(0),
        receiveDropRate(0),
lm's avatar
lm committed
73
        sendDropRate(0),
74 75
        lowBattAlarm(false),
        positionLock(false),
lm's avatar
lm committed
76 77 78 79 80 81
        localX(0),
        localY(0),
        localZ(0),
        roll(0),
        pitch(0),
        yaw(0),
82
        statusTimeout(new QTimer(this))
pixhawk's avatar
pixhawk committed
83
{
84
    color = UASInterface::getNextColor();
pixhawk's avatar
pixhawk committed
85
    setBattery(LIPOLY, 3);
86 87
    statusTimeout->setInterval(500);
    connect(statusTimeout, SIGNAL(timeout()), this, SLOT(updateState()));
pixhawk's avatar
pixhawk committed
88 89 90 91 92 93 94
}

UAS::~UAS()
{
    delete links;
}

95
int UAS::getUASID() const
pixhawk's avatar
pixhawk committed
96 97 98 99
{
    return uasId;
}

100 101 102 103 104 105 106 107 108 109
void UAS::updateState()
{
    // Position lock is set by the MAVLink message handler
    // if no position lock is available, indicate an error
    if (positionLock)
    {
        positionLock = false;
    }
    else
    {
pixhawk's avatar
pixhawk committed
110
        if (mode > (uint8_t)MAV_MODE_LOCKED && positionLock)
111 112 113 114 115 116
        {
            GAudioOutput::instance()->notifyNegative();
        }
    }
}

pixhawk's avatar
pixhawk committed
117 118 119 120 121 122 123 124 125 126
void UAS::setSelected()
{
    UASManager::instance()->setActiveUAS(this);
}

void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
    if (!links->contains(link))
    {
        addLink(link);
127
//        qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName();
pixhawk's avatar
pixhawk committed
128
    }
129 130 131 132
//    else
//    {
//        qDebug() << __FILE__ << __LINE__ << "DID NOT ADD LINK" << link->getName() << "ALREADY IN LIST";
//    }
pixhawk's avatar
pixhawk committed
133

134
//    qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq;
pixhawk's avatar
pixhawk committed
135

pixhawk's avatar
pixhawk committed
136 137 138 139 140 141 142 143 144 145
    if (message.sysid == uasId)
    {
        QString uasState;
        QString stateDescription;
        QString patternPath;
        switch (message.msgid)
        {
        case MAVLINK_MSG_ID_HEARTBEAT:
            emit heartbeat(this);
            // Set new type if it has changed
pixhawk's avatar
pixhawk committed
146
            if (this->type != mavlink_msg_heartbeat_get_type(&message))
pixhawk's avatar
pixhawk committed
147
            {
pixhawk's avatar
pixhawk committed
148
                this->type = mavlink_msg_heartbeat_get_type(&message);
pixhawk's avatar
pixhawk committed
149 150 151 152 153 154 155 156
                emit systemTypeSet(this, type);
            }
            break;
        case MAVLINK_MSG_ID_BOOT:
            getStatusForCode((int)MAV_STATE_BOOT, uasState, stateDescription);
            emit statusChanged(this, uasState, stateDescription);
            onboardTimeOffset = 0; // Reset offset measurement
            break;
pixhawk's avatar
pixhawk committed
157 158
        case MAVLINK_MSG_ID_SYS_STATUS:
            {
pixhawk's avatar
pixhawk committed
159 160
                mavlink_sys_status_t state;
                mavlink_msg_sys_status_decode(&message, &state);
pixhawk's avatar
pixhawk committed
161

pixhawk's avatar
pixhawk committed
162
                // FIXME
163
                //qDebug() << "SYSTEM NAV MODE:" << state.nav_mode;
pixhawk's avatar
pixhawk committed
164

pixhawk's avatar
pixhawk committed
165 166 167 168 169
                QString audiostring = "System " + QString::number(this->getUASID());
                QString stateAudio = "";
                QString modeAudio = "";
                bool statechanged = false;
                bool modechanged = false;
pixhawk's avatar
pixhawk committed
170

pixhawk's avatar
pixhawk committed
171
                if (state.status != this->status)
pixhawk's avatar
pixhawk committed
172
                {
pixhawk's avatar
pixhawk committed
173
                    statechanged = true;
lm's avatar
lm committed
174
                    this->status = (int)state.status;
pixhawk's avatar
pixhawk committed
175 176
                    getStatusForCode((int)state.status, uasState, stateDescription);
                    emit statusChanged(this, uasState, stateDescription);
177
                    emit statusChanged(this->status);
178
                    stateAudio = " changed status to " + uasState;
pixhawk's avatar
pixhawk committed
179 180
                }

lm's avatar
lm committed
181
                if (this->mode != static_cast<unsigned int>(state.mode))
pixhawk's avatar
pixhawk committed
182 183
                {
                    modechanged = true;
lm's avatar
lm committed
184
                    this->mode = static_cast<unsigned int>(state.mode);
pixhawk's avatar
pixhawk committed
185 186
                    QString mode;

lm's avatar
lm committed
187
                    switch (state.mode)
pixhawk's avatar
pixhawk committed
188
                    {
lm's avatar
lm committed
189
                    case (uint8_t)MAV_MODE_LOCKED:
pixhawk's avatar
pixhawk committed
190 191
                        mode = "LOCKED MODE";
                        break;
lm's avatar
lm committed
192
                    case (uint8_t)MAV_MODE_MANUAL:
pixhawk's avatar
pixhawk committed
193 194
                        mode = "MANUAL MODE";
                        break;
lm's avatar
lm committed
195
                    case (uint8_t)MAV_MODE_AUTO:
pixhawk's avatar
pixhawk committed
196 197
                        mode = "AUTO MODE";
                        break;
lm's avatar
lm committed
198
                    case (uint8_t)MAV_MODE_GUIDED:
199 200
                        mode = "GUIDED MODE";
                        break;
lm's avatar
lm committed
201
                    case (uint8_t)MAV_MODE_READY:
lm's avatar
lm committed
202 203
                        mode = "READY";
                        break;
lm's avatar
lm committed
204
                    case (uint8_t)MAV_MODE_TEST1:
pixhawk's avatar
pixhawk committed
205 206
                        mode = "TEST1 MODE";
                        break;
lm's avatar
lm committed
207
                    case (uint8_t)MAV_MODE_TEST2:
pixhawk's avatar
pixhawk committed
208 209
                        mode = "TEST2 MODE";
                        break;
lm's avatar
lm committed
210
                    case (uint8_t)MAV_MODE_TEST3:
pixhawk's avatar
pixhawk committed
211 212 213 214 215 216 217 218 219 220
                        mode = "TEST3 MODE";
                        break;
                    default:
                        mode = "UNINIT MODE";
                        break;
                    }

                    emit modeChanged(this->getUASID(), mode, "");
                    modeAudio = " is now in " + mode;
                }
lm's avatar
lm committed
221
                currentVoltage = state.vbat/1000.0f;
pixhawk's avatar
pixhawk committed
222
                lpVoltage = filterVoltage(currentVoltage);
pixhawk's avatar
pixhawk committed
223 224 225
                if (startVoltage == 0) startVoltage = currentVoltage;
                timeRemaining = calculateTimeRemaining();
                //qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining;
pixhawk's avatar
pixhawk committed
226
                emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining);
pixhawk's avatar
pixhawk committed
227
                emit voltageChanged(message.sysid, state.vbat/1000.0f);
pixhawk's avatar
pixhawk committed
228

lm's avatar
lm committed
229 230
                // LOW BATTERY ALARM
                float chargeLevel = getChargeLevel();
231
                if (chargeLevel <= 20.0f)
lm's avatar
lm committed
232 233 234 235 236 237 238 239
                {
                    startLowBattAlarm();
                }
                else
                {
                    stopLowBattAlarm();
                }

lm's avatar
lm committed
240
                // COMMUNICATIONS DROP RATE
241
                emit dropRateChanged(this->getUASID(), state.packet_drop);
242
                //qDebug() << __FILE__ << __LINE__ << "RCV LOSS: " << state.packet_drop;
lm's avatar
lm committed
243 244

                // AUDIO
pixhawk's avatar
pixhawk committed
245 246 247 248 249 250 251 252 253 254
                if (modechanged && statechanged)
                {
                    // Output both messages
                    audiostring += modeAudio + " and " + stateAudio;
                }
                else
                {
                    // Output the one message
                    audiostring += modeAudio + stateAudio;
                }
255
                if ((int)state.status == (int)MAV_STATE_CRITICAL || state.status == (int)MAV_STATE_EMERGENCY)
pixhawk's avatar
pixhawk committed
256 257 258 259 260 261 262 263
                {
                    GAudioOutput::instance()->startEmergency();
                }
                else if (modechanged || statechanged)
                {
                    GAudioOutput::instance()->stopEmergency();
                    GAudioOutput::instance()->say(audiostring);
                }
pixhawk's avatar
pixhawk committed
264 265 266
            }
            break;
        case MAVLINK_MSG_ID_RAW_IMU:
pixhawk's avatar
pixhawk committed
267
            {
pixhawk's avatar
pixhawk committed
268 269
                mavlink_raw_imu_t raw;
                mavlink_msg_raw_imu_decode(&message, &raw);
270
                quint64 time = getUnixTime(raw.usec);
pixhawk's avatar
pixhawk committed
271

pixhawk's avatar
pixhawk committed
272 273 274 275 276 277 278 279 280
                emit valueChanged(uasId, "Accel. X", raw.xacc, time);
                emit valueChanged(uasId, "Accel. Y", raw.yacc, time);
                emit valueChanged(uasId, "Accel. Z", raw.zacc, time);
                emit valueChanged(uasId, "Gyro Phi", raw.xgyro, time);
                emit valueChanged(uasId, "Gyro Theta", raw.ygyro, time);
                emit valueChanged(uasId, "Gyro Psi", raw.zgyro, time);
                emit valueChanged(uasId, "Mag. X", raw.xmag, time);
                emit valueChanged(uasId, "Mag. Y", raw.ymag, time);
                emit valueChanged(uasId, "Mag. Z", raw.zmag, time);
pixhawk's avatar
pixhawk committed
281 282 283 284
            }
            break;
        case MAVLINK_MSG_ID_ATTITUDE:
            //std::cerr << std::endl;
pixhawk's avatar
pixhawk committed
285
            //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
pixhawk's avatar
pixhawk committed
286
            {
pixhawk's avatar
pixhawk committed
287 288
                mavlink_attitude_t attitude;
                mavlink_msg_attitude_decode(&message, &attitude);
289
                quint64 time = getUnixTime(attitude.usec);
lm's avatar
lm committed
290 291 292
                roll = attitude.roll;
                pitch = attitude.pitch;
                yaw = attitude.yaw;
pixhawk's avatar
pixhawk committed
293 294 295 296 297 298
                emit valueChanged(uasId, "roll IMU", mavlink_msg_attitude_get_roll(&message), time);
                emit valueChanged(uasId, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time);
                emit valueChanged(uasId, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time);
                emit valueChanged(this, "roll IMU", mavlink_msg_attitude_get_roll(&message), time);
                emit valueChanged(this, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time);
                emit valueChanged(this, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time);
lm's avatar
lm committed
299 300 301
                emit valueChanged(uasId, "rollspeed IMU", attitude.rollspeed, time);
                emit valueChanged(uasId, "pitchspeed IMU", attitude.pitchspeed, time);
                emit valueChanged(uasId, "yawspeed IMU", attitude.yawspeed, time);
pixhawk's avatar
pixhawk committed
302
                emit attitudeChanged(this, mavlink_msg_attitude_get_roll(&message), mavlink_msg_attitude_get_pitch(&message), mavlink_msg_attitude_get_yaw(&message), time);
pixhawk's avatar
pixhawk committed
303 304
            }
            break;
305
        case MAVLINK_MSG_ID_LOCAL_POSITION:
lm's avatar
lm committed
306
            //std::cerr << std::endl;
pixhawk's avatar
pixhawk committed
307
            //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
lm's avatar
lm committed
308
            {
309 310
                mavlink_local_position_t pos;
                mavlink_msg_local_position_decode(&message, &pos);
311
                quint64 time = getUnixTime(pos.usec);
lm's avatar
lm committed
312 313 314
                localX = pos.x;
                localY = pos.y;
                localZ = pos.z;
lm's avatar
lm committed
315 316 317
                emit valueChanged(uasId, "x", pos.x, time);
                emit valueChanged(uasId, "y", pos.y, time);
                emit valueChanged(uasId, "z", pos.z, time);
318 319 320
                emit valueChanged(uasId, "Vx", pos.vx, time);
                emit valueChanged(uasId, "Vy", pos.vy, time);
                emit valueChanged(uasId, "Vz", pos.vz, time);
lm's avatar
lm committed
321
                emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
322 323
                emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
                //emit attitudeChanged(this, pos.roll, pos.pitch, pos.yaw, time);
pixhawk's avatar
pixhawk committed
324 325 326 327 328 329 330
                // Set internal state
                if (!positionLock)
                {
                    // If position was not locked before, notify positive
                    GAudioOutput::instance()->notifyPositive();
                }
                positionLock = true;
lm's avatar
lm committed
331 332
            }
            break;
333 334 335 336 337 338 339 340 341 342 343 344 345 346
        case MAVLINK_MSG_ID_GLOBAL_POSITION:
            //std::cerr << std::endl;
            //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
            {
                mavlink_global_position_t pos;
                mavlink_msg_global_position_decode(&message, &pos);
                quint64 time = getUnixTime(pos.usec);
                emit valueChanged(uasId, "lat", pos.lat, time);
                emit valueChanged(uasId, "lon", pos.lon, time);
                emit valueChanged(uasId, "alt", pos.alt, time);
                emit valueChanged(uasId, "g-vx", pos.vx, time);
                emit valueChanged(uasId, "g-vy", pos.vy, time);
                emit valueChanged(uasId, "g-vz", pos.vz, time);
                emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);
pixhawk's avatar
pixhawk committed
347 348 349 350 351 352 353 354
                emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
                // Set internal state
                if (!positionLock)
                {
                    // If position was not locked before, notify positive
                    GAudioOutput::instance()->notifyPositive();
                }
                positionLock = true;
355 356 357 358 359 360 361 362
            }
            break;
        case MAVLINK_MSG_ID_GPS_RAW:
            //std::cerr << std::endl;
            //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
            {
                mavlink_gps_raw_t pos;
                mavlink_msg_gps_raw_decode(&message, &pos);
363 364 365

                // SANITY CHECK
                // only accept values in a realistic range
pixhawk's avatar
pixhawk committed
366 367
               // quint64 time = getUnixTime(pos.usec);
                quint64 time = MG::TIME::getGroundTimeNow();
368 369
                emit valueChanged(uasId, "lat", pos.lat, time);
                emit valueChanged(uasId, "lon", pos.lon, time);
370 371 372 373 374
                // Check for NaN
                int alt = pos.alt;
                if (alt != alt)
                {
                    alt = 0;
375
                    emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE");
376
                }
377
                emit valueChanged(uasId, "alt", pos.alt, time);
378 379 380 381 382 383 384 385 386
                // Smaller than threshold and not NaN
                if (pos.v < 1000000 && pos.v == pos.v)
                {
                    emit valueChanged(uasId, "speed", pos.v, time);
                    //qDebug() << "GOT GPS RAW";
                    emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
                }
                else
                {
387
                     emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v));
388 389
                }
                emit globalPositionChanged(this, pos.lon, pos.lat, alt, time);
390 391
            }
            break;
lm's avatar
lm committed
392 393 394 395
        case MAVLINK_MSG_ID_GPS_STATUS:
            {
                mavlink_gps_status_t pos;
                mavlink_msg_gps_status_decode(&message, &pos);
lm's avatar
lm committed
396
                for(int i = 0; i < (int)pos.satellites_visible; i++)
lm's avatar
lm committed
397
                {
lm's avatar
lm committed
398
                    emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i]));
lm's avatar
lm committed
399 400 401
                }
            }
            break;
lm's avatar
lm committed
402 403 404 405
        case MAVLINK_MSG_ID_RC_CHANNELS:
            {
                mavlink_rc_channels_t channels;
                mavlink_msg_rc_channels_decode(&message, &channels);
pixhawk's avatar
pixhawk committed
406
                emit remoteControlRSSIChanged(channels.rssi/255.0f);
lm's avatar
lm committed
407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438
                for (int i = 0; i < 8; i++)
                {
                    switch (i)
                    {
                    case 0:
                        emit remoteControlChannelChanged(i, channels.chan1_raw, channels.chan1_255/255.0f);
                        break;
                    case 1:
                        emit remoteControlChannelChanged(i, channels.chan2_raw, channels.chan2_255/255.0f);
                        break;
                    case 2:
                        emit remoteControlChannelChanged(i, channels.chan3_raw, channels.chan3_255/255.0f);
                        break;
                    case 3:
                        emit remoteControlChannelChanged(i, channels.chan4_raw, channels.chan4_255/255.0f);
                        break;
                    case 4:
                        emit remoteControlChannelChanged(i, channels.chan5_raw, channels.chan5_255/255.0f);
                        break;
                    case 5:
                        emit remoteControlChannelChanged(i, channels.chan6_raw, channels.chan6_255/255.0f);
                        break;
                    case 6:
                        emit remoteControlChannelChanged(i, channels.chan7_raw, channels.chan7_255/255.0f);
                        break;
                    case 7:
                        emit remoteControlChannelChanged(i, channels.chan8_raw, channels.chan8_255/255.0f);
                        break;
                    }
                }
            }
            break;
439 440 441 442 443 444 445
        case MAVLINK_MSG_ID_PARAM_VALUE:
            {
                mavlink_param_value_t value;
                mavlink_msg_param_value_decode(&message, &value);
                emit parameterChanged(uasId, message.compid, QString((char*)value.param_id), value.param_value);
            }
            break;
pixhawk's avatar
pixhawk committed
446
        case MAVLINK_MSG_ID_DEBUG:
pixhawk's avatar
pixhawk committed
447
            emit valueChanged(uasId, QString("debug ") + QString::number(mavlink_msg_debug_get_ind(&message)), mavlink_msg_debug_get_value(&message), MG::TIME::getGroundTimeNow());
pixhawk's avatar
pixhawk committed
448
            break;
449 450 451 452 453
        case MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT:
            {
                mavlink_attitude_controller_output_t out;
                mavlink_msg_attitude_controller_output_decode(&message, &out);
                quint64 time = MG::TIME::getGroundTimeNowUsecs();
lm's avatar
lm committed
454
                emit attitudeThrustSetPointChanged(this, out.roll/127.0f, out.pitch/127.0f, out.yaw/127.0f, (uint8_t)out.thrust, time);
455 456 457 458 459 460 461 462 463
                emit valueChanged(uasId, "att control roll", out.roll, time/1000.0f);
                emit valueChanged(uasId, "att control pitch", out.pitch, time/1000.0f);
                emit valueChanged(uasId, "att control yaw", out.yaw, time/1000.0f);
            }
            break;
        case MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT:
            {
                mavlink_position_controller_output_t out;
                mavlink_msg_position_controller_output_decode(&message, &out);
464
                quint64 time = MG::TIME::getGroundTimeNow();
465
                //emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time);
466 467 468
                emit valueChanged(uasId, "pos control x", out.x, time);
                emit valueChanged(uasId, "pos control y", out.y, time);
                emit valueChanged(uasId, "pos control z", out.z, time);
469 470
            }
            break;
pixhawk's avatar
pixhawk committed
471 472 473 474
        case MAVLINK_MSG_ID_WAYPOINT_COUNT:
            {
                mavlink_waypoint_count_t wpc;
                mavlink_msg_waypoint_count_decode(&message, &wpc);
pixhawk's avatar
pixhawk committed
475 476 477 478
                if (wpc.target_system == mavlink->getSystemId() && wpc.target_component == mavlink->getComponentId())
                {
                    waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count);
                }
pixhawk's avatar
pixhawk committed
479 480 481
            }
            break;

482
        case MAVLINK_MSG_ID_WAYPOINT:
483
            {
484 485
                mavlink_waypoint_t wp;
                mavlink_msg_waypoint_decode(&message, &wp);
pixhawk's avatar
pixhawk committed
486
                //qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z;
pixhawk's avatar
pixhawk committed
487 488 489 490
                if(wp.target_system == mavlink->getSystemId() && wp.target_component == mavlink->getComponentId())
                {
                    waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
                }
491
            }
pixhawk's avatar
pixhawk committed
492
            break;
pixhawk's avatar
pixhawk committed
493

494 495 496 497 498 499 500 501 502 503 504
        case MAVLINK_MSG_ID_WAYPOINT_ACK:
            {
                mavlink_waypoint_ack_t wpa;
                mavlink_msg_waypoint_ack_decode(&message, &wpa);
                if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId())
                {
                    waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa);
                }
            }
            break;

pixhawk's avatar
pixhawk committed
505 506 507 508
        case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
            {
                mavlink_waypoint_request_t wpr;
                mavlink_msg_waypoint_request_decode(&message, &wpr);
pixhawk's avatar
pixhawk committed
509 510 511 512
                if(wpr.target_system == mavlink->getSystemId() && wpr.target_component == mavlink->getComponentId())
                {
                    waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr);
                }
pixhawk's avatar
pixhawk committed
513 514 515
            }
            break;

516
        case MAVLINK_MSG_ID_WAYPOINT_REACHED:
517
            {
pixhawk's avatar
pixhawk committed
518 519 520
                mavlink_waypoint_reached_t wpr;
                mavlink_msg_waypoint_reached_decode(&message, &wpr);
                waypointManager.handleWaypointReached(message.sysid, message.compid, &wpr);
521
            }
pixhawk's avatar
pixhawk committed
522
            break;
pixhawk's avatar
pixhawk committed
523

524
        case MAVLINK_MSG_ID_WAYPOINT_CURRENT:
pixhawk's avatar
pixhawk committed
525
            {
526 527 528
                mavlink_waypoint_current_t wpc;
                mavlink_msg_waypoint_current_decode(&message, &wpc);
                waypointManager.handleWaypointCurrent(message.sysid, message.compid, &wpc);
529
            }
pixhawk's avatar
pixhawk committed
530
            break;
pixhawk's avatar
pixhawk committed
531

532 533 534 535 536 537 538
        case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT:
            {
                mavlink_local_position_setpoint_t p;
                mavlink_msg_local_position_setpoint_decode(&message, &p);
                emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs());
            }
            break;
pixhawk's avatar
pixhawk committed
539

540
        case MAVLINK_MSG_ID_STATUSTEXT:
lm's avatar
lm committed
541 542 543
            {
                QByteArray b;
                b.resize(256);
pixhawk's avatar
pixhawk committed
544
                mavlink_msg_statustext_get_text(&message, (int8_t*)b.data());
lm's avatar
lm committed
545 546
                //b.append('\0');
                QString text = QString(b);
pixhawk's avatar
pixhawk committed
547
                int severity = mavlink_msg_statustext_get_severity(&message);
548
                //qDebug() << "RECEIVED STATUS:" << text;false
lm's avatar
lm committed
549
                //emit statusTextReceived(severity, text);
550
                emit textMessageReceived(uasId, message.compid, severity, text);
lm's avatar
lm committed
551 552
            }
            break;
553
#ifdef MAVLINK_ENABLED_UALBERTA_MESSAGES
554 555 556 557 558 559 560 561 562 563 564 565 566 567
        case MAVLINK_MSG_ID_NAV_FILTER_BIAS:
            {
                mavlink_nav_filter_bias_t bias;
                mavlink_msg_nav_filter_bias_decode(&message, &bias);
                quint64 time = MG::TIME::getGroundTimeNow();
                emit valueChanged(uasId, "b_f[0]", bias.accel_0, time);
                emit valueChanged(uasId, "b_f[1]", bias.accel_1, time);
                emit valueChanged(uasId, "b_f[2]", bias.accel_2, time);
                emit valueChanged(uasId, "b_w[0]", bias.gyro_0, time);
                emit valueChanged(uasId, "b_w[1]", bias.gyro_1, time);
                emit valueChanged(uasId, "b_w[2]", bias.gyro_2, time);
            }
            break;
#endif
pixhawk's avatar
pixhawk committed
568
        default:
lm's avatar
lm committed
569 570 571 572
            {
                if (!unknownPackets.contains(message.msgid))
                {
                    unknownPackets.append(message.msgid);
573 574
                    //GAudioOutput::instance()->say("UNABLE TO DECODE MESSAGE WITH ID " + QString::number(message.msgid) + " FROM SYSTEM " + QString::number(message.sysid));
                    std::cout << "Unable to decode message from system " << std::dec << static_cast<int>(message.sysid) << " with message id:" << static_cast<int>(message.msgid) << std::endl;
lm's avatar
lm committed
575 576 577
                    //qDebug() << std::cerr << "Unable to decode message from system " << std::dec << static_cast<int>(message.acid) << " with message id:" << static_cast<int>(message.msgid) << std::endl;
                }
            }
pixhawk's avatar
pixhawk committed
578 579 580 581 582
            break;
        }
    }
}

pixhawk's avatar
pixhawk committed
583 584
void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw)
{
585
  #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
pixhawk's avatar
pixhawk committed
586
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
587 588
    mavlink_msg_position_control_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, 0, x, y, z, yaw);
    sendMessage(msg);
589
  #endif
pixhawk's avatar
pixhawk committed
590 591
}

pixhawk's avatar
pixhawk committed
592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628
void UAS::setLocalPositionOffset(float x, float y, float z, float yaw)
{
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
  mavlink_message_t msg;
  mavlink_msg_position_control_offset_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, x, y, z, yaw);
  sendMessage(msg);
#endif
}

void UAS::startRadioControlCalibration()
{
  mavlink_message_t msg;
  mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_RC);
  sendMessage(msg);
}

void UAS::startMagnetometerCalibration()
{
    mavlink_message_t msg;
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_MAG);
    sendMessage(msg);
}

void UAS::startGyroscopeCalibration()
{
    mavlink_message_t msg;
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_GYRO);
    sendMessage(msg);
}

void UAS::startPressureCalibration()
{
    mavlink_message_t msg;
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_PRESSURE);
    sendMessage(msg);
}

629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650
quint64 UAS::getUnixTime(quint64 time)
{
    if (time == 0)
    {
        return MG::TIME::getGroundTimeNow();
    }
    // Check if time is smaller than 40 years,
    // assuming no system without Unix timestamp
    // runs longer than 40 years continuously without
    // reboot. In worst case this will add/subtract the
    // communication delay between GCS and MAV,
    // it will never alter the timestamp in a safety
    // critical way.
    //
    // Calculation:
    // 40 years
    // 365 days
    // 24 hours
    // 60 minutes
    // 60 seconds
    // 1000 milliseconds
    // 1000 microseconds
651
    else if (time < 1261440000000000LLU)
652 653 654
    {
        if (onboardTimeOffset == 0)
        {
655
            onboardTimeOffset = MG::TIME::getGroundTimeNow() - time/1000;
656
        }
657
        return time/1000 + onboardTimeOffset;
658 659 660 661 662
    }
    else
    {
        // Time is not zero and larger than 40 years -> has to be
        // a Unix epoch timestamp. Do nothing.
663
        return time/1000;
664 665 666
    }
}

pixhawk's avatar
pixhawk committed
667
void UAS::setMode(int mode)
pixhawk's avatar
pixhawk committed
668
{
lm's avatar
lm committed
669
    if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_TEST3)
pixhawk's avatar
pixhawk committed
670 671
    {
        mavlink_message_t msg;
lm's avatar
lm committed
672
        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode);
pixhawk's avatar
pixhawk committed
673
        sendMessage(msg);
lm's avatar
lm committed
674
        qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode;
pixhawk's avatar
pixhawk committed
675
    }
pixhawk's avatar
pixhawk committed
676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692
}

void UAS::sendMessage(mavlink_message_t message)
{
    // Emit message on all links that are currently connected
    QList<LinkInterface*>::iterator i;
    for (i = links->begin(); i != links->end(); ++i)
    {
        sendMessage(*i, message);
    }
}

void UAS::sendMessage(LinkInterface* link, mavlink_message_t message)
{
    // Create buffer
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    // Write message into buffer, prepending start sign
pixhawk's avatar
pixhawk committed
693
    int len = mavlink_msg_to_send_buffer(buffer, &message);
pixhawk's avatar
pixhawk committed
694 695 696 697 698 699 700 701 702 703 704
    // If link is connected
    if (link->isConnected())
    {
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
}

/**
 * @param value battery voltage
 */
705
float UAS::filterVoltage(float value) const
pixhawk's avatar
pixhawk committed
706
{
707
    return lpVoltage * 0.7f + value * 0.3f;
pixhawk's avatar
pixhawk committed
708 709 710 711 712 713
}

void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription)
{
    switch (statusCode)
    {
lm's avatar
lm committed
714
    case MAV_STATE_UNINIT:
pixhawk's avatar
pixhawk committed
715 716 717
        uasState = tr("UNINIT");
        stateDescription = tr("Not initialized");
        break;
lm's avatar
lm committed
718
    case MAV_STATE_BOOT:
pixhawk's avatar
pixhawk committed
719 720 721
        uasState = tr("BOOT");
        stateDescription = tr("Booting system, please wait..");
        break;
lm's avatar
lm committed
722
    case MAV_STATE_CALIBRATING:
pixhawk's avatar
pixhawk committed
723 724 725
        uasState = tr("CALIBRATING");
        stateDescription = tr("Calibrating sensors..");
        break;
lm's avatar
lm committed
726
    case MAV_STATE_ACTIVE:
pixhawk's avatar
pixhawk committed
727 728 729
        uasState = tr("ACTIVE");
        stateDescription = tr("Normal operation mode");
        break;
lm's avatar
lm committed
730 731 732 733 734
    case MAV_STATE_STANDBY:
        uasState = tr("STANDBY");
        stateDescription = tr("Standby, operational");
        break;
    case MAV_STATE_CRITICAL:
pixhawk's avatar
pixhawk committed
735 736 737
        uasState = tr("CRITICAL");
        stateDescription = tr("Failure occured!");
        break;
lm's avatar
lm committed
738
    case MAV_STATE_EMERGENCY:
pixhawk's avatar
pixhawk committed
739 740 741
        uasState = tr("EMERGENCY");
        stateDescription = tr("EMERGENCY: Please land");
        break;
lm's avatar
lm committed
742
    case MAV_STATE_POWEROFF:
pixhawk's avatar
pixhawk committed
743 744 745
        uasState = tr("SHUTDOWN");
        stateDescription = tr("Powering off system");
        break;
lm's avatar
lm committed
746
    default:
pixhawk's avatar
pixhawk committed
747 748 749 750 751 752 753 754 755 756 757 758 759 760 761
        uasState = tr("UNKNOWN");
        stateDescription = tr("FAILURE: Unknown system state");
        break;
    }
}



/* MANAGEMENT */

/*
 *
 * @return The uptime in milliseconds
 *
 **/
762
quint64 UAS::getUptime() const
pixhawk's avatar
pixhawk committed
763 764 765 766 767 768 769 770
{
    if(startTime == 0) {
        return 0;
    } else {
        return MG::TIME::getGroundTimeNow() - startTime;
    }
}

771
int UAS::getCommunicationStatus() const
pixhawk's avatar
pixhawk committed
772 773 774 775
{
    return commStatus;
}

776 777 778
void UAS::requestParameters()
{
    mavlink_message_t msg;
779
    mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 25);
780 781
    // Send message twice to increase chance of reception
    sendMessage(msg);
782 783
}

784
void UAS::writeParametersToStorage()
785
{
786 787 788
    mavlink_message_t msg;
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_STORAGE_WRITE);
unknown's avatar
unknown committed
789
    //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_STORAGE_WRITE);
790 791 792 793 794 795 796 797 798
    sendMessage(msg);
}

void UAS::readParametersFromStorage()
{
    mavlink_message_t msg;
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(uint8_t)MAV_ACTION_STORAGE_READ);
    sendMessage(msg);
lm's avatar
lm committed
799 800 801 802
}

void UAS::enableAllDataTransmission(bool enabled)
{
803
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
lm's avatar
lm committed
804
    // Buffers to write data to
805
    mavlink_message_t msg;
806
    mavlink_request_data_stream_t stream;
lm's avatar
lm committed
807 808
    // Select the message to request from now on
    // 0 is a magic ID and will enable/disable the standard message set except for heartbeat
809
    stream.req_stream_id = 0;
lm's avatar
lm committed
810 811 812 813 814 815 816 817 818 819
    // Select the update rate in Hz the message should be send
    // All messages will be send with their default rate
    stream.req_message_rate = 0;
    // Start / stop the message
    stream.start_stop = (enabled) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
820
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
821 822
    // Send message twice to increase chance of reception
    sendMessage(msg);
lm's avatar
lm committed
823
    sendMessage(msg);
824
#endif
lm's avatar
lm committed
825 826 827 828
}

void UAS::enableRawSensorDataTransmission(bool enabled)
{
829
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
lm's avatar
lm committed
830 831
    // Buffers to write data to
    mavlink_message_t msg;
832
    mavlink_request_data_stream_t stream;
lm's avatar
lm committed
833
    // Select the message to request from now on
834
    stream.req_stream_id = 1;
lm's avatar
lm committed
835 836 837 838 839 840 841 842 843
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = 200;
    // Start / stop the message
    stream.start_stop = (enabled) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
844
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
845 846
    // Send message twice to increase chance of reception
    sendMessage(msg);
lm's avatar
lm committed
847
    sendMessage(msg);
848
#endif
lm's avatar
lm committed
849 850 851 852
}

void UAS::enableExtendedSystemStatusTransmission(bool enabled)
{
853
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
854 855 856 857 858 859 860 861 862 863 864 865 866 867 868
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = 2;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = 10;
    // Start / stop the message
    stream.start_stop = (enabled) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
869 870
    // Send message twice to increase chance of reception
    sendMessage(msg);
871
    sendMessage(msg);
872
#endif
lm's avatar
lm committed
873
}
874

lm's avatar
lm committed
875 876
void UAS::enableRCChannelDataTransmission(bool enabled)
{
877
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
878 879 880 881 882 883 884 885 886 887 888 889 890 891 892
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = 3;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = 200;
    // Start / stop the message
    stream.start_stop = (enabled) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
893 894
    // Send message twice to increase chance of reception
    sendMessage(msg);
895
    sendMessage(msg);
896
#endif
lm's avatar
lm committed
897 898 899 900
}

void UAS::enableRawControllerDataTransmission(bool enabled)
{
901
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
902 903 904 905 906 907 908 909 910 911 912 913 914 915 916
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = 4;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = 200;
    // Start / stop the message
    stream.start_stop = (enabled) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
917 918
    // Send message twice to increase chance of reception
    sendMessage(msg);
919
    sendMessage(msg);
920
#endif
lm's avatar
lm committed
921 922 923 924
}

void UAS::enableRawSensorFusionTransmission(bool enabled)
{
925
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
926 927 928 929 930 931 932 933 934 935 936 937 938 939 940
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = 5;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = 200;
    // Start / stop the message
    stream.start_stop = (enabled) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
941 942
    // Send message twice to increase chance of reception
    sendMessage(msg);
943
    sendMessage(msg);
944
#endif
945 946
}

pixhawk's avatar
pixhawk committed
947 948
void UAS::enablePositionTransmission(bool enabled)
{
949
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
pixhawk's avatar
pixhawk committed
950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = 6;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = 200;
    // Start / stop the message
    stream.start_stop = (enabled) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
968
#endif
pixhawk's avatar
pixhawk committed
969 970 971 972
}

void UAS::enableExtra1Transmission(bool enabled)
{
973
  #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
pixhawk's avatar
pixhawk committed
974 975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990 991
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = 7;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = 200;
    // Start / stop the message
    stream.start_stop = (enabled) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
992
#endif
pixhawk's avatar
pixhawk committed
993 994 995 996
}

void UAS::enableExtra2Transmission(bool enabled)
{
997
  #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
pixhawk's avatar
pixhawk committed
998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = 8;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = 200;
    // Start / stop the message
    stream.start_stop = (enabled) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
1016
#endif
pixhawk's avatar
pixhawk committed
1017 1018 1019 1020
}

void UAS::enableExtra3Transmission(bool enabled)
{
1021
  #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
pixhawk's avatar
pixhawk committed
1022 1023 1024 1025 1026 1027 1028
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = 9;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = 200;
1029 1030 1031 1032 1033 1034 1035 1036
    // Start / stop the message
    stream.start_stop = (enabled) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
1037 1038
    // Send message twice to increase chance of reception
    sendMessage(msg);
1039
    sendMessage(msg);
1040
#endif
1041 1042
}

lm's avatar
lm committed
1043 1044 1045 1046 1047 1048 1049
/**
 * Set a parameter value onboard
 *
 * @param component The component to set the parameter
 * @param id Name of the parameter
 * @param value Parameter value
 */
1050
void UAS::setParameter(int component, QString id, float value)
1051
{    
1052 1053 1054
    mavlink_message_t msg;
    mavlink_param_set_t p;
    p.param_value = value;
1055 1056
    p.target_system = (uint8_t)uasId;
    p.target_component = (uint8_t)component;
1057

1058
    // Copy string into buffer, ensuring not to exceed the buffer size    
1059
    for (unsigned int i = 0; i < sizeof(p.param_id); i++)
1060
    {
lm's avatar
lm committed
1061
        // String characters
1062
        if ((int)i < id.length() && i < (sizeof(p.param_id) - 1))
lm's avatar
lm committed
1063
        {
1064
            p.param_id[i] = id.toAscii()[i];
lm's avatar
lm committed
1065 1066
        }
        // Null termination at end of string or end of buffer
1067
        else if ((int)i == id.length() || i == (sizeof(p.param_id) - 1))
lm's avatar
lm committed
1068 1069 1070 1071 1072 1073 1074 1075
        {
            p.param_id[i] = '\0';
        }
        // Zero fill
        else
        {
            p.param_id[i] = 0;
        }
1076
    }    
1077
    mavlink_msg_param_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &p);
1078
    sendMessage(msg);
1079 1080
}

pixhawk's avatar
pixhawk committed
1081
/**
lm's avatar
lm committed
1082
 * Launches the system
pixhawk's avatar
pixhawk committed
1083 1084 1085 1086
 *
 **/
void UAS::launch()
{
1087
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1088
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1089
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_LAUNCH);
1090 1091 1092
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1093 1094 1095 1096 1097 1098 1099 1100
}

/**
 * Depending on the UAS, this might make the rotors of a helicopter spinning
 *
 **/
void UAS::enable_motors()
{
1101
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1102
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1103
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START);
1104 1105 1106
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1107 1108 1109 1110 1111 1112 1113 1114
}

/**
 * @warning Depending on the UAS, this might completely stop all motors.
 *
 **/
void UAS::disable_motors()
{
1115
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1116
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1117
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP);
1118 1119 1120
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134
}

void UAS::setManualControlCommands(double roll, double pitch, double yaw, double thrust)
{
    // Scale values
    double rollPitchScaling = 0.2f;
    double yawScaling = 0.5f;
    double thrustScaling = 1.0f;

    manualRollAngle = roll * rollPitchScaling;
    manualPitchAngle = pitch * rollPitchScaling;
    manualYawAngle = yaw * yawScaling;
    manualThrust = thrust * thrustScaling;

lm's avatar
lm committed
1135
    if(mode == (int)MAV_MODE_MANUAL)
pixhawk's avatar
pixhawk committed
1136
    {
1137
      #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
pixhawk's avatar
pixhawk committed
1138
        mavlink_message_t message;
pixhawk's avatar
pixhawk committed
1139
        mavlink_msg_manual_control_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual);
pixhawk's avatar
pixhawk committed
1140 1141 1142 1143
        sendMessage(message);
        qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust;

        emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow());
1144
      #endif
pixhawk's avatar
pixhawk committed
1145 1146 1147
    }
}

1148 1149 1150 1151 1152
int UAS::getSystemType()
{
    return this->type;
}

pixhawk's avatar
pixhawk committed
1153 1154 1155 1156 1157
void UAS::receiveButton(int buttonIndex)
{
    switch (buttonIndex)
    {
    case 0:
pixhawk's avatar
pixhawk committed
1158

pixhawk's avatar
pixhawk committed
1159 1160
        break;
    case 1:
pixhawk's avatar
pixhawk committed
1161

pixhawk's avatar
pixhawk committed
1162 1163 1164 1165 1166 1167 1168 1169 1170
        break;
    default:

        break;
    }
    qDebug() << __FILE__ << __LINE__ << ": Received button clicked signal (button # is: " << buttonIndex << "), UNIMPLEMENTED IN MAVLINK!";

}

1171

1172
/*void UAS::requestWaypoints()
1173 1174 1175 1176 1177
{
//    mavlink_message_t msg;
//    mavlink_msg_waypoint_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 25);
//    // Send message twice to increase chance of reception
//    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1178 1179
    waypointManager.requestWaypoints();
    qDebug() << "UAS Request WPs";
1180 1181
}

pixhawk's avatar
pixhawk committed
1182 1183
void UAS::setWaypoint(Waypoint* wp)
{
1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201
//    mavlink_message_t msg;
//    mavlink_waypoint_set_t set;
//    set.id = wp->id;
//    //QString name = wp->name;
//    // FIXME Check if this works properly
//    //name.truncate(MAVLINK_MSG_WAYPOINT_SET_FIELD_NAME_LEN);
//    //strcpy((char*)set.name, name.toStdString().c_str());
//    set.autocontinue = wp->autocontinue;
//    set.target_component = 25; // FIXME
//    set.target_system = uasId;
//    set.active = wp->current;
//    set.x = wp->x;
//    set.y = wp->y;
//    set.z = wp->z;
//    set.yaw = wp->yaw;
//    mavlink_msg_waypoint_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &set);
//    // Send message twice to increase chance of reception
//    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1202 1203 1204 1205
}

void UAS::setWaypointActive(int id)
{
1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228
//    mavlink_message_t msg;
//    mavlink_waypoint_set_active_t active;
//    active.id = id;
//    active.target_system = uasId;
//    active.target_component = 25; // FIXME
//    mavlink_msg_waypoint_set_active_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &active);
//    // Send message twice to increase chance of reception
//    sendMessage(msg);
//    sendMessage(msg);
//    // TODO This should be not directly emitted, but rather being fed back from the UAS
//    emit waypointSelected(getUASID(), id);
}

void UAS::clearWaypointList()
{
//    mavlink_message_t msg;
//    // FIXME
//    mavlink_waypoint_clear_list_t clist;
//    clist.target_system = uasId;
//    clist.target_component = 25;  // FIXME
//    mavlink_msg_waypoint_clear_list_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &clist);
//    sendMessage(msg);
//    qDebug() << "UAS clears Waypoints!";
1229
}*/
pixhawk's avatar
pixhawk committed
1230 1231 1232 1233


void UAS::halt()
{
1234
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1235
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1236
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_HALT);
1237 1238 1239
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1240 1241 1242 1243
}

void UAS::go()
{
1244
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1245
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1246
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,  (int)MAV_ACTION_CONTINUE);
1247 1248 1249
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1250 1251 1252 1253 1254
}

/** Order the robot to return home / to land on the runway **/
void UAS::home()
{
1255
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1256
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1257
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,  (int)MAV_ACTION_RETURN);
1258 1259 1260
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1261 1262 1263 1264 1265 1266 1267 1268
}

/**
 * The MAV starts the emergency landing procedure. The behaviour depends on the onboard implementation
 * and might differ between systems.
 */
void UAS::emergencySTOP()
{
1269
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1270
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1271
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_LAND);
1272 1273 1274
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1275 1276 1277 1278 1279 1280 1281 1282 1283 1284 1285 1286 1287 1288 1289 1290 1291 1292 1293 1294 1295 1296 1297 1298 1299
}

/**
 * All systems are immediately shut down (e.g. the main power line is cut).
 * @warning This might lead to a crash
 *
 * The command will not be executed until emergencyKILLConfirm is issues immediately afterwards
 */
bool UAS::emergencyKILL()
{
    bool result = false;
    QMessageBox msgBox;
    msgBox.setIcon(QMessageBox::Critical);
    msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS");
    msgBox.setInformativeText("Do you want to cut power on all systems?");
    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
    msgBox.setDefaultButton(QMessageBox::Cancel);
    int ret = msgBox.exec();

    // Close the message box shortly after the click to prevent accidental clicks
    QTimer::singleShot(5000, &msgBox, SLOT(reject()));


    if (ret == QMessageBox::Yes)
    {
1300
        mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1301
        // TODO Replace MG System ID with static function call and allow to change ID in GUI
1302
        mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_KILL);
1303 1304 1305
        // Send message twice to increase chance of reception
        sendMessage(msg);
        sendMessage(msg);
pixhawk's avatar
pixhawk committed
1306 1307 1308 1309 1310 1311 1312 1313 1314 1315 1316 1317 1318 1319 1320 1321 1322 1323 1324 1325 1326 1327 1328
        result = true;
    }
    return result;
}

void UAS::shutdown()
{
    bool result = false;
    QMessageBox msgBox;
    msgBox.setIcon(QMessageBox::Critical);
    msgBox.setText("Shutting down the UAS");
    msgBox.setInformativeText("Do you want to shut down the onboard computer?");
    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
    msgBox.setDefaultButton(QMessageBox::Cancel);
    int ret = msgBox.exec();

    // Close the message box shortly after the click to prevent accidental clicks
    QTimer::singleShot(5000, &msgBox, SLOT(reject()));


    if (ret == QMessageBox::Yes)
    {
        // If the active UAS is set, execute command
1329
        mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1330
        // TODO Replace MG System ID with static function call and allow to change ID in GUI
1331
        mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_SHUTDOWN);
1332 1333 1334
        // Send message twice to increase chance of reception
        sendMessage(msg);
        sendMessage(msg);
pixhawk's avatar
pixhawk committed
1335 1336 1337 1338 1339 1340 1341
        result = true;
    }
}

/**
 * @return The name of this system as string in human-readable form
 */
1342
QString UAS::getUASName(void) const
pixhawk's avatar
pixhawk committed
1343 1344 1345 1346 1347 1348 1349 1350 1351 1352 1353 1354 1355 1356 1357 1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368 1369 1370 1371 1372 1373 1374 1375 1376 1377 1378 1379 1380 1381 1382
{
    QString result;
    if (name == "")
    {
        result = tr("MAV ") + result.sprintf("%03d", getUASID());
    }
    else
    {
        result = name;
    }
    return result;
}

void UAS::addLink(LinkInterface* link)
{
    if (!links->contains(link))
    {
        links->append(link);
    }
    //links->append(link);
    //qDebug() << " ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK";
}

/**
 * @brief Get the links associated with this robot
 *
 **/
QList<LinkInterface*>* UAS::getLinks()
{
    return links;
}



void UAS::setBattery(BatteryType type, int cells)
{
    this->batteryType = type;
    this->cells = cells;
    switch (batteryType)
    {
lm's avatar
lm committed
1383
    case NICD:
pixhawk's avatar
pixhawk committed
1384
        break;
lm's avatar
lm committed
1385
    case NIMH:
pixhawk's avatar
pixhawk committed
1386
        break;
lm's avatar
lm committed
1387
    case LIION:
pixhawk's avatar
pixhawk committed
1388
        break;
lm's avatar
lm committed
1389
    case LIPOLY:
pixhawk's avatar
pixhawk committed
1390 1391 1392
        fullVoltage = this->cells * UAS::lipoFull;
        emptyVoltage = this->cells * UAS::lipoEmpty;
        break;
lm's avatar
lm committed
1393
    case LIFE:
pixhawk's avatar
pixhawk committed
1394
        break;
lm's avatar
lm committed
1395
    case AGZN:
pixhawk's avatar
pixhawk committed
1396 1397 1398 1399 1400 1401 1402 1403 1404 1405 1406 1407 1408 1409 1410 1411 1412
        break;
    }
}

int UAS::calculateTimeRemaining()
{
    quint64 dt = MG::TIME::getGroundTimeNow() - startTime;
    double seconds = dt / 1000.0f;
    double voltDifference = startVoltage - currentVoltage;
    if (voltDifference <= 0) voltDifference = 0.00000000001f;
    double dischargePerSecond = voltDifference / seconds;
    int remaining = static_cast<int>((currentVoltage - emptyVoltage) / dischargePerSecond);
    // Can never be below 0
    if (remaining < 0) remaining = 0;
    return remaining;
}

lm's avatar
lm committed
1413 1414 1415
/**
 * @return charge level in percent - 0 - 100
 */
pixhawk's avatar
pixhawk committed
1416 1417
double UAS::getChargeLevel()
{
1418 1419 1420 1421 1422 1423 1424 1425 1426 1427 1428 1429 1430 1431
    float chargeLevel;
    if (lpVoltage < emptyVoltage)
    {
        chargeLevel = 0.0f;
    }
    else if (lpVoltage > fullVoltage)
    {
        chargeLevel = 100.0f;
    }
    else
    {
        chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage));
    }
    return chargeLevel;
pixhawk's avatar
pixhawk committed
1432 1433
}

lm's avatar
lm committed
1434 1435 1436 1437
void UAS::startLowBattAlarm()
{
    if (!lowBattAlarm)
    {