UAS.cc

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

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

#include <QList>
#include <QTimer>
#include <QSettings>
#include <iostream>
#include <QDebug>

#include <cmath>
#include <qmath.h>

#include "UAS.h"
#include "LinkInterface.h"
#include "UASManager.h"
#include "QGC.h"
#include "GAudioOutput.h"
#include "MAVLinkProtocol.h"
#include "QGCMAVLink.h"
#include "LinkManager.h"
#include "SerialLink.h"
#include "UASParameterCommsMgr.h"
#include <Eigen/Geometry>
#include "AutoPilotPluginManager.h"
#include "QGCMessageBox.h"

/**
* Gets the settings from the previous UAS (name, airframe, autopilot, battery specs)
* by calling readSettings. This means the new UAS will have the same settings
* as the previous one created unless one calls deleteSettings in the code after
* creating the UAS.
*/

UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
    lipoFull(4.2f),
    lipoEmpty(3.5f),
    uasId(id),
    links(new QList<LinkInterface*>()),
    unknownPackets(),
    mavlink(protocol),
    commStatus(COMM_DISCONNECTED),
    receiveDropRate(0),
    sendDropRate(0),
    statusTimeout(thread),

    name(""),
    type(MAV_TYPE_GENERIC),
    airframe(QGC_AIRFRAME_GENERIC),
    autopilot(-1),
    systemIsArmed(false),
    base_mode(0),
    custom_mode(0),
    // custom_mode not initialized
    status(-1),
    // shortModeText not initialized
    // shortStateText not initialized

    // actuatorValues not initialized
    // actuatorNames not initialized
    // motorValues not initialized
    // motorNames mnot initialized
    thrustSum(0),
    thrustMax(10),

    // batteryType not initialized
    // cells not initialized
    // fullVoltage not initialized
    // emptyVoltage not initialized
    startVoltage(-1.0f),
    tickVoltage(10.5f),
    lastTickVoltageValue(13.0f),
    tickLowpassVoltage(12.0f),
    warnVoltage(9.5f),
    warnLevelPercent(20.0f),
    currentVoltage(12.6f),
    lpVoltage(12.0f),
    currentCurrent(0.4f),
    batteryRemainingEstimateEnabled(false),
    chargeLevel(-1),
    timeRemaining(0),
    lowBattAlarm(false),

    startTime(QGC::groundTimeMilliseconds()),
    onboardTimeOffset(0),

    controlRollManual(true),
    controlPitchManual(true),
    controlYawManual(true),
    controlThrustManual(true),
    manualRollAngle(0),
    manualPitchAngle(0),
    manualYawAngle(0),
    manualThrust(0),

    positionLock(false),
    isLocalPositionKnown(false),
    isGlobalPositionKnown(false),

    localX(0.0),
    localY(0.0),
    localZ(0.0),

    latitude(0.0),
    longitude(0.0),
    altitudeAMSL(0.0),
    altitudeAMSLFT(0.0),
    altitudeWGS84(0.0),
    altitudeRelative(0.0),

    globalEstimatorActive(false),

    latitude_gps(0.0),
    longitude_gps(0.0),
    altitude_gps(0.0),

    speedX(0.0),
    speedY(0.0),
    speedZ(0.0),

    nedPosGlobalOffset(0,0,0),
    nedAttGlobalOffset(0,0,0),

    airSpeed(std::numeric_limits<double>::quiet_NaN()),
    groundSpeed(std::numeric_limits<double>::quiet_NaN()),
    waypointManager(this),
    fileManager(this, this),

    attitudeKnown(false),
    attitudeStamped(false),
    lastAttitude(0),

    roll(0.0),
    pitch(0.0),
    yaw(0.0),

    imagePackets(0),    // We must initialize to 0, otherwise extended data packets maybe incorrectly thought to be images

    blockHomePositionChanges(false),
    receivedMode(false),


    paramsOnceRequested(false),
    paramMgr(this),
    simulation(0),
    _thread(thread),

    // The protected members.
    connectionLost(false),
    lastVoltageWarning(0),
    lastNonNullTime(0),
    onboardTimeOffsetInvalidCount(0),
    hilEnabled(false),
    sensorHil(false),
    lastSendTimeGPS(0),
    lastSendTimeSensors(0),
    lastSendTimeOpticalFlow(0)
{
    moveToThread(thread);

    for (unsigned int i = 0; i<255;++i)
    {
        componentID[i] = -1;
        componentMulti[i] = false;
    }

    connect(mavlink, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), &fileManager, SLOT(receiveMessage(LinkInterface*,mavlink_message_t)));

    // Store a list of available actions for this UAS.
    // Basically everything exposed as a SLOT with no return value or arguments.

    QAction* newAction = new QAction(tr("Arm"), thread);
    newAction->setToolTip(tr("Enable the UAS so that all actuators are online"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(armSystem()));
    actions.append(newAction);

    newAction = new QAction(tr("Disarm"), thread);
    newAction->setToolTip(tr("Disable the UAS so that all actuators are offline"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(disarmSystem()));
    actions.append(newAction);

    newAction = new QAction(tr("Toggle armed"), thread);
    newAction->setToolTip(tr("Toggle between armed and disarmed"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(toggleAutonomy()));
    actions.append(newAction);

    newAction = new QAction(tr("Go home"), thread);
    newAction->setToolTip(tr("Command the UAS to return to its home position"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(home()));
    actions.append(newAction);

    newAction = new QAction(tr("Land"), thread);
    newAction->setToolTip(tr("Command the UAS to land"));