mavlink_high_latency2.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   Copyright (c) 2015-2018 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file mavlink_high_latency2.cpp
 *
 * @author Achermann Florian <acfloria@ethz.ch>
 */

#include "mavlink_high_latency2.h"

#include <commander/px4_custom_mode.h>
#include <lib/ecl/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/uORB.h>

using matrix::wrap_2pi;

MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : MavlinkStream(mavlink),
    _actuator_sub_0(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
    _actuator_time_0(0),
    _actuator_sub_1(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_1))),
    _actuator_time_1(0),
    _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
    _airspeed_time(0),
    _attitude_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
    _attitude_sp_time(0),
    _estimator_status_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))),
    _estimator_status_time(0),
    _pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(position_controller_status))),
    _pos_ctrl_status_time(0),
    _geofence_sub(_mavlink->add_orb_subscription(ORB_ID(geofence_result))),
    _geofence_time(0),
    _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
    _global_pos_time(0),
    _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))),
    _gps_time(0),
    _mission_result_sub(_mavlink->add_orb_subscription(ORB_ID(mission_result))),
    _mission_result_time(0),
    _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
    _status_time(0),
    _status_flags_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status_flags))),
    _status_flags_time(0),
    _tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status))),
    _tecs_time(0),
    _wind_sub(_mavlink->add_orb_subscription(ORB_ID(wind_estimate))),
    _wind_time(0),
    _airspeed(SimpleAnalyzer::AVERAGE),
    _airspeed_sp(SimpleAnalyzer::AVERAGE),
    _climb_rate(SimpleAnalyzer::MAX),
    _eph(SimpleAnalyzer::MAX),
    _epv(SimpleAnalyzer::MAX),
    _groundspeed(SimpleAnalyzer::AVERAGE),
    _temperature(SimpleAnalyzer::AVERAGE),
    _throttle(SimpleAnalyzer::AVERAGE),
    _windspeed(SimpleAnalyzer::AVERAGE)
{

    for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
        _batteries[i].subscription = _mavlink->add_orb_subscription(ORB_ID(battery_status), i);
    }

    reset_last_sent();
}

bool MavlinkStreamHighLatency2::send(const hrt_abstime t)
{
    // only send the struct if transmitting is allowed
    // this assures that the stream timer is only reset when actually a message is transmitted
    if (_mavlink->should_transmit()) {
        mavlink_high_latency2_t msg = {};
        set_default_values(msg);

        bool updated = _airspeed.valid();
        updated |= _airspeed_sp.valid();

        for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
            updated |= _batteries[i].analyzer.valid();
        }

        updated |= _climb_rate.valid();
        updated |= _eph.valid();
        updated |= _epv.valid();
        updated |= _groundspeed.valid();
        updated |= _temperature.valid();
        updated |= _throttle.valid();
        updated |= _windspeed.valid();
        updated |= write_airspeed(&msg);
        updated |= write_attitude_sp(&msg);
        updated |= write_battery_status(&msg);
        updated |= write_estimator_status(&msg);
        updated |= write_fw_ctrl_status(&msg);
        updated |= write_geofence_result(&msg);
        updated |= write_global_position(&msg);
        updated |= write_mission_result(&msg);
        updated |= write_tecs_status(&msg);
        updated |= write_vehicle_status(&msg);
        updated |= write_vehicle_status_flags(&msg);
        updated |= write_wind_estimate(&msg);

        if (updated) {
            msg.timestamp = t / 1000;

            msg.type = _mavlink->get_system_type();
            msg.autopilot = MAV_AUTOPILOT_PX4;

            if (_airspeed.valid()) {
                _airspeed.get_scaled(msg.airspeed, 5.0f);
            }

            if (_airspeed_sp.valid()) {
                _airspeed_sp.get_scaled(msg.airspeed_sp, 5.0f);
            }

            int lowest = 0;

            for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) {
                const bool battery_connected = _batteries[i].connected && _batteries[i].analyzer.valid();
                const bool battery_is_lowest = _batteries[i].analyzer.get_scaled(100.0f) <= _batteries[lowest].analyzer.get_scaled(
                                       100.0f);

                if (battery_connected && battery_is_lowest) {
                    lowest = i;
                }

            }

            if (_batteries[lowest].connected) {
                _batteries[lowest].analyzer.get_scaled(msg.battery, 100.0f);

            } else {
                msg.battery = -1;
            }


            if (_climb_rate.valid()) {
                _climb_rate.get_scaled(msg.climb_rate, 10.0f);
            }

            if (_eph.valid()) {
                _eph.get_scaled(msg.eph, 10.0f);
            }

            if (_epv.valid()) {
                _epv.get_scaled(msg.epv, 10.0f);
            }

            if (_groundspeed.valid()) {
                _groundspeed.get_scaled(msg.groundspeed, 5.0f);
            }

            if (_temperature.valid()) {
                _temperature.get_scaled(msg.temperature_air, 1.0f);
            }

            if (_throttle.valid()) {
                _throttle.get_scaled(msg.throttle, 100.0f);
            }

            if (_windspeed.valid()) {
                _windspeed.get_scaled(msg.windspeed, 5.0f);
            }

            reset_analysers(t);

            mavlink_msg_high_latency2_send_struct(_mavlink->get_channel(), &msg);
        }

        return updated;

    } else {
        // reset the analysers every 60 seconds if nothing should be transmitted
        if ((t - _last_reset_time) > 60000000) {
            reset_analysers(t);
            PX4_DEBUG("Resetting HIGH_LATENCY2 analysers");
        }

        return false;
    }
}

