UAS.cc 34.2 KB
Newer Older
pixhawk's avatar
pixhawk committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41
/*=====================================================================

PIXHAWK Micro Air Vehicle Flying Robotics Toolkit

(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>

This file is part of the PIXHAWK project

    PIXHAWK is free software: you can redistribute it and/or modify
    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.

    PIXHAWK is distributed in the hope that it will be useful,
    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
    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.

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

/**
 * @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"
pixhawk's avatar
pixhawk committed
42
#include "GAudioOutput.h"
43 44
#include "MAVLinkProtocol.h"
#include <mavlink.h>
pixhawk's avatar
pixhawk committed
45

46
UAS::UAS(MAVLinkProtocol* protocol, int id) :
47
        uasId(id),
pixhawk's avatar
pixhawk committed
48 49 50 51 52
        startTime(MG::TIME::getGroundTimeNow()),
        commStatus(COMM_DISCONNECTED),
        name(""),
        links(new QList<LinkInterface*>()),
        thrustSum(0),
pixhawk's avatar
pixhawk committed
53
        thrustMax(10),
pixhawk's avatar
pixhawk committed
54
        startVoltage(0),
55 56
        currentVoltage(12.0f),
        lpVoltage(12.0f),
pixhawk's avatar
pixhawk committed
57 58 59
        mode(MAV_MODE_UNINIT),
        status(MAV_STATE_UNINIT),
        onboardTimeOffset(0),
pixhawk's avatar
pixhawk committed
60 61 62 63
        controlRollManual(true),
        controlPitchManual(true),
        controlYawManual(true),
        controlThrustManual(true),
pixhawk's avatar
pixhawk committed
64 65 66
        manualRollAngle(0),
        manualPitchAngle(0),
        manualYawAngle(0),
lm's avatar
lm committed
67 68
        manualThrust(0),
        receiveDropRate(0),
lm's avatar
lm committed
69
        sendDropRate(0),
lm's avatar
lm committed
70 71
        unknownPackets(),
        lowBattAlarm(false)
pixhawk's avatar
pixhawk committed
72 73
{
    setBattery(LIPOLY, 3);
74
    mavlink = protocol;
pixhawk's avatar
pixhawk committed
75 76 77 78 79 80 81
}

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

82
int UAS::getUASID() const
pixhawk's avatar
pixhawk committed
83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108
{
    return uasId;
}

void UAS::setSelected()
{
    UASManager::instance()->setActiveUAS(this);
}

void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
    if (!links->contains(link))
    {
        addLink(link);
    }

    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
109
            if (this->type != mavlink_msg_heartbeat_get_type(&message))
pixhawk's avatar
pixhawk committed
110
            {
pixhawk's avatar
pixhawk committed
111
                this->type = mavlink_msg_heartbeat_get_type(&message);
pixhawk's avatar
pixhawk committed
112 113 114 115 116 117 118 119
                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
120 121
        case MAVLINK_MSG_ID_SYS_STATUS:
            {
pixhawk's avatar
pixhawk committed
122 123
                mavlink_sys_status_t state;
                mavlink_msg_sys_status_decode(&message, &state);
pixhawk's avatar
pixhawk committed
124

pixhawk's avatar
pixhawk committed
125 126 127 128 129
                QString audiostring = "System " + QString::number(this->getUASID());
                QString stateAudio = "";
                QString modeAudio = "";
                bool statechanged = false;
                bool modechanged = false;
pixhawk's avatar
pixhawk committed
130

pixhawk's avatar
pixhawk committed
131
                if (state.status != this->status)
pixhawk's avatar
pixhawk committed
132
                {
pixhawk's avatar
pixhawk committed
133 134 135 136
                    statechanged = true;
                    this->status = state.status;
                    getStatusForCode((int)state.status, uasState, stateDescription);
                    emit statusChanged(this, uasState, stateDescription);
137
                    stateAudio = " changed status to " + uasState;
pixhawk's avatar
pixhawk committed
138 139
                }

pixhawk's avatar
pixhawk committed
140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156
                if (static_cast<int>(this->mode) != static_cast<int>(state.mode))
                {
                    modechanged = true;
                    this->mode = state.mode;
                    QString mode;

                    switch (state.mode)
                    {
                    case MAV_MODE_LOCKED:
                        mode = "LOCKED MODE";
                        break;
                    case MAV_MODE_MANUAL:
                        mode = "MANUAL MODE";
                        break;
                    case MAV_MODE_AUTO:
                        mode = "AUTO MODE";
                        break;
157 158 159
                    case MAV_MODE_GUIDED:
                        mode = "GUIDED MODE";
                        break;
lm's avatar
lm committed
160 161 162
                    case MAV_MODE_READY:
                        mode = "READY";
                        break;
pixhawk's avatar
pixhawk committed
163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179
                    case MAV_MODE_TEST1:
                        mode = "TEST1 MODE";
                        break;
                    case MAV_MODE_TEST2:
                        mode = "TEST2 MODE";
                        break;
                    case MAV_MODE_TEST3:
                        mode = "TEST3 MODE";
                        break;
                    default:
                        mode = "UNINIT MODE";
                        break;
                    }

                    emit modeChanged(this->getUASID(), mode, "");
                    modeAudio = " is now in " + mode;
                }
lm's avatar
lm committed
180
                currentVoltage = state.vbat/1000.0f;
pixhawk's avatar
pixhawk committed
181
                lpVoltage = filterVoltage(currentVoltage);
pixhawk's avatar
pixhawk committed
182 183 184
                if (startVoltage == 0) startVoltage = currentVoltage;
                timeRemaining = calculateTimeRemaining();
                //qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining;
pixhawk's avatar
pixhawk committed
185
                emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining);
pixhawk's avatar
pixhawk committed
186
                emit voltageChanged(message.sysid, state.vbat/1000.0f);
pixhawk's avatar
pixhawk committed
187

lm's avatar
lm committed
188 189 190 191 192 193 194 195 196 197 198
                // LOW BATTERY ALARM
                float chargeLevel = getChargeLevel();
                if (chargeLevel <= 10.0f)
                {
                    startLowBattAlarm();
                }
                else
                {
                    stopLowBattAlarm();
                }

lm's avatar
lm committed
199 200 201 202
                // COMMUNICATIONS DROP RATE
                emit dropRateChanged(this->getUASID(), this->receiveDropRate, this->sendDropRate);

                // AUDIO
pixhawk's avatar
pixhawk committed
203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221
                if (modechanged && statechanged)
                {
                    // Output both messages
                    audiostring += modeAudio + " and " + stateAudio;
                }
                else
                {
                    // Output the one message
                    audiostring += modeAudio + stateAudio;
                }
                if (state.status == (int)MAV_STATE_CRITICAL || state.status == (int)MAV_STATE_EMERGENCY)
                {
                    GAudioOutput::instance()->startEmergency();
                }
                else if (modechanged || statechanged)
                {
                    GAudioOutput::instance()->stopEmergency();
                    GAudioOutput::instance()->say(audiostring);
                }
pixhawk's avatar
pixhawk committed
222 223
            }
            break;
lm's avatar
lm committed
224
        case MAVLINK_MSG_ID_AUX_STATUS:
lm's avatar
lm committed
225
            {
pixhawk's avatar
pixhawk committed
226 227
                mavlink_aux_status_t status;
                mavlink_msg_aux_status_decode(&message, &status);
228 229
                emit loadChanged(this, status.load/10.0f);
                emit valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow());
lm's avatar
lm committed
230 231
            }
            break;
pixhawk's avatar
pixhawk committed
232
        case MAVLINK_MSG_ID_RAW_IMU:
pixhawk's avatar
pixhawk committed
233
            {
pixhawk's avatar
pixhawk committed
234 235
                mavlink_raw_imu_t raw;
                mavlink_msg_raw_imu_decode(&message, &raw);
236
                quint64 time = raw.msec/1000;
pixhawk's avatar
pixhawk committed
237 238 239 240 241 242 243 244 245 246 247 248 249
                if (time == 0)
                {
                    time = MG::TIME::getGroundTimeNow();
                }
                else
                {
                    if (onboardTimeOffset == 0)
                    {
                        onboardTimeOffset = MG::TIME::getGroundTimeNow() - time;
                    }
                    time += onboardTimeOffset;
                }

pixhawk's avatar
pixhawk committed
250 251 252 253 254 255 256 257 258
                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
259 260
            }
            break;
pixhawk's avatar
pixhawk committed
261
        case MAVLINK_MSG_ID_RAW_AUX:
lm's avatar
lm committed
262
            {
pixhawk's avatar
pixhawk committed
263 264
                mavlink_raw_aux_t raw;
                mavlink_msg_raw_aux_decode(&message, &raw);
pixhawk's avatar
pixhawk committed
265
                quint64 time = MG::TIME::getGroundTimeNow();//raw.msec;
lm's avatar
lm committed
266 267 268 269 270 271 272 273 274 275 276 277 278
                if (time == 0)
                {
                    time = MG::TIME::getGroundTimeNow();
                }
                else
                {
                    if (onboardTimeOffset == 0)
                    {
                        onboardTimeOffset = MG::TIME::getGroundTimeNow() - time;
                    }
                    time += onboardTimeOffset;
                }
                emit valueChanged(uasId, "Pressure", raw.baro, time);
lm's avatar
lm committed
279
                emit valueChanged(uasId, "Temperature", raw.temp, time);
lm's avatar
lm committed
280 281
            }
            break;
pixhawk's avatar
pixhawk committed
282 283
        case MAVLINK_MSG_ID_ATTITUDE:
            //std::cerr << std::endl;
pixhawk's avatar
pixhawk committed
284
            //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
285
            {
pixhawk's avatar
pixhawk committed
286 287
                mavlink_attitude_t attitude;
                mavlink_msg_attitude_decode(&message, &attitude);
288
                quint64 time = mavlink_msg_attitude_get_msec(&message)/1000;
pixhawk's avatar
pixhawk committed
289 290 291 292 293 294 295 296 297 298 299 300
                if (time == 0)
                {
                    time = MG::TIME::getGroundTimeNow();
                }
                else
                {
                    if (onboardTimeOffset == 0)
                    {
                        onboardTimeOffset = MG::TIME::getGroundTimeNow() - time;
                    }
                    time += onboardTimeOffset;
                }
pixhawk's avatar
pixhawk committed
301 302 303 304 305 306
                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
307 308 309
                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
310
                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
311 312
            }
            break;
lm's avatar
lm committed
313 314
        case MAVLINK_MSG_ID_POSITION:
            //std::cerr << std::endl;
pixhawk's avatar
pixhawk committed
315
            //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
316
            {
pixhawk's avatar
pixhawk committed
317 318
                mavlink_position_t pos;
                mavlink_msg_position_decode(&message, &pos);
319
                quint64 time = pos.msec/1000;
lm's avatar
lm committed
320 321 322 323 324 325 326 327 328 329 330 331 332 333 334
                if (time == 0)
                {
                    time = MG::TIME::getGroundTimeNow();
                }
                else
                {
                    if (onboardTimeOffset == 0)
                    {
                        onboardTimeOffset = MG::TIME::getGroundTimeNow() - time;
                    }
                    time += onboardTimeOffset;
                }
                emit valueChanged(uasId, "x", pos.x, time);
                emit valueChanged(uasId, "y", pos.y, time);
                emit valueChanged(uasId, "z", pos.z, time);
pixhawk's avatar
pixhawk committed
335 336 337
                //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
338 339 340 341 342 343
                emit valueChanged(uasId, "vx", pos.vx, time);
                emit valueChanged(uasId, "vy", pos.vy, time);
                emit valueChanged(uasId, "vz", pos.vz, time);
                emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
            }
            break;
344 345
            case MAVLINK_MSG_ID_PARAM_VALUE:
                {
pixhawk's avatar
pixhawk committed
346 347
                    mavlink_param_value_t value;
                    mavlink_msg_param_value_decode(&message, &value);
348 349 350
                    emit parameterChanged(uasId, message.compid, QString((char*)value.param_id), value.param_value);
                }
                break;
pixhawk's avatar
pixhawk committed
351
        case MAVLINK_MSG_ID_DEBUG:
pixhawk's avatar
pixhawk committed
352
            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
353
            break;
pixhawk's avatar
pixhawk committed
354
            /*
pixhawk's avatar
pixhawk committed
355
        case MAVLINK_MSG_ID_WP:
pixhawk's avatar
pixhawk committed
356
            emit waypointUpdated(this->getUASID(), mavlink_msg_emitwaypoint_get_id(message.payload), mavlink_msg_emitwaypoint_get_x(message.payload), mavlink_msg_emitwaypoint_get_y(message.payload), mavlink_msg_emitwaypoint_get_z(message.payload), mavlink_msg_emitwaypoint_get_yaw(message.payload), (message_emitwaypoint_get_autocontinue(message.payload) == 1 ? true : false), (message_emitwaypoint_get_active(message.payload) == 1 ? true : false));
pixhawk's avatar
pixhawk committed
357
            break;
pixhawk's avatar
pixhawk committed
358
        case MAVLINK_MSG_ID_SET_POSITION:
pixhawk's avatar
pixhawk committed
359 360 361 362 363 364 365
            emit valueChanged(uasId, "WP X", mavlink_msg_gotowaypoint_get_x(message.payload), MG::TIME::getGroundTimeNow());
            emit valueChanged(uasId, "WP Y", mavlink_msg_gotowaypoint_get_y(message.payload), MG::TIME::getGroundTimeNow());
            emit valueChanged(uasId, "WP Z", mavlink_msg_gotowaypoint_get_z(message.payload), MG::TIME::getGroundTimeNow());
            emit valueChanged(uasId, "WP speed X", mavlink_msg_gotowaypoint_get_speedX(message.payload), MG::TIME::getGroundTimeNow());
            emit valueChanged(uasId, "WP speed Y", mavlink_msg_gotowaypoint_get_speedY(message.payload), MG::TIME::getGroundTimeNow());
            emit valueChanged(uasId, "WP speed Z", mavlink_msg_gotowaypoint_get_speedZ(message.payload), MG::TIME::getGroundTimeNow());
            emit valueChanged(uasId, "WP yaw", mavlink_msg_gotowaypoint_get_yaw(message.payload)/M_PI*180.0f, MG::TIME::getGroundTimeNow());
pixhawk's avatar
pixhawk committed
366 367 368
            break;
        case MAVLINK_MSG_ID_WP_REACHED:
            qDebug() << "WAYPOINT REACHED";
pixhawk's avatar
pixhawk committed
369
            emit waypointReached(this, mavlink_msg_wp_reached_get_id(message.payload));
pixhawk's avatar
pixhawk committed
370
            break;
pixhawk's avatar
pixhawk committed
371
            */
