UAS.cc

#ifndef __mobile__
float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
{
    /* Calculate normally distributed variable noise with mean = 0 and variance = noise_var.  Calculated according to 
    Box-Muller transform */
    static const float epsilon = std::numeric_limits<float>::min(); //used to ensure non-zero uniform numbers
    static const float two_pi = 2.0*3.14159265358979323846; // 2*pi
    static float z0; //calculated normal distribution random variables with mu = 0, var = 1;
    float u1, u2;        //random variables generated from c++ rand();
    
    /*Generate random variables in range (0 1] */
    do
    {
        //TODO seed rand() with srand(time) but srand(time should be called once on startup)
        //currently this will generate repeatable random noise
        u1 = rand() * (1.0 / RAND_MAX);
        u2 = rand() * (1.0 / RAND_MAX);
    }
    while ( u1 <= epsilon );  //Have a catch to ensure non-zero for log()

    z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2); //calculate normally distributed variable with mu = 0, var = 1
    
    //TODO add bias term that changes randomly to simulate accelerometer and gyro bias the exf should handle these
    //as well
    float noise = z0 * (noise_var*noise_var); //calculate normally distributed variable with mu = 0, std = var^2  
    
    //Finally gaurd against any case where the noise is not real
    if(std::isfinite(noise)){
            return truth_meas + noise;
    }
    else{
        return truth_meas;
    }
}
#endif

/*
* @param abs_pressure Absolute Pressure (hPa)
* @param diff_pressure Differential Pressure  (hPa)
*/
#ifndef __mobile__
void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
                                    float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed)
{
    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
    {
        float xacc_corrupt = addZeroMeanNoise(xacc, xacc_var);
        float yacc_corrupt = addZeroMeanNoise(yacc, yacc_var);
        float zacc_corrupt = addZeroMeanNoise(zacc, zacc_var);
        float rollspeed_corrupt = addZeroMeanNoise(rollspeed,rollspeed_var);
        float pitchspeed_corrupt = addZeroMeanNoise(pitchspeed,pitchspeed_var);
        float yawspeed_corrupt = addZeroMeanNoise(yawspeed,yawspeed_var);
        float xmag_corrupt = addZeroMeanNoise(xmag, xmag_var);
        float ymag_corrupt = addZeroMeanNoise(ymag, ymag_var);
        float zmag_corrupt = addZeroMeanNoise(zmag, zmag_var);
        float abs_pressure_corrupt = addZeroMeanNoise(abs_pressure,abs_pressure_var);
        float diff_pressure_corrupt = addZeroMeanNoise(diff_pressure, diff_pressure_var);
        float pressure_alt_corrupt = addZeroMeanNoise(pressure_alt, pressure_alt_var);
        float temperature_corrupt = addZeroMeanNoise(temperature,temperature_var);

        mavlink_message_t msg;
        mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
                                   time_us, xacc_corrupt, yacc_corrupt, zacc_corrupt, rollspeed_corrupt, pitchspeed_corrupt,
                                    yawspeed_corrupt, xmag_corrupt, ymag_corrupt, zmag_corrupt, abs_pressure_corrupt, 
                                    diff_pressure_corrupt, pressure_alt_corrupt, temperature_corrupt, fields_changed);
        sendMessage(msg);
        lastSendTimeSensors = QGC::groundTimeMilliseconds();
    }
    else
    {
        // Attempt to set HIL mode
        setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}
#endif

#ifndef __mobile__
void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
                    float flow_comp_m_y, quint8 quality, float ground_distance)
{
    // FIXME: This needs to be updated for new mavlink_msg_hil_optical_flow_pack api

    Q_UNUSED(time_us);
    Q_UNUSED(flow_x);
    Q_UNUSED(flow_y);
    Q_UNUSED(flow_comp_m_x);
    Q_UNUSED(flow_comp_m_y);
    Q_UNUSED(quality);
    Q_UNUSED(ground_distance);

    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
    {
#if 0
        mavlink_message_t msg;
        mavlink_msg_hil_optical_flow_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
                                   time_us, 0, 0 /* hack */, flow_x, flow_y, 0.0f /* hack */, 0.0f /* hack */, 0.0f /* hack */, 0 /* hack */, quality, ground_distance);

        sendMessage(msg);
        lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds();
#endif
    }
    else
    {
        // Attempt to set HIL mode
        setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }

}
#endif