void MavlinkStreamHighLatency2::reset_analysers(const hrt_abstime t)
{
    // reset the analyzers
    _airspeed.reset();
    _airspeed_sp.reset();

    for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
        _batteries[i].analyzer.reset();
    }

    _climb_rate.reset();
    _eph.reset();
    _epv.reset();
    _groundspeed.reset();
    _temperature.reset();
    _throttle.reset();
    _windspeed.reset();

    _last_reset_time = t;
}

bool MavlinkStreamHighLatency2::write_airspeed(mavlink_high_latency2_t *msg)
{
    struct airspeed_s airspeed;

    const bool updated = _airspeed_sub->update(&_airspeed_time, &airspeed);

    if (_airspeed_time > 0) {
        if (airspeed.confidence < 0.95f) { // the same threshold as for the commander
            msg->failure_flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE;
        }
    }

    return updated;
}

bool MavlinkStreamHighLatency2::write_attitude_sp(mavlink_high_latency2_t *msg)
{
    struct vehicle_attitude_setpoint_s attitude_sp;

    const bool updated = _attitude_sp_sub->update(&_attitude_sp_time, &attitude_sp);

    if (_attitude_sp_time > 0) {
        msg->target_heading = static_cast<uint8_t>(math::degrees(wrap_2pi(attitude_sp.yaw_body)) * 0.5f);
    }

    return updated;
}

bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *msg)
{
    struct battery_status_s battery;
    bool updated = false;

    for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
        if (_batteries[i].subscription->update(&_batteries[i].timestamp, &battery)) {
            updated = true;
            _batteries[i].connected = battery.connected;

            if (battery.warning > battery_status_s::BATTERY_WARNING_LOW) {
                msg->failure_flags |= HL_FAILURE_FLAG_BATTERY;
            }
        }
    }

    return updated;
}

bool MavlinkStreamHighLatency2::write_estimator_status(mavlink_high_latency2_t *msg)
{
    struct estimator_status_s estimator_status;

    const bool updated = _estimator_status_sub->update(&_estimator_status_time, &estimator_status);

    if (_estimator_status_time > 0) {
        if (estimator_status.gps_check_fail_flags > 0 ||
            estimator_status.filter_fault_flags > 0 ||
            estimator_status.innovation_check_flags > 0) {
            msg->failure_flags |= HL_FAILURE_FLAG_ESTIMATOR;
        }
    }

    return updated;
}

bool MavlinkStreamHighLatency2::write_fw_ctrl_status(mavlink_high_latency2_t *msg)
{
    position_controller_status_s pos_ctrl_status = {};

    const bool updated = _pos_ctrl_status_sub->update(&_pos_ctrl_status_time, &pos_ctrl_status);

    if (_pos_ctrl_status_time > 0) {
        uint16_t target_distance;
        convert_limit_safe(pos_ctrl_status.wp_dist * 0.1f, target_distance);
        msg->target_distance = target_distance;
    }

    return updated;
}

bool MavlinkStreamHighLatency2::write_geofence_result(mavlink_high_latency2_t *msg)
{
    struct geofence_result_s geofence;

    const bool updated = _geofence_sub->update(&_geofence_time, &geofence);

    if (_geofence_time > 0) {
        if (geofence.geofence_violated) {
            msg->failure_flags |= HL_FAILURE_FLAG_GEOFENCE;
        }
    }

    return updated;
}