lm's avatar
lm committed
372 373 374 375
            case MAVLINK_MSG_ID_STATUSTEXT:
            {
                QByteArray b;
                b.resize(256);
pixhawk's avatar
pixhawk committed
376
                mavlink_msg_statustext_get_text(&message, (int8_t*)b.data());
lm's avatar
lm committed
377 378
                //b.append('\0');
                QString text = QString(b);
pixhawk's avatar
pixhawk committed
379
                int severity = mavlink_msg_statustext_get_severity(&message);
380
                //qDebug() << "RECEIVED STATUS:" << text;
lm's avatar
lm committed
381
                //emit statusTextReceived(severity, text);
382
                emit textMessageReceived(uasId, severity, text);
lm's avatar
lm committed
383 384 385 386 387 388
            }
            break;
        case MAVLINK_MSG_ID_PATTERN_DETECTED:
            {
                QByteArray b;
                b.resize(256);
pixhawk's avatar
pixhawk committed
389
                mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data());
lm's avatar
lm committed
390 391
                b.append('\0');
                QString path = QString(b);
pixhawk's avatar
pixhawk committed
392 393
                bool detected (mavlink_msg_pattern_detected_get_detected(&message) == 1 ? true : false );
                emit detectionReceived(uasId, path, 0, 0, 0, 0, 0, 0, 0, 0, mavlink_msg_pattern_detected_get_confidence(&message), detected);
