mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 19:43:34 +08:00
Move HIGH_LATENCY2 stream to separate file and update some comments"
This commit is contained in:
acfloria
@@ -47,6 +47,7 @@ px4_add_module(
|
||||
mavlink.c
|
||||
mavlink_command_sender.cpp
|
||||
mavlink_ftp.cpp
|
||||
mavlink_high_latency2.cpp
|
||||
mavlink_log_handler.cpp
|
||||
mavlink_main.cpp
|
||||
mavlink_messages.cpp
|
||||
|
||||
@@ -0,0 +1,584 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <mathlib/mathlib.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <commander/px4_custom_mode.h>
|
||||
|
||||
#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/fw_pos_ctrl_status.h>
|
||||
#include <uORB/topics/geofence_result.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>
|
||||
|
||||
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),
|
||||
_battery_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status))),
|
||||
_battery_time(0),
|
||||
_estimator_status_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))),
|
||||
_estimator_status_time(0),
|
||||
_fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))),
|
||||
_fw_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),
|
||||
_battery(SimpleAnalyzer::AVERAGE),
|
||||
_climb_rate(SimpleAnalyzer::MAX),
|
||||
_eph(SimpleAnalyzer::MAX),
|
||||
_epv(SimpleAnalyzer::MAX),
|
||||
_groundspeed(SimpleAnalyzer::AVERAGE),
|
||||
_temperature(SimpleAnalyzer::AVERAGE),
|
||||
_throttle(SimpleAnalyzer::AVERAGE),
|
||||
_windspeed(SimpleAnalyzer::AVERAGE)
|
||||
{}
|
||||
|
||||
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();
|
||||
updated |= _battery.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) {
|
||||
convert_limit_safe(t / 1000, &msg.timestamp);
|
||||
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);
|
||||
}
|
||||
|
||||
if (_battery.valid()) {
|
||||
_battery.get_scaled(msg.battery, 100.0f);
|
||||
}
|
||||
|
||||
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();
|
||||
_battery.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;
|
||||
|
||||
const bool updated = _battery_sub->update(&_battery_time, &battery);
|
||||
|
||||
if (_battery_time > 0) {
|
||||
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)
|
||||
{
|
||||
struct fw_pos_ctrl_status_s fw_pos_ctrl_status;
|
||||
|
||||
const bool updated = _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_time, &fw_pos_ctrl_status);
|
||||
|
||||
if (_fw_pos_ctrl_status_time > 0) {
|
||||
convert_limit_safe(fw_pos_ctrl_status.wp_dist * 0.1f, &msg->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) {
|
||||
convert_limit_safe(global_pos.lat * 1e7, &msg->latitude);
|
||||
convert_limit_safe(global_pos.lon * 1e7, &msg->longitude);
|
||||
|
||||
if (global_pos.alt > 0) {
|
||||
convert_limit_safe(global_pos.alt + 0.5f, &msg->altitude);
|
||||
|
||||
} else {
|
||||
convert_limit_safe(global_pos.alt - 0.5f, &msg->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) {
|
||||
convert_limit_safe(tecs_status.altitudeSp, &msg->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.airspeedSp, _update_rate_filtered);
|
||||
}
|
||||
}
|
||||
|
||||
void MavlinkStreamHighLatency2::update_battery_status()
|
||||
{
|
||||
battery_status_s battery;
|
||||
|
||||
if (_battery_sub->update(&battery)) {
|
||||
_battery.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.is_rotary_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;
|
||||
}
|
||||
@@ -0,0 +1,201 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-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.h
|
||||
*
|
||||
* @author Achermann Florian <acfloria@ethz.ch>
|
||||
*/
|
||||
|
||||
#include "mavlink_main.h"
|
||||
#include "mavlink_messages.h"
|
||||
#include "mavlink_simple_analyzer.h"
|
||||
#include "mavlink_stream.h"
|
||||
|
||||
#ifndef MAVLINK_HIGH_LATENCY2_H_
|
||||
#define MAVLINK_HIGH_LATENCY2_H_
|
||||
|
||||
class MavlinkStreamHighLatency2 : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
const char *get_name() const
|
||||
{
|
||||
return MavlinkStreamHighLatency2::get_name_static();
|
||||
}
|
||||
|
||||
static const char *get_name_static()
|
||||
{
|
||||
return "HIGH_LATENCY2";
|
||||
}
|
||||
|
||||
static uint16_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_HIGH_LATENCY2;
|
||||
}
|
||||
|
||||
uint16_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamHighLatency2(mavlink);
|
||||
}
|
||||
|
||||
unsigned get_size()
|
||||
{
|
||||
return MAVLINK_MSG_ID_HIGH_LATENCY2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
}
|
||||
|
||||
bool const_rate()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
MavlinkOrbSubscription *_actuator_sub_0;
|
||||
uint64_t _actuator_time_0;
|
||||
|
||||
MavlinkOrbSubscription *_actuator_sub_1;
|
||||
uint64_t _actuator_time_1;
|
||||
|
||||
MavlinkOrbSubscription *_airspeed_sub;
|
||||
uint64_t _airspeed_time;
|
||||
|
||||
MavlinkOrbSubscription *_attitude_sp_sub;
|
||||
uint64_t _attitude_sp_time;
|
||||
|
||||
MavlinkOrbSubscription *_battery_sub;
|
||||
uint64_t _battery_time;
|
||||
|
||||
MavlinkOrbSubscription *_estimator_status_sub;
|
||||
uint64_t _estimator_status_time;
|
||||
|
||||
MavlinkOrbSubscription *_fw_pos_ctrl_status_sub;
|
||||
uint64_t _fw_pos_ctrl_status_time;
|
||||
|
||||
MavlinkOrbSubscription *_geofence_sub;
|
||||
uint64_t _geofence_time;
|
||||
|
||||
MavlinkOrbSubscription *_global_pos_sub;
|
||||
uint64_t _global_pos_time;
|
||||
|
||||
MavlinkOrbSubscription *_gps_sub;
|
||||
uint64_t _gps_time;
|
||||
|
||||
MavlinkOrbSubscription *_mission_result_sub;
|
||||
uint64_t _mission_result_time;
|
||||
|
||||
MavlinkOrbSubscription *_status_sub;
|
||||
uint64_t _status_time;
|
||||
|
||||
MavlinkOrbSubscription *_status_flags_sub;
|
||||
uint64_t _status_flags_time;
|
||||
|
||||
MavlinkOrbSubscription *_tecs_status_sub;
|
||||
uint64_t _tecs_time;
|
||||
|
||||
MavlinkOrbSubscription *_wind_sub;
|
||||
uint64_t _wind_time;
|
||||
|
||||
SimpleAnalyzer _airspeed;
|
||||
SimpleAnalyzer _airspeed_sp;
|
||||
SimpleAnalyzer _battery;
|
||||
SimpleAnalyzer _climb_rate;
|
||||
SimpleAnalyzer _eph;
|
||||
SimpleAnalyzer _epv;
|
||||
SimpleAnalyzer _groundspeed;
|
||||
SimpleAnalyzer _temperature;
|
||||
SimpleAnalyzer _throttle;
|
||||
SimpleAnalyzer _windspeed;
|
||||
|
||||
hrt_abstime _last_reset_time = 0;
|
||||
hrt_abstime _last_update_time = 0;
|
||||
float _update_rate_filtered = 0.0f;
|
||||
|
||||
/* do not allow top copying this class */
|
||||
MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &);
|
||||
MavlinkStreamHighLatency2 &operator = (const MavlinkStreamHighLatency2 &);
|
||||
|
||||
protected:
|
||||
explicit MavlinkStreamHighLatency2(Mavlink *mavlink);
|
||||
|
||||
bool send(const hrt_abstime t);
|
||||
|
||||
void reset_analysers(const hrt_abstime t);
|
||||
|
||||
bool write_airspeed(mavlink_high_latency2_t *msg);
|
||||
|
||||
bool write_attitude_sp(mavlink_high_latency2_t *msg);
|
||||
|
||||
bool write_battery_status(mavlink_high_latency2_t *msg);
|
||||
|
||||
bool write_estimator_status(mavlink_high_latency2_t *msg);
|
||||
|
||||
bool write_fw_ctrl_status(mavlink_high_latency2_t *msg);
|
||||
|
||||
bool write_geofence_result(mavlink_high_latency2_t *msg);
|
||||
|
||||
bool write_global_position(mavlink_high_latency2_t *msg);
|
||||
|
||||
bool write_mission_result(mavlink_high_latency2_t *msg);
|
||||
|
||||
bool write_tecs_status(mavlink_high_latency2_t *msg);
|
||||
|
||||
bool write_vehicle_status(mavlink_high_latency2_t *msg);
|
||||
|
||||
bool write_vehicle_status_flags(mavlink_high_latency2_t *msg);
|
||||
|
||||
bool write_wind_estimate(mavlink_high_latency2_t *msg);
|
||||
|
||||
void update_data();
|
||||
|
||||
void update_airspeed();
|
||||
|
||||
void update_tecs_status();
|
||||
|
||||
void update_battery_status();
|
||||
|
||||
void update_global_position();
|
||||
|
||||
void update_gps();
|
||||
|
||||
void update_vehicle_status();
|
||||
|
||||
void update_wind_estimate();
|
||||
|
||||
void set_default_values(mavlink_high_latency2_t &msg) const;
|
||||
};
|
||||
|
||||
#endif /* MAVLINK_HIGH_LATENCY2_H_ */
|
||||
@@ -48,6 +48,7 @@
|
||||
#include "mavlink_messages.h"
|
||||
#include "mavlink_command_sender.h"
|
||||
#include "mavlink_simple_analyzer.h"
|
||||
#include "mavlink_high_latency2.h"
|
||||
|
||||
#include <commander/px4_custom_mode.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
@@ -106,8 +107,7 @@
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
static uint16_t cm_uint16_from_m_float(float m);
|
||||
static void get_mavlink_navigation_mode(const struct vehicle_status_s *const status, uint8_t *mavlink_base_mode,
|
||||
union px4_custom_mode *custom_mode);
|
||||
|
||||
static void get_mavlink_mode_state(const struct vehicle_status_s *const status, uint8_t *mavlink_state,
|
||||
uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
|
||||
|
||||
@@ -3942,635 +3942,6 @@ protected:
|
||||
}
|
||||
};
|
||||
|
||||
class MavlinkStreamHighLatency2 : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
const char *get_name() const
|
||||
{
|
||||
return MavlinkStreamHighLatency2::get_name_static();
|
||||
}
|
||||
|
||||
static const char *get_name_static()
|
||||
{
|
||||
return "HIGH_LATENCY2";
|
||||
}
|
||||
|
||||
static uint16_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_HIGH_LATENCY2;
|
||||
}
|
||||
|
||||
uint16_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamHighLatency2(mavlink);
|
||||
}
|
||||
|
||||
unsigned get_size()
|
||||
{
|
||||
return MAVLINK_MSG_ID_HIGH_LATENCY2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
}
|
||||
|
||||
bool const_rate()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
MavlinkOrbSubscription *_actuator_sub_0;
|
||||
uint64_t _actuator_time_0;
|
||||
|
||||
MavlinkOrbSubscription *_actuator_sub_1;
|
||||
uint64_t _actuator_time_1;
|
||||
|
||||
MavlinkOrbSubscription *_airspeed_sub;
|
||||
uint64_t _airspeed_time;
|
||||
|
||||
MavlinkOrbSubscription *_attitude_sp_sub;
|
||||
uint64_t _attitude_sp_time;
|
||||
|
||||
MavlinkOrbSubscription *_battery_sub;
|
||||
uint64_t _battery_time;
|
||||
|
||||
MavlinkOrbSubscription *_estimator_status_sub;
|
||||
uint64_t _estimator_status_time;
|
||||
|
||||
MavlinkOrbSubscription *_fw_pos_ctrl_status_sub;
|
||||
uint64_t _fw_pos_ctrl_status_time;
|
||||
|
||||
MavlinkOrbSubscription *_geofence_sub;
|
||||
uint64_t _geofence_time;
|
||||
|
||||
MavlinkOrbSubscription *_global_pos_sub;
|
||||
uint64_t _global_pos_time;
|
||||
|
||||
MavlinkOrbSubscription *_gps_sub;
|
||||
uint64_t _gps_time;
|
||||
|
||||
MavlinkOrbSubscription *_mission_result_sub;
|
||||
uint64_t _mission_result_time;
|
||||
|
||||
MavlinkOrbSubscription *_status_sub;
|
||||
uint64_t _status_time;
|
||||
|
||||
MavlinkOrbSubscription *_status_flags_sub;
|
||||
uint64_t _status_flags_time;
|
||||
|
||||
MavlinkOrbSubscription *_tecs_status_sub;
|
||||
uint64_t _tecs_time;
|
||||
|
||||
MavlinkOrbSubscription *_wind_sub;
|
||||
uint64_t _wind_time;
|
||||
|
||||
SimpleAnalyzer _airspeed;
|
||||
SimpleAnalyzer _airspeed_sp;
|
||||
SimpleAnalyzer _battery;
|
||||
SimpleAnalyzer _climb_rate;
|
||||
SimpleAnalyzer _eph;
|
||||
SimpleAnalyzer _epv;
|
||||
SimpleAnalyzer _groundspeed;
|
||||
SimpleAnalyzer _temperature;
|
||||
SimpleAnalyzer _throttle;
|
||||
SimpleAnalyzer _windspeed;
|
||||
|
||||
hrt_abstime _last_reset_time = 0;
|
||||
hrt_abstime _last_update_time = 0;
|
||||
float _update_rate_filtered = 0.0f;
|
||||
|
||||
/* do not allow top copying this class */
|
||||
MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &);
|
||||
MavlinkStreamHighLatency2 &operator = (const MavlinkStreamHighLatency2 &);
|
||||
|
||||
protected:
|
||||
explicit 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),
|
||||
_battery_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status))),
|
||||
_battery_time(0),
|
||||
_estimator_status_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))),
|
||||
_estimator_status_time(0),
|
||||
_fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))),
|
||||
_fw_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),
|
||||
_battery(SimpleAnalyzer::AVERAGE),
|
||||
_climb_rate(SimpleAnalyzer::MAX),
|
||||
_eph(SimpleAnalyzer::MAX),
|
||||
_epv(SimpleAnalyzer::MAX),
|
||||
_groundspeed(SimpleAnalyzer::AVERAGE),
|
||||
_temperature(SimpleAnalyzer::AVERAGE),
|
||||
_throttle(SimpleAnalyzer::AVERAGE),
|
||||
_windspeed(SimpleAnalyzer::AVERAGE)
|
||||
{}
|
||||
|
||||
bool 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();
|
||||
updated |= _battery.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) {
|
||||
convert_limit_safe(t / 1000, &msg.timestamp);
|
||||
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);
|
||||
}
|
||||
|
||||
if (_battery.valid()) {
|
||||
_battery.get_scaled(msg.battery, 100.0f);
|
||||
}
|
||||
|
||||
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 reset_analysers(const hrt_abstime t)
|
||||
{
|
||||
// reset the analyzers
|
||||
_airspeed.reset();
|
||||
_airspeed_sp.reset();
|
||||
_battery.reset();
|
||||
_climb_rate.reset();
|
||||
_eph.reset();
|
||||
_epv.reset();
|
||||
_groundspeed.reset();
|
||||
_temperature.reset();
|
||||
_throttle.reset();
|
||||
_windspeed.reset();
|
||||
|
||||
_last_reset_time = t;
|
||||
}
|
||||
|
||||
bool 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 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 write_battery_status(mavlink_high_latency2_t *msg)
|
||||
{
|
||||
struct battery_status_s battery = {};
|
||||
|
||||
const bool updated = _battery_sub->update(&_battery_time, &battery);
|
||||
|
||||
if (_battery_time > 0) {
|
||||
if (battery.warning > battery_status_s::BATTERY_WARNING_LOW) {
|
||||
msg->failure_flags |= HL_FAILURE_FLAG_BATTERY;
|
||||
}
|
||||
}
|
||||
|
||||
return updated;
|
||||
}
|
||||
|
||||
bool 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 write_fw_ctrl_status(mavlink_high_latency2_t *msg)
|
||||
{
|
||||
struct fw_pos_ctrl_status_s fw_pos_ctrl_status = {};
|
||||
|
||||
const bool updated = _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_time, &fw_pos_ctrl_status);
|
||||
|
||||
if (_fw_pos_ctrl_status_time > 0) {
|
||||
convert_limit_safe(fw_pos_ctrl_status.wp_dist * 0.1f, &msg->target_distance);
|
||||
}
|
||||
|
||||
return updated;
|
||||
}
|
||||
|
||||
bool 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 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) {
|
||||
convert_limit_safe(global_pos.lat * 1e7, &msg->latitude);
|
||||
convert_limit_safe(global_pos.lon * 1e7, &msg->longitude);
|
||||
|
||||
if (global_pos.alt > 0) {
|
||||
convert_limit_safe(global_pos.alt + 0.5f, &msg->altitude);
|
||||
|
||||
} else {
|
||||
convert_limit_safe(global_pos.alt - 0.5f, &msg->altitude);
|
||||
}
|
||||
|
||||
msg->heading = static_cast<uint8_t>(math::degrees(_wrap_2pi(global_pos.yaw)) * 0.5f);
|
||||
}
|
||||
|
||||
return updated;
|
||||
}
|
||||
|
||||
bool 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 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) {
|
||||
convert_limit_safe(tecs_status.altitudeSp, &msg->target_altitude);
|
||||
}
|
||||
|
||||
return updated;
|
||||
}
|
||||
|
||||
bool 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 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 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 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_rate_filtered);
|
||||
|
||||
update_tecs_status(_update_rate_filtered);
|
||||
|
||||
update_battery_status(_update_rate_filtered);
|
||||
|
||||
update_global_position(_update_rate_filtered);
|
||||
|
||||
update_gps(_update_rate_filtered);
|
||||
|
||||
update_vehicle_status(_update_rate_filtered);
|
||||
|
||||
update_wind_estimate(_update_rate_filtered);
|
||||
}
|
||||
|
||||
void update_airspeed(float update_rate)
|
||||
{
|
||||
airspeed_s airspeed = {};
|
||||
|
||||
if (_airspeed_sub->update(&airspeed)) {
|
||||
_airspeed.add_value(airspeed.indicated_airspeed_m_s, update_rate);
|
||||
_temperature.add_value(airspeed.air_temperature_celsius, update_rate);
|
||||
}
|
||||
}
|
||||
|
||||
void update_tecs_status(float update_rate)
|
||||
{
|
||||
tecs_status_s tecs_status = {};
|
||||
|
||||
if (_tecs_status_sub->update(&tecs_status)) {
|
||||
_airspeed_sp.add_value(tecs_status.airspeedSp, update_rate);
|
||||
}
|
||||
}
|
||||
|
||||
void update_battery_status(float update_rate)
|
||||
{
|
||||
battery_status_s battery = {};
|
||||
|
||||
if (_battery_sub->update(&battery)) {
|
||||
_battery.add_value(battery.remaining, update_rate);
|
||||
}
|
||||
}
|
||||
|
||||
void update_global_position(float update_rate)
|
||||
{
|
||||
vehicle_global_position_s global_pos = {};
|
||||
|
||||
if (_global_pos_sub->update(&global_pos)) {
|
||||
_climb_rate.add_value(fabsf(global_pos.vel_d), update_rate);
|
||||
_groundspeed.add_value(sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e), update_rate);
|
||||
}
|
||||
}
|
||||
|
||||
void update_gps(float update_rate)
|
||||
{
|
||||
vehicle_gps_position_s gps = {};
|
||||
|
||||
if (_gps_sub->update(&gps)) {
|
||||
_eph.add_value(gps.eph, update_rate);
|
||||
_epv.add_value(gps.epv, update_rate);
|
||||
}
|
||||
}
|
||||
|
||||
void update_vehicle_status(float update_rate)
|
||||
{
|
||||
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.is_rotary_wing) {
|
||||
if (_actuator_sub_1->update(&actuator)) {
|
||||
_throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], update_rate);
|
||||
}
|
||||
|
||||
} else {
|
||||
if (_actuator_sub_0->update(&actuator)) {
|
||||
_throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], update_rate);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
_throttle.add_value(0.0f, update_rate);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void update_wind_estimate(float update_rate)
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
void 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;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
class MavlinkStreamGroundTruth : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
|
||||
@@ -61,4 +61,7 @@ public:
|
||||
const char *get_stream_name(const uint16_t msg_id);
|
||||
MavlinkStream *create_mavlink_stream(const char *stream_name, Mavlink *mavlink);
|
||||
|
||||
void get_mavlink_navigation_mode(const struct vehicle_status_s *const status, uint8_t *mavlink_base_mode,
|
||||
union px4_custom_mode *custom_mode);
|
||||
|
||||
#endif /* MAVLINK_MESSAGES_H_ */
|
||||
|
||||
@@ -120,10 +120,10 @@ MavlinkStream::update(const hrt_abstime t)
|
||||
|
||||
// If the interval is non-zero and dt is smaller than 1.5 times the interval
|
||||
// do not use the actual time but increment at a fixed rate, so that processing delays do not
|
||||
// distort the average rate. The check of the maximum interval is done to ensure that after a long time
|
||||
// not sending anything multiple messages in a short time are sent.
|
||||
// distort the average rate. The check of the maximum interval is done to ensure that after a
|
||||
// long time not sending anything, sending multiple messages in a short time is avoided.
|
||||
if (sent) {
|
||||
_last_sent = ((interval > 0) && ((int64_t)(1.5 * interval) > dt)) ? _last_sent + interval : t;
|
||||
_last_sent = ((interval > 0) && ((int64_t)(1.5f * interval) > dt)) ? _last_sent + interval : t;
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
|
||||
@@ -104,7 +104,10 @@ protected:
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Function to collect/update data for the streams when update() is called.
|
||||
* Function to collect/update data for the streams at a high rate independant of
|
||||
* actual stream rate.
|
||||
*
|
||||
* This function is called at every iteration of the mavlink module.
|
||||
*/
|
||||
virtual void update_data() { }
|
||||
|
||||
|
||||
Reference in New Issue
Block a user