bool MavlinkStreamHighLatency2::write_global_position(mavlink_high_latency2_t *msg)
{
    struct vehicle_global_position_s global_pos;

    const bool updated = _global_pos_sub->update(&_global_pos_time, &global_pos);

    if (_global_pos_time > 0) {
        msg->latitude = global_pos.lat * 1e7;
        msg->longitude = global_pos.lon * 1e7;

        int16_t altitude = 0;

        if (global_pos.alt > 0) {
            convert_limit_safe(global_pos.alt + 0.5f, altitude);

        } else {
            convert_limit_safe(global_pos.alt - 0.5f, altitude);
        }

        msg->altitude = altitude;

        msg->heading = static_cast<uint8_t>(math::degrees(wrap_2pi(global_pos.yaw)) * 0.5f);
    }

    return updated;
}

bool MavlinkStreamHighLatency2::write_mission_result(mavlink_high_latency2_t *msg)
{
    struct mission_result_s mission_result;

    const bool updated = _mission_result_sub->update(&_mission_result_time, &mission_result);

    if (_mission_result_time > 0) {
        msg->wp_num = mission_result.seq_current;
    }

    return updated;
}

bool MavlinkStreamHighLatency2::write_tecs_status(mavlink_high_latency2_t *msg)
{
    struct tecs_status_s tecs_status;

    const bool updated = _tecs_status_sub->update(&_tecs_time, &tecs_status);

    if (_tecs_time > 0) {
        int16_t target_altitude;
        convert_limit_safe(tecs_status.altitude_sp, target_altitude);
        msg->target_altitude = target_altitude;
    }

    return updated;
}

bool MavlinkStreamHighLatency2::write_vehicle_status(mavlink_high_latency2_t *msg)
{
    struct vehicle_status_s status;

    const bool updated = _status_sub->update(&_status_time, &status);

    if (_status_time > 0) {
        if ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE)
            && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE)) {
            msg->failure_flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE;
        }

        if (((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_ACCEL)
             && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_ACCEL)) ||
            ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_ACCEL2) &&
             !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_ACCEL2))) {
            msg->failure_flags |= HL_FAILURE_FLAG_3D_ACCEL;
        }

        if (((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_GYRO)
             && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_GYRO)) ||
            ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_GYRO2) &&
             !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_GYRO2))) {
            msg->failure_flags |= HL_FAILURE_FLAG_3D_GYRO;
        }

        if (((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_MAG)
             && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_MAG)) ||
            ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_MAG2) &&
             !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_MAG2))) {
            msg->failure_flags |= HL_FAILURE_FLAG_3D_MAG;
        }

        if ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_TERRAIN)
            && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_TERRAIN)) {
            msg->failure_flags |= HL_FAILURE_FLAG_TERRAIN;
        }

        if (status.rc_signal_lost) {
            msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER;
        }

        if (status.engine_failure) {
            msg->failure_flags |= HL_FAILURE_FLAG_ENGINE;
        }

        if (status.mission_failure) {
            msg->failure_flags |= HL_FAILURE_FLAG_MISSION;
        }

        // flight mode
        union px4_custom_mode custom_mode;
        uint8_t mavlink_base_mode;
        get_mavlink_navigation_mode(&status, &mavlink_base_mode, &custom_mode);
        msg->custom_mode = custom_mode.custom_mode_hl;
    }

    return updated;
}

bool MavlinkStreamHighLatency2::write_vehicle_status_flags(mavlink_high_latency2_t *msg)
{
    struct vehicle_status_flags_s status_flags;

    const bool updated = _status_flags_sub->update(&_status_flags_time, &status_flags);

    if (_status_flags_time > 0) {
        if (!status_flags.condition_global_position_valid) { //TODO check if there is a better way to get only GPS failure
            msg->failure_flags |= HL_FAILURE_FLAG_GPS;
        }

        if (status_flags.offboard_control_signal_lost && status_flags.offboard_control_signal_found_once) {
            msg->failure_flags |= HL_FAILURE_FLAG_OFFBOARD_LINK;
        }
    }

    return updated;
}