lm's avatar
lm committed
394 395
            }
            break;
pixhawk's avatar
pixhawk committed
396
        default:
lm's avatar
lm committed
397 398 399 400 401 402 403 404 405
            {
                if (!unknownPackets.contains(message.msgid))
                {
                    unknownPackets.append(message.msgid);
                    GAudioOutput::instance()->say("UNABLE TO DECODE MESSAGE WITH ID " + QString::number(message.msgid) + " FROM SYSTEM " + QString::number(message.sysid));
                    std::cerr << "Unable to decode message from system " << std::dec << static_cast<int>(message.sysid) << " with message id:" << static_cast<int>(message.msgid) << std::endl;
                    //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
406 407 408 409 410
            break;
        }
    }
}

pixhawk's avatar
pixhawk committed
411
void UAS::setMode(int mode)
pixhawk's avatar
pixhawk committed
412
{
pixhawk's avatar
pixhawk committed
413 414 415 416
    if (mode >= MAV_MODE_LOCKED && mode <= MAV_MODE_TEST3)
    {
        this->mode = mode;
        mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
417
        mavlink_msg_set_mode_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, getUASID(), (unsigned char)mode);
pixhawk's avatar
pixhawk committed
418 419
        sendMessage(msg);
    }
pixhawk's avatar
pixhawk committed
420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443
}