#ifndef __mobile__
void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites)
{
    // Only send at 10 Hz max rate
    if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100)
        return;

    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
    {
        float course = cog;
        // map to 0..2pi
        if (course < 0)
            course += 2.0f * static_cast<float>(M_PI);
        // scale from radians to degrees
        course = (course / M_PI) * 180.0f;

        mavlink_message_t msg;
        mavlink_msg_hil_gps_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
                                   time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, vn*1e2, ve*1e2, vd*1e2, course*1e2, satellites);
        lastSendTimeGPS = QGC::groundTimeMilliseconds();
        sendMessage(msg);
    }
    else
    {
        // Attempt to set HIL mode
        setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}
#endif

/**
* Connect flight gear link.
**/
#ifndef __mobile__
void UAS::startHil()
{
    if (hilEnabled) return;
    hilEnabled = true;
    sensorHil = false;
    setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
    qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    // Connect HIL simulation link
    simulation->connectSimulation();
}
#endif

/**
* disable flight gear link.
*/
#ifndef __mobile__
void UAS::stopHil()
{
    if (simulation && simulation->isConnected()) {
        simulation->disconnectSimulation();
        setMode(base_mode & ~MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to disable.";
    }
    hilEnabled = false;
    sensorHil = false;
}
#endif

void UAS::shutdown()
{
    QMessageBox::StandardButton button = QGCMessageBox::question(tr("Shutting down the UAS"),
                                                                 tr("Do you want to shut down the onboard computer?"),
                                                                 QMessageBox::Yes | QMessageBox::Cancel,
                                                                 QMessageBox::Cancel);
    if (button == QMessageBox::Yes)
    {
        // If the active UAS is set, execute command
        mavlink_message_t msg;
        mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 0, 2, 0, 0, 0, 0, 0);
        sendMessage(msg);
    }
}

/**
* @param x position
* @param y position
* @param z position
* @param yaw
*/
void UAS::setTargetPosition(float x, float y, float z, float yaw)
{
    mavlink_message_t msg;
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 1, 1, 0, yaw, x, y, z);
    sendMessage(msg);
}

/**
 * @return The name of this system as string in human-readable form
 */
QString UAS::getUASName(void) const
{
    QString result;
    if (name == "")
    {
        result = tr("MAV ") + result.sprintf("%03d", getUASID());
    }
    else
    {
        result = name;
    }
    return result;
}

/**
* @return the state of the uas as a short text.
*/
const QString& UAS::getShortState() const
{
    return shortStateText;
}

/**
* The mode can be autonomous, guided, manual or armed. It will also return if
* hardware in the loop is being used.
* @return the audio mode text for the id given.
*/
QString UAS::getAudioModeTextFor(uint8_t base_mode, uint32_t custom_mode) const
{
    QString mode = AutoPilotPluginManager::instance()->getAudioModeText(base_mode, custom_mode, autopilot);

    if (mode.length() == 0)
    {
        // Fall back to generic decoding

        QString mode;
        uint8_t modeid = base_mode;

        // BASE MODE DECODING
        if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_AUTO)
        {
            mode += "autonomous";
        }
        else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_GUIDED)
        {
            mode += "guided";
        }
        else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_STABILIZE)
        {
            mode += "stabilized";
        }
        else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_MANUAL)
        {
            mode += "manual";
        }
        else
        {
            // Nothing else applies, we're in preflight
            mode += "preflight";
        }

        if (modeid != 0)
        {
            mode += " mode";
        }

        // ARMED STATE DECODING
        if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
        {
            mode.append(" and armed");
        }

        // HARDWARE IN THE LOOP DECODING
        if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_HIL)
        {
            mode.append(" using hardware in the loop simulation");
        }
    }

    return mode;
}

/**
* The mode returned depends on the specific autopilot used.
* @return the short text of the mode for the id given.
*/
QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode) const
{
    QString mode = AutoPilotPluginManager::instance()->getShortModeText(base_mode, custom_mode, autopilot);

    if (mode.length() == 0)
    {
        mode = "|UNKNOWN";
        qDebug() << __FILE__ << __LINE__ << " Unknown mode: base_mode=" << base_mode << " custom_mode=" << custom_mode << " autopilot=" << autopilot;
    }

    // ARMED STATE DECODING
    if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
    {
        mode.prepend("A");
    }
    else
    {
        mode.prepend("D");
    }

    // HARDWARE IN THE LOOP DECODING
    if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL)
    {
        mode.prepend("HIL:");
    }

    //qDebug() << "base_mode=" << base_mode << " custom_mode=" << custom_mode << " autopilot=" << autopilot << ": " << mode;

    return mode;
}