bool MavlinkStreamHighLatency2::write_wind_estimate(mavlink_high_latency2_t *msg)
{
    struct wind_estimate_s wind;

    const bool updated = _wind_sub->update(&_wind_time, &wind);

    if (_wind_time > 0) {
        msg->wind_heading = static_cast<uint8_t>(
                        math::degrees(wrap_2pi(atan2f(wind.windspeed_east, wind.windspeed_north))) * 0.5f);
    }

    return updated;
}

void MavlinkStreamHighLatency2::update_data()
{
    const hrt_abstime t = hrt_absolute_time();

    if (t > _last_update_time) {
        // first order low pass filter for the update rate
        _update_rate_filtered = 0.97f * _update_rate_filtered + 0.03f / ((t - _last_update_time) * 1e-6f);
        _last_update_time = t;
    }

    update_airspeed();

    update_tecs_status();

    update_battery_status();

    update_global_position();

    update_gps();

    update_vehicle_status();

    update_wind_estimate();
}

void MavlinkStreamHighLatency2::update_airspeed()
{
    airspeed_s airspeed;

    if (_airspeed_sub->update(&airspeed)) {
        _airspeed.add_value(airspeed.indicated_airspeed_m_s, _update_rate_filtered);
        _temperature.add_value(airspeed.air_temperature_celsius, _update_rate_filtered);
    }
}

void MavlinkStreamHighLatency2::update_tecs_status()
{
    tecs_status_s tecs_status;

    if (_tecs_status_sub->update(&tecs_status)) {
        _airspeed_sp.add_value(tecs_status.airspeed_sp, _update_rate_filtered);
    }
}

void MavlinkStreamHighLatency2::update_battery_status()
{
    battery_status_s battery;

    for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
        if (_batteries[i].subscription->update(&battery)) {
            _batteries[i].connected = battery.connected;
            _batteries[i].analyzer.add_value(battery.remaining, _update_rate_filtered);
        }
    }
}

void MavlinkStreamHighLatency2::update_global_position()
{
    vehicle_global_position_s global_pos;

    if (_global_pos_sub->update(&global_pos)) {
        _climb_rate.add_value(fabsf(global_pos.vel_d), _update_rate_filtered);
        _groundspeed.add_value(sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e),
                       _update_rate_filtered);
    }
}

void MavlinkStreamHighLatency2::update_gps()
{
    vehicle_gps_position_s gps;

    if (_gps_sub->update(&gps)) {
        _eph.add_value(gps.eph, _update_rate_filtered);
        _epv.add_value(gps.epv, _update_rate_filtered);
    }
}

void MavlinkStreamHighLatency2::update_vehicle_status()
{
    vehicle_status_s status;

    if (_status_sub->update(&status)) {
        if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
            struct actuator_controls_s actuator = {};

            if (status.is_vtol && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
                if (_actuator_sub_1->update(&actuator)) {
                    _throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered);
                }

            } else {
                if (_actuator_sub_0->update(&actuator)) {
                    _throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered);
                }
            }

        } else {
            _throttle.add_value(0.0f, _update_rate_filtered);
        }
    }
}

void MavlinkStreamHighLatency2::update_wind_estimate()
{
    wind_estimate_s wind;

    if (_wind_sub->update(&wind)) {
        _windspeed.add_value(sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east),
                     _update_rate_filtered);
    }
}

void MavlinkStreamHighLatency2::set_default_values(mavlink_high_latency2_t &msg) const
{
    msg.airspeed = 0;
    msg.airspeed_sp = 0;
    msg.altitude = 0;
    msg.autopilot = MAV_AUTOPILOT_ENUM_END;
    msg.battery = -1;
    msg.climb_rate = 0;
    msg.custom0 = INT8_MIN;
    msg.custom1 = INT8_MIN;
    msg.custom2 = INT8_MIN;
    msg.eph = UINT8_MAX;
    msg.epv = UINT8_MAX;
    msg.failure_flags = 0;
    msg.custom_mode = 0;
    msg.groundspeed = 0;
    msg.heading = 0;
    msg.latitude = 0;
    msg.longitude = 0;
    msg.target_altitude = 0;
    msg.target_distance = 0;
    msg.target_heading = 0;
    msg.temperature_air = INT8_MIN;
    msg.throttle = 0;
    msg.timestamp = 0;
    msg.type = MAV_TYPE_ENUM_END;
    msg.wind_heading = 0;
    msg.windspeed = 0;
    msg.wp_num = UINT16_MAX;
}