void UAS::sendMessage(mavlink_message_t message)
{
    qDebug() << "In send message";
    // Emit message on all links that are currently connected
    QList<LinkInterface*>::iterator i;
    qDebug() << "LINKS: " << links->length();
    for (i = links->begin(); i != links->end(); ++i)
    {
        //        if (i != NULL)
        //        {
        qDebug() << "UAS::sendMessage()";
        sendMessage(*i, message);

    }
}

void UAS::sendMessage(LinkInterface* link, mavlink_message_t message)
{
    qDebug() << "UAS::sendMessage(link, message)";
    // Create buffer
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    // Write message into buffer, prepending start sign
pixhawk's avatar
pixhawk committed
444
    int len = mavlink_msg_to_send_buffer(buffer, &message);
pixhawk's avatar
pixhawk committed
445 446 447 448 449 450 451 452 453 454 455
    // 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
 */
456
float UAS::filterVoltage(float value) const
pixhawk's avatar
pixhawk committed
457
{
458
    return lpVoltage * 0.7f + value * 0.3f;
pixhawk's avatar
pixhawk committed
459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479
    /*
    currentVoltage = value;
    static QList<float> voltages<float>(20);
    const int dropPercent = 20;
    voltages.takeFirst();
    voltages.append(value);
    // Drop top and bottom xx percent of values
    QList<float> v(voltages);
    qSort(v);
    v = QList::mid(v.size()/dropPercent, v.size() - v.size()/dropPercent);
    lpVoltage = 0.0f;
    foreach (float value, v)
    {
        lpVoltage += v;
    }*/
}

void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription)
{
    switch (statusCode)
    {
lm's avatar
lm committed
480
    case MAV_STATE_UNINIT:
pixhawk's avatar
pixhawk committed
481 482 483
        uasState = tr("UNINIT");
        stateDescription = tr("Not initialized");
        break;
lm's avatar
lm committed
484
    case MAV_STATE_BOOT:
pixhawk's avatar
pixhawk committed
485 486 487
        uasState = tr("BOOT");
        stateDescription = tr("Booting system, please wait..");
        break;
lm's avatar
lm committed
488
    case MAV_STATE_CALIBRATING:
pixhawk's avatar
pixhawk committed
489 490 491
        uasState = tr("CALIBRATING");
        stateDescription = tr("Calibrating sensors..");
        break;
lm's avatar
lm committed
492
    case MAV_STATE_ACTIVE:
pixhawk's avatar
pixhawk committed
493 494 495
        uasState = tr("ACTIVE");
        stateDescription = tr("Normal operation mode");
        break;
lm's avatar
lm committed
496 497 498 499 500
    case MAV_STATE_STANDBY:
        uasState = tr("STANDBY");
        stateDescription = tr("Standby, operational");
        break;
    case MAV_STATE_CRITICAL:
pixhawk's avatar
pixhawk committed
501 502 503
        uasState = tr("CRITICAL");
        stateDescription = tr("Failure occured!");
        break;
lm's avatar
lm committed
504
    case MAV_STATE_EMERGENCY:
pixhawk's avatar
pixhawk committed
505 506 507
        uasState = tr("EMERGENCY");
        stateDescription = tr("EMERGENCY: Please land");
        break;
lm's avatar
lm committed
508
    case MAV_STATE_POWEROFF:
pixhawk's avatar
pixhawk committed
509 510 511
        uasState = tr("SHUTDOWN");
        stateDescription = tr("Powering off system");
        break;
lm's avatar
lm committed
512
    default:
pixhawk's avatar
pixhawk committed
513 514 515 516 517 518 519 520 521 522 523 524 525 526 527
        uasState = tr("UNKNOWN");
        stateDescription = tr("FAILURE: Unknown system state");
        break;
    }
}