const QString& UAS::getShortMode() const
{
    return shortModeText;
}

/**
* Add the link and connect a signal to it which will be set off when it is destroyed.
*/
void UAS::addLink(LinkInterface* link)
{
    if (!_containsLink(link))
    {
        _links.append(LinkManager::instance()->sharedPointerForLink(link));
        qCDebug(UASLog) << "addLink:" << QString("%1").arg((ulong)link, 0, 16);
        connect(LinkManager::instance(), &LinkManager::linkDisconnected, this, &UAS::_linkDisconnected);
    }
}

void UAS::_linkDisconnected(LinkInterface* link)
{
    qCDebug(UASLog) << "_linkDisconnected:" << link->getName();
    qCDebug(UASLog) << "link count:" << _links.count();

    for (int i=0; i<_links.count(); i++) {
        if (_links[i].data() == link) {
            _links.removeAt(i);
            break;
        }
    }
    
    if (_links.count() == 0) {
        // Remove the UAS when all links to it close
        UASManager::instance()->removeUAS(this);
    }
}

/**
* @return the list of links
*/
QList<LinkInterface*> UAS::getLinks()
{
    QList<LinkInterface*> list;
    
    foreach (SharedLinkInterface sharedLink, _links) {
        list << sharedLink.data();
    }
    
    return list;
}

/**
* @rerturn the map of the components
*/
QMap<int, QString> UAS::getComponents()
{
    return components;
}

/**
* Set the battery specificaitons: empty voltage, warning voltage, and full voltage.
* @param specifications of the battery
*/
void UAS::setBatterySpecs(const QString& specs)
{
    batteryRemainingEstimateEnabled = false;
    bool ok;
    QString percent = specs;
    percent = percent.remove("%");
    float temp = percent.toFloat(&ok);
    if (ok)
    {
        warnLevelPercent = temp;
    }
    else
    {
        emit textMessageReceived(0, 0, MAV_SEVERITY_WARNING, "Could not set battery options, format is wrong");
    }
}

/**
* @return the battery specifications(empty voltage, warning voltage, full voltage)
*/
QString UAS::getBatterySpecs()
{
    return QString("%1%").arg(warnLevelPercent);
}

/**
* @return the time remaining.
*/
int UAS::calculateTimeRemaining()
{
    // XXX needs fixing
    return 0;
}

/**
 * @return charge level in percent - 0 - 100
 */
float UAS::getChargeLevel()
{
    return chargeLevel;
}

void UAS::startLowBattAlarm()
{
    if (!lowBattAlarm)
    {
        GAudioOutput::instance()->alert(tr("System %1 has low battery").arg(getUASID()));
        lowBattAlarm = true;
    }
}

void UAS::stopLowBattAlarm()
{
    if (lowBattAlarm)
    {
        lowBattAlarm = false;
    }
}

void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax)
{
    mavlink_message_t message;

    char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};
    // Copy string into buffer, ensuring not to exceed the buffer size
    for (unsigned int i = 0; i < sizeof(param_id_cstr); i++)
    {
        if ((int)i < param_id.length())
        {
            param_id_cstr[i] = param_id.toLatin1()[i];
        }
    }

    mavlink_msg_param_map_rc_pack(mavlink->getSystemId(),
                                  mavlink->getComponentId(),
                                  &message,
                                  this->uasId,
                                  0,
                                  param_id_cstr,
                                  -1,
                                  param_rc_channel_index,
                                  value0,
                                  scale,
                                  valueMin,
                                  valueMax);
    sendMessage(message);
    qDebug() << "Mavlink message sent";
}

void UAS::unsetRCToParameterMap()
{
    char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};

    for (int i = 0; i < 3; i++) {
        mavlink_message_t message;
        mavlink_msg_param_map_rc_pack(mavlink->getSystemId(),
                                      mavlink->getComponentId(),
                                      &message,
                                      this->uasId,
                                      0,
                                      param_id_cstr,
                                      -2,
                                      i,
                                      0.0f,
                                      0.0f,
                                      0.0f,
                                      0.0f);
        sendMessage(message);
    }
}

bool UAS::_containsLink(LinkInterface* link)
{
    foreach (SharedLinkInterface sharedLink, _links) {
        if (sharedLink.data() == link) {
            return true;
        }
    }

    return false;
}

bool UAS::isLogReplay(void)
{
    QList<LinkInterface*> links = getLinks();
    
    if (links.count() == 1) {
        return links[0]->isLogReplay();
    } else {
        return false;
    }
}