/* MANAGEMENT */

/*
 *
 * @return The uptime in milliseconds
 *
 **/
528
quint64 UAS::getUptime() const
pixhawk's avatar
pixhawk committed
529 530 531 532 533 534 535 536
{
    if(startTime == 0) {
        return 0;
    } else {
        return MG::TIME::getGroundTimeNow() - startTime;
    }
}

537
int UAS::getCommunicationStatus() const
pixhawk's avatar
pixhawk committed
538 539 540 541 542 543 544 545 546 547 548 549
{
    return commStatus;
}

void UAS::requestWaypoints()
{
    mavlink_message_t message;
    //messagePackGetWaypoints(MG::SYSTEM::ID, &message); FIXME
    sendMessage(message);
    qDebug() << "UAS Request WPs";
}

550 551 552
void UAS::requestParameters()
{
    mavlink_message_t msg;
553 554 555 556 557 558
    mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0);
    sendMessage(msg);
}

void UAS::writeParameters()
{
lm's avatar
lm committed
559 560 561 562 563 564 565
    //mavlink_message_t msg;
    qDebug() << __FILE__ << __LINE__ << __func__ << "IS NOT IMPLEMENTED!";
}

void UAS::enableAllDataTransmission(bool enabled)
{
    // Buffers to write data to
566
    mavlink_message_t msg;
lm's avatar
lm committed
567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609
    mavlink_request_stream_t stream;
    // Select the message to request from now on
    // 0 is a magic ID and will enable/disable the standard message set except for heartbeat
    stream.req_message_id = 0;
    // 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
    mavlink_msg_request_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    sendMessage(msg);
}

void UAS::enableRawSensorDataTransmission(bool enabled)
{
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_stream_t stream;
    // Select the message to request from now on
    stream.req_message_id = MAVLINK_MSG_ID_RAW_IMU;
    // 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_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    sendMessage(msg);
}

void UAS::enableExtendedSystemStatusTransmission(bool enabled)
{
    // FIXME
    qDebug() << __FILE__ << __LINE__ << __func__ << "IS NOT IMPLEMENTED!";
}
610

lm's avatar
lm committed
611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626
void UAS::enableRCChannelDataTransmission(bool enabled)
{
    // FIXME
    qDebug() << __FILE__ << __LINE__ << __func__ << "IS NOT IMPLEMENTED!";
}

void UAS::enableRawControllerDataTransmission(bool enabled)
{
    // FIXME
    qDebug() << __FILE__ << __LINE__ << __func__ << "IS NOT IMPLEMENTED!";
}

void UAS::enableRawSensorFusionTransmission(bool enabled)
{
    // FIXME
    qDebug() << __FILE__ << __LINE__ << __func__ << "IS NOT IMPLEMENTED!";
627 628 629 630 631 632 633
}

void UAS::setParameter(int component, QString id, float value)
{
    mavlink_message_t msg;
    mavlink_param_set_t p;
    p.param_value = value;
634 635
    p.target_system = (uint8_t)uasId;
    p.target_component = (uint8_t)component;
636 637 638

    // Copy string into buffer, ensuring not to exceed the buffer size
    char* s = (char*)id.toStdString().c_str();
lm's avatar
lm committed
639
    for (int i = 0; i < sizeof(p.param_id); i++)
640
    {
lm's avatar
lm committed
641 642 643 644 645 646 647 648 649 650 651 652 653 654 655
        // String characters
        if (i < id.length() && i < (sizeof(p.param_id) - 1))
        {
            p.param_id[i] = s[i];
        }
        // Null termination at end of string or end of buffer
        else if (i == id.length() || i == (sizeof(p.param_id) - 1))
        {
            p.param_id[i] = '\0';
        }
        // Zero fill
        else
        {
            p.param_id[i] = 0;
        }
656 657
    }
    mavlink_msg_param_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &p);
658
    sendMessage(msg);
659 660
}

pixhawk's avatar
pixhawk committed
661 662 663 664 665 666 667 668
/**
 * @brief Launches the system
 *
 **/
void UAS::launch()
{
    mavlink_message_t message;
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
pixhawk's avatar
pixhawk committed
669
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(),(int)MAV_ACTION_LAUNCH);
pixhawk's avatar
pixhawk committed
670 671 672 673 674 675 676 677 678 679 680 681 682
    sendMessage(message);
    qDebug() << "UAS LAUNCHED!";
    //emit commandSent(LAUNCH);
}

/**
 * Depending on the UAS, this might make the rotors of a helicopter spinning
 *
 **/
void UAS::enable_motors()
{
    mavlink_message_t message;
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
pixhawk's avatar
pixhawk committed
683
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(),(int)MAV_ACTION_MOTORS_START);
pixhawk's avatar
pixhawk committed
684 685 686 687 688 689 690 691 692 693 694
    sendMessage(message);
}

/**
 * @warning Depending on the UAS, this might completely stop all motors.
 *
 **/
void UAS::disable_motors()
{
    mavlink_message_t message;
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
pixhawk's avatar
pixhawk committed
695
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(),(int)MAV_ACTION_MOTORS_STOP);
pixhawk's avatar
pixhawk committed
696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713
    sendMessage(message);
}

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;

    if(mode == MAV_MODE_MANUAL)
    {
        mavlink_message_t message;
pixhawk's avatar
pixhawk committed
714
        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
715 716 717 718 719 720 721 722 723 724 725 726
        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());
    }
}

void UAS::receiveButton(int buttonIndex)
{
    switch (buttonIndex)
    {
    case 0:
pixhawk's avatar
pixhawk committed
727

pixhawk's avatar
pixhawk committed
728 729
        break;
    case 1:
pixhawk's avatar
pixhawk committed
730

pixhawk's avatar
pixhawk committed
731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763
        break;
    default:

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

}

void UAS::setWaypoint(Waypoint* wp)
{
    mavlink_message_t message;
    // FIXME
    //messagePackSetWaypoint(MG::SYSTEM::ID, &message, wp->id, wp->x, wp->y, wp->z, wp->yaw, (wp->autocontinue ? 1 : 0));
    sendMessage(message);
    qDebug() << "UAS SENT Waypoint " << wp->id;
}

void UAS::setWaypointActive(int id)
{
    mavlink_message_t message;
    // FIXME
    //messagePackChooseWaypoint(MG::SYSTEM::ID, &message, id);
    sendMessage(message);
    // TODO This should be not directly emitted, but rather being fed back from the UAS
    emit waypointSelected(getUASID(), id);
}


void UAS::halt()
{
    mavlink_message_t message;
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
pixhawk's avatar
pixhawk committed
764
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_HALT);
pixhawk's avatar
pixhawk committed
765 766 767 768 769 770 771
    sendMessage(message);
}

void UAS::go()
{
    mavlink_message_t message;
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
pixhawk's avatar
pixhawk committed
772
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_CONTINUE);
pixhawk's avatar
pixhawk committed
773 774 775 776 777 778 779 780
    sendMessage(message);
}

/** Order the robot to return home / to land on the runway **/
void UAS::home()
{
    mavlink_message_t message;
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
pixhawk's avatar
pixhawk committed
781
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_RETURN);
pixhawk's avatar
pixhawk committed
782 783 784 785 786 787 788 789 790 791 792
    sendMessage(message);
}

/**
 * The MAV starts the emergency landing procedure. The behaviour depends on the onboard implementation
 * and might differ between systems.
 */
void UAS::emergencySTOP()
{
    mavlink_message_t message;
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
pixhawk's avatar
pixhawk committed
793
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_EMCY_LAND);
pixhawk's avatar
pixhawk committed
794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820
}

/**
 * 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)
    {
        mavlink_message_t message;
        // TODO Replace MG System ID with static function call and allow to change ID in GUI
pixhawk's avatar
pixhawk committed
821
        mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_EMCY_KILL);
pixhawk's avatar
pixhawk committed
822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847
        sendMessage(message);
        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
        mavlink_message_t message;
        // TODO Replace MG System ID with static function call and allow to change ID in GUI
pixhawk's avatar
pixhawk committed
848
        mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_SHUTDOWN);
pixhawk's avatar
pixhawk committed
849 850 851 852 853 854 855 856
        sendMessage(message);
        result = true;
    }
}

/**
 * @return The name of this system as string in human-readable form
 */
857
QString UAS::getUASName(void) const
pixhawk's avatar
pixhawk committed
858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897
{
    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
898
    case NICD:
pixhawk's avatar
pixhawk committed
899
        break;
lm's avatar
lm committed
900
    case NIMH:
pixhawk's avatar
pixhawk committed
901
        break;
lm's avatar
lm committed
902
    case LIION:
pixhawk's avatar
pixhawk committed
903
        break;
lm's avatar
lm committed
904
    case LIPOLY:
pixhawk's avatar
pixhawk committed
905 906 907
        fullVoltage = this->cells * UAS::lipoFull;
        emptyVoltage = this->cells * UAS::lipoEmpty;
        break;
lm's avatar
lm committed
908
    case LIFE:
pixhawk's avatar
pixhawk committed
909
        break;
lm's avatar
lm committed
910
    case AGZN:
pixhawk's avatar
pixhawk committed
911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927
        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
928 929 930
/**
 * @return charge level in percent - 0 - 100
 */
pixhawk's avatar
pixhawk committed
931 932 933 934 935
double UAS::getChargeLevel()
{
    return 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage));
}

lm's avatar
lm committed
936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953
void UAS::startLowBattAlarm()
{
    if (!lowBattAlarm)
    {
        GAudioOutput::instance()->startEmergency("BATTERY");
        lowBattAlarm = true;
    }
}

void UAS::stopLowBattAlarm()
{
    if (lowBattAlarm)
    {
        GAudioOutput::instance()->stopEmergency();
        lowBattAlarm = false;
    }
}

pixhawk's avatar
pixhawk committed
954 955 956 957 958 959 960 961
void UAS::clearWaypointList()
{
    mavlink_message_t message;
    // FIXME
    //messagePackRemoveWaypoints(MG::SYSTEM::ID, &message);
    sendMessage(message);
    qDebug() << "UAS clears Waypoints!";
}