dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink messages delete unnecessary timestamps and cleanup

This commit is contained in:
Daniel Agar 2018-03-25 14:21:09 -04:00
parent c181501e27
commit 3db287ba67
2 changed files with 278 additions and 258 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

529
src/modules/mavlink/mavlink_messages.cpp
View File

@ -523,6 +523,10 @@ private:
MavlinkOrbSubscription *_cpuload_sub; MavlinkOrbSubscription *_cpuload_sub;
MavlinkOrbSubscription *_battery_status_sub; MavlinkOrbSubscription *_battery_status_sub;
uint64_t _status_timestamp{0};
uint64_t _cpuload_timestamp{0};
uint64_t _battery_status_timestamp{0};
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkStreamSysStatus(MavlinkStreamSysStatus &); MavlinkStreamSysStatus(MavlinkStreamSysStatus &);
MavlinkStreamSysStatus &operator = (const MavlinkStreamSysStatus &); MavlinkStreamSysStatus &operator = (const MavlinkStreamSysStatus &);
@ -536,13 +540,13 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct vehicle_status_s status = {}; vehicle_status_s status = {};
struct cpuload_s cpuload = {}; cpuload_s cpuload = {};
struct battery_status_s battery_status = {}; battery_status_s battery_status = {};
const bool updated_status = _status_sub->update(&status); const bool updated_status = _status_sub->update(&_status_timestamp, &status);
const bool updated_cpuload = _cpuload_sub->update(&cpuload); const bool updated_cpuload = _cpuload_sub->update(&_cpuload_timestamp, &cpuload);
const bool updated_battery = _battery_status_sub->update(&battery_status); const bool updated_battery = _battery_status_sub->update(&_battery_status_timestamp, &battery_status);
if (updated_status || updated_battery || updated_cpuload) { if (updated_status || updated_battery || updated_cpuload) {
mavlink_sys_status_t msg = {}; mavlink_sys_status_t msg = {};
@ -638,10 +642,7 @@ private:
uint64_t _sensor_time; uint64_t _sensor_time;
MavlinkOrbSubscription *_bias_sub; MavlinkOrbSubscription *_bias_sub;
uint64_t _bias_time;
MavlinkOrbSubscription *_differential_pressure_sub; MavlinkOrbSubscription *_differential_pressure_sub;
uint64_t _differential_pressure_time;
uint64_t _accel_timestamp; uint64_t _accel_timestamp;
uint64_t _gyro_timestamp; uint64_t _gyro_timestamp;
@ -657,9 +658,7 @@ protected:
_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
_sensor_time(0), _sensor_time(0),
_bias_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_bias))), _bias_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_bias))),
_bias_time(0),
_differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))), _differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))),
_differential_pressure_time(0),
_accel_timestamp(0), _accel_timestamp(0),
_gyro_timestamp(0), _gyro_timestamp(0),
_mag_timestamp(0), _mag_timestamp(0),
@ -668,9 +667,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct sensor_combined_s sensor = {}; sensor_combined_s sensor;
struct sensor_bias_s bias = {};
struct differential_pressure_s differential_pressure = {};
if (_sensor_sub->update(&_sensor_time, &sensor)) { if (_sensor_sub->update(&_sensor_time, &sensor)) {
uint16_t fields_updated = 0; uint16_t fields_updated = 0;
@ -699,8 +696,10 @@ protected:
_baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative; _baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;
} }
_bias_sub->update(&_bias_time, &bias); sensor_bias_s bias = {};
_differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure); _bias_sub->update(&bias);
differential_pressure_s differential_pressure = {};
_differential_pressure_sub->update(&differential_pressure);
mavlink_highres_imu_t msg = {}; mavlink_highres_imu_t msg = {};
@ -769,7 +768,6 @@ private:
MavlinkOrbSubscription *_raw_gyro_sub; MavlinkOrbSubscription *_raw_gyro_sub;
uint64_t _raw_accel_time; uint64_t _raw_accel_time;
uint64_t _raw_gyro_time; uint64_t _raw_gyro_time;
struct sensor_gyro_s _sensor_gyro;
// do not allow top copy this class // do not allow top copy this class
MavlinkStreamScaledIMU(MavlinkStreamScaledIMU &); MavlinkStreamScaledIMU(MavlinkStreamScaledIMU &);
@ -780,19 +778,19 @@ protected:
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel))), _raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel))),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro))), _raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro))),
_raw_accel_time(0), _raw_accel_time(0),
_raw_gyro_time(0), _raw_gyro_time(0)
_sensor_gyro{}
{} {}
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct sensor_accel_s sensor_accel = {}; sensor_accel_s sensor_accel = {};
sensor_accel_s sensor_gyro = {};
bool raw_accel_updated = _raw_accel_sub->update(&_raw_accel_time, &sensor_accel); bool updated = false;
_raw_gyro_sub->update(&_raw_gyro_time, &_sensor_gyro); updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel);
updated |= _raw_gyro_sub->update(&_raw_gyro_time, &sensor_gyro);
// send if raw_accel has been updated and use the newest gyro values we have if (updated) {
if (raw_accel_updated) {
mavlink_scaled_imu_t msg = {}; mavlink_scaled_imu_t msg = {};
@ -800,9 +798,9 @@ protected:
msg.xacc = (int16_t)(sensor_accel.x_raw / CONSTANTS_ONE_G); // [milli g] msg.xacc = (int16_t)(sensor_accel.x_raw / CONSTANTS_ONE_G); // [milli g]
msg.yacc = (int16_t)(sensor_accel.y_raw / CONSTANTS_ONE_G); // [milli g] msg.yacc = (int16_t)(sensor_accel.y_raw / CONSTANTS_ONE_G); // [milli g]
msg.zacc = (int16_t)(sensor_accel.z_raw / CONSTANTS_ONE_G); // [milli g] msg.zacc = (int16_t)(sensor_accel.z_raw / CONSTANTS_ONE_G); // [milli g]
msg.xgyro = _sensor_gyro.x_raw; // [milli rad/s] msg.xgyro = sensor_gyro.x_raw; // [milli rad/s]
msg.ygyro = _sensor_gyro.y_raw; // [milli rad/s] msg.ygyro = sensor_gyro.y_raw; // [milli rad/s]
msg.zgyro = _sensor_gyro.z_raw; // [milli rad/s] msg.zgyro = sensor_gyro.z_raw; // [milli rad/s]
msg.xmag = 0; msg.xmag = 0;
msg.ymag = 0; msg.ymag = 0;
msg.zmag = 0; msg.zmag = 0;
@ -867,7 +865,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct vehicle_attitude_s att; vehicle_attitude_s att;
if (_att_sub->update(&_att_time, &att)) { if (_att_sub->update(&_att_time, &att)) {
mavlink_attitude_t msg = {}; mavlink_attitude_t msg = {};
@ -939,7 +937,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct vehicle_attitude_s att; vehicle_attitude_s att;
if (_att_sub->update(&_att_time, &att)) { if (_att_sub->update(&_att_time, &att)) {
mavlink_attitude_quaternion_t msg = {}; mavlink_attitude_quaternion_t msg = {};
@ -999,12 +997,22 @@ public:
private: private:
MavlinkOrbSubscription *_att_sub; MavlinkOrbSubscription *_att_sub;
uint64_t _att_time;
MavlinkOrbSubscription *_pos_sub; MavlinkOrbSubscription *_pos_sub;
uint64_t _pos_time;
MavlinkOrbSubscription *_armed_sub; MavlinkOrbSubscription *_armed_sub;
uint64_t _armed_time;
MavlinkOrbSubscription *_act0_sub; MavlinkOrbSubscription *_act0_sub;
MavlinkOrbSubscription *_act1_sub; MavlinkOrbSubscription *_act1_sub;
MavlinkOrbSubscription *_airspeed_sub; MavlinkOrbSubscription *_airspeed_sub;
uint64_t _airspeed_time;
MavlinkOrbSubscription *_sensor_sub; MavlinkOrbSubscription *_sensor_sub;
uint64_t _sensor_time;
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
@ -1013,11 +1021,15 @@ private:
protected: protected:
explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink),
_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
_att_time(0),
_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
_pos_time(0),
_armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))), _armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))),
_armed_time(0),
_act0_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), _act0_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
_act1_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_1))), _act1_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_1))),
_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
_airspeed_time(0),
_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))) _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined)))
{} {}
@ -1027,13 +1039,13 @@ protected:
vehicle_local_position_s pos = {}; vehicle_local_position_s pos = {};
actuator_armed_s armed = {}; actuator_armed_s armed = {};
airspeed_s airspeed = {}; airspeed_s airspeed = {};
sensor_combined_s sensor = {};
bool updated = _att_sub->update(&att);
updated |= _pos_sub->update(&pos); bool updated = false;
updated |= _armed_sub->update(&armed); updated |= _att_sub->update(&_att_time, &att);
updated |= _airspeed_sub->update(&airspeed); updated |= _pos_sub->update(&_pos_time, &pos);
updated |= _sensor_sub->update(&sensor); updated |= _armed_sub->update(&_armed_time, &armed);
updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
if (updated) { if (updated) {
mavlink_vfr_hud_t msg = {}; mavlink_vfr_hud_t msg = {};
@ -1066,6 +1078,9 @@ protected:
msg.alt = -pos.z + pos.ref_alt; msg.alt = -pos.z + pos.ref_alt;
} else { } else {
sensor_combined_s sensor = {};
_sensor_sub->update(&sensor);
/* fall back to baro altitude */ /* fall back to baro altitude */
if (sensor.timestamp > 0) { if (sensor.timestamp > 0) {
msg.alt = sensor.baro_alt_meter; msg.alt = sensor.baro_alt_meter;
@ -1135,7 +1150,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct vehicle_gps_position_s gps; vehicle_gps_position_s gps;
if (_gps_sub->update(&_gps_time, &gps)) { if (_gps_sub->update(&_gps_time, &gps)) {
mavlink_gps_raw_int_t msg = {}; mavlink_gps_raw_int_t msg = {};
@ -1152,9 +1167,9 @@ protected:
msg.v_acc = gps.epv * 1e3f; msg.v_acc = gps.epv * 1e3f;
msg.vel_acc = gps.s_variance_m_s * 1e3f; msg.vel_acc = gps.s_variance_m_s * 1e3f;
msg.hdg_acc = gps.c_variance_rad * 1e5f / M_DEG_TO_RAD_F; msg.hdg_acc = gps.c_variance_rad * 1e5f / M_DEG_TO_RAD_F;
msg.vel = cm_uint16_from_m_float(gps.vel_m_s), msg.vel = cm_uint16_from_m_float(gps.vel_m_s);
msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f;
msg.satellites_visible = gps.satellites_used; msg.satellites_visible = gps.satellites_used;
mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg); mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg);
@ -1679,7 +1694,11 @@ public:
private: private:
MavlinkOrbSubscription *_gpos_sub; MavlinkOrbSubscription *_gpos_sub;
uint64_t _gpos_time;
MavlinkOrbSubscription *_lpos_sub; MavlinkOrbSubscription *_lpos_sub;
uint64_t _lpos_time;
MavlinkOrbSubscription *_home_sub; MavlinkOrbSubscription *_home_sub;
MavlinkOrbSubscription *_sensor_sub; MavlinkOrbSubscription *_sensor_sub;
@ -1690,7 +1709,9 @@ private:
protected: protected:
explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink), explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink),
_gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), _gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
_gpos_time(0),
_lpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), _lpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
_lpos_time(0),
_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))),
_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))) _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined)))
{} {}
@ -1699,16 +1720,11 @@ protected:
{ {
vehicle_global_position_s gpos = {}; vehicle_global_position_s gpos = {};
vehicle_local_position_s lpos = {}; vehicle_local_position_s lpos = {};
home_position_s home = {};
sensor_combined_s sensor = {};
bool updated = false; bool gpos_updated = _gpos_sub->update(&_gpos_time, &gpos);
updated |= _gpos_sub->update(&gpos); bool lpos_updated = _lpos_sub->update(&_lpos_time, &lpos);
updated |= _lpos_sub->update(&lpos);
updated |= _home_sub->update(&home);
updated |= _sensor_sub->update(&sensor);
if (updated) { if (gpos_updated && lpos_updated) {
mavlink_global_position_int_t msg = {}; mavlink_global_position_int_t msg = {};
if (lpos.z_valid && lpos.z_global) { if (lpos.z_valid && lpos.z_global) {
@ -1716,12 +1732,18 @@ protected:
} else { } else {
// fall back to baro altitude // fall back to baro altitude
sensor_combined_s sensor = {};
_sensor_sub->update(&sensor);
if (sensor.timestamp > 0) { if (sensor.timestamp > 0) {
msg.alt = sensor.baro_alt_meter * 1000.0f; msg.alt = sensor.baro_alt_meter * 1000.0f;
} }
} }
if (home.valid_alt) { home_position_s home = {};
_home_sub->update(&home);
if ((home.timestamp > 0) && home.valid_alt) {
if (lpos.z_valid) { if (lpos.z_valid) {
msg.relative_alt = -(lpos.z - home.z) * 1000.0f; msg.relative_alt = -(lpos.z - home.z) * 1000.0f;
@ -1746,9 +1768,11 @@ protected:
msg.hdg = _wrap_2pi(lpos.yaw) * M_RAD_TO_DEG_F * 100.0f; msg.hdg = _wrap_2pi(lpos.yaw) * M_RAD_TO_DEG_F * 100.0f;
mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg); mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
return true;
} }
return updated; return false;
} }
}; };
@ -1806,8 +1830,8 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct vehicle_local_position_s vpos = {}; vehicle_local_position_s vpos = {};
struct vehicle_attitude_s vatt = {}; vehicle_attitude_s vatt = {};
bool att_updated = _att_sub->update(&_att_time, &vatt); bool att_updated = _att_sub->update(&_att_time, &vatt);
bool pos_updated = _pos_sub->update(&_pos_time, &vpos); bool pos_updated = _pos_sub->update(&_pos_time, &vpos);
@ -1880,7 +1904,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct vehicle_local_position_s pos; vehicle_local_position_s pos;
if (_pos_sub->update(&_pos_time, &pos)) { if (_pos_sub->update(&_pos_time, &pos)) {
mavlink_local_position_ned_t msg = {}; mavlink_local_position_ned_t msg = {};
@ -2034,32 +2058,29 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct estimator_status_s est; estimator_status_s est;
if (_est_sub->update(&_est_time, &est)) { if (_est_sub->update(&_est_time, &est)) {
// ESTIMATOR_STATUS
mavlink_estimator_status_t est_msg = { mavlink_estimator_status_t est_msg = {};
.time_usec = est.timestamp, est_msg.time_usec = est.timestamp;
.vel_ratio = est.vel_test_ratio, est_msg.vel_ratio = est.vel_test_ratio;
.pos_horiz_ratio = est.pos_test_ratio, est_msg.pos_horiz_ratio = est.pos_test_ratio;
.pos_vert_ratio = est.hgt_test_ratio, est_msg.pos_vert_ratio = est.hgt_test_ratio;
.mag_ratio = est.mag_test_ratio, est_msg.mag_ratio = est.mag_test_ratio;
.hagl_ratio = est.hagl_test_ratio, est_msg.hagl_ratio = est.hagl_test_ratio;
.tas_ratio = est.tas_test_ratio, est_msg.tas_ratio = est.tas_test_ratio;
.pos_horiz_accuracy = est.pos_horiz_accuracy, est_msg.pos_horiz_accuracy = est.pos_horiz_accuracy;
.pos_vert_accuracy = est.pos_vert_accuracy, est_msg.pos_vert_accuracy = est.pos_vert_accuracy;
.flags = est.solution_status_flags est_msg.flags = est.solution_status_flags;
};
mavlink_msg_estimator_status_send_struct(_mavlink->get_channel(), &est_msg); mavlink_msg_estimator_status_send_struct(_mavlink->get_channel(), &est_msg);
mavlink_vibration_t msg = { // VIBRATION
.time_usec = est.timestamp, mavlink_vibration_t msg = {};
.vibration_x = est.vibe[0], msg.time_usec = est.timestamp;
.vibration_y = est.vibe[1], msg.vibration_x = est.vibe[0];
.vibration_z = est.vibe[2] msg.vibration_y = est.vibe[1];
}; msg.vibration_z = est.vibe[2];
mavlink_msg_vibration_send_struct(_mavlink->get_channel(), &msg); mavlink_msg_vibration_send_struct(_mavlink->get_channel(), &msg);
return true; return true;
@ -2118,7 +2139,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct att_pos_mocap_s mocap; att_pos_mocap_s mocap;
if (_mocap_sub->update(&_mocap_time, &mocap)) { if (_mocap_sub->update(&_mocap_time, &mocap)) {
mavlink_att_pos_mocap_t msg = {}; mavlink_att_pos_mocap_t msg = {};
@ -2192,7 +2213,7 @@ protected:
/* we're sending the GPS home periodically to ensure the /* we're sending the GPS home periodically to ensure the
* the GCS does pick it up at one point */ * the GCS does pick it up at one point */
if (_home_sub->is_published()) { if (_home_sub->is_published()) {
struct home_position_s home; home_position_s home;
if (_home_sub->update(&home)) { if (_home_sub->update(&home)) {
if (home.valid_hpos) { if (home.valid_hpos) {
@ -2284,15 +2305,13 @@ private:
protected: protected:
explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink), explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink),
_act_sub(nullptr), _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N)),
_act_time(0) _act_time(0)
{ {}
_act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N);
}
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct actuator_outputs_s act; actuator_outputs_s act;
if (_act_sub->update(&_act_time, &act)) { if (_act_sub->update(&_act_time, &act)) {
mavlink_servo_output_raw_t msg = {}; mavlink_servo_output_raw_t msg = {};
@ -2398,7 +2417,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct actuator_controls_s att_ctrl; actuator_controls_s att_ctrl;
if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) { if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) {
mavlink_actuator_control_target_t msg = {}; mavlink_actuator_control_target_t msg = {};
@ -2475,10 +2494,11 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct vehicle_status_s status; vehicle_status_s status = {};
struct actuator_outputs_s act; actuator_outputs_s act = {};
bool updated = _act_sub->update(&_act_time, &act); bool updated = false;
updated |= _act_sub->update(&_act_time, &act);
updated |= _status_sub->update(&_status_time, &status); updated |= _status_sub->update(&_status_time, &status);
if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
@ -2621,7 +2641,6 @@ public:
private: private:
MavlinkOrbSubscription *_status_sub; MavlinkOrbSubscription *_status_sub;
uint64_t _status_time;
MavlinkOrbSubscription *_act_sub; MavlinkOrbSubscription *_act_sub;
uint64_t _act_time; uint64_t _act_time;
@ -2633,116 +2652,118 @@ private:
protected: protected:
explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink), explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink),
_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
_status_time(0),
_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))), _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))),
_act_time(0) _act_time(0)
{} {}
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct vehicle_status_s status; actuator_outputs_s act;
struct actuator_outputs_s act;
bool updated = _act_sub->update(&_act_time, &act); if (_act_sub->update(&_act_time, &act)) {
updated |= _status_sub->update(&_status_time, &status); vehicle_status_s status = {};
_status_sub->update(&status);
if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { if ((status.timestamp > 0) && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
/* translate the current syste state to mavlink state and mode */ /* translate the current system state to mavlink state and mode */
uint8_t mavlink_state; uint8_t mavlink_state;
uint8_t mavlink_base_mode; uint8_t mavlink_base_mode;
uint32_t mavlink_custom_mode; uint32_t mavlink_custom_mode;
mavlink_hil_actuator_controls_t msg = {}; mavlink_hil_actuator_controls_t msg = {};
get_mavlink_mode_state(&status, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); get_mavlink_mode_state(&status, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
unsigned system_type = _mavlink->get_system_type(); unsigned system_type = _mavlink->get_system_type();
/* scale outputs depending on system type */ /* scale outputs depending on system type */
if (system_type == MAV_TYPE_QUADROTOR || if (system_type == MAV_TYPE_QUADROTOR ||
system_type == MAV_TYPE_HEXAROTOR || system_type == MAV_TYPE_HEXAROTOR ||
system_type == MAV_TYPE_OCTOROTOR || system_type == MAV_TYPE_OCTOROTOR ||
system_type == MAV_TYPE_VTOL_DUOROTOR || system_type == MAV_TYPE_VTOL_DUOROTOR ||
system_type == MAV_TYPE_VTOL_QUADROTOR || system_type == MAV_TYPE_VTOL_QUADROTOR ||
system_type == MAV_TYPE_VTOL_RESERVED2) { system_type == MAV_TYPE_VTOL_RESERVED2) {
/* multirotors: set number of rotor outputs depending on type */ /* multirotors: set number of rotor outputs depending on type */
unsigned n; unsigned n;
switch (system_type) { switch (system_type) {
case MAV_TYPE_QUADROTOR: case MAV_TYPE_QUADROTOR:
n = 4; n = 4;
break; break;
case MAV_TYPE_HEXAROTOR: case MAV_TYPE_HEXAROTOR:
n = 6; n = 6;
break; break;
case MAV_TYPE_VTOL_DUOROTOR: case MAV_TYPE_VTOL_DUOROTOR:
n = 2; n = 2;
break; break;
case MAV_TYPE_VTOL_QUADROTOR: case MAV_TYPE_VTOL_QUADROTOR:
n = 4; n = 4;
break; break;
case MAV_TYPE_VTOL_RESERVED2: case MAV_TYPE_VTOL_RESERVED2:
n = 8; n = 8;
break; break;
default: default:
n = 8; n = 8;
break; break;
} }
for (unsigned i = 0; i < 16; i++) { for (unsigned i = 0; i < 16; i++) {
if (act.output[i] > PWM_DEFAULT_MIN / 2) { if (act.output[i] > PWM_DEFAULT_MIN / 2) {
if (i < n) { if (i < n) {
/* scale PWM out 900..2100 us to 0..1 for rotors */ /* scale PWM out 900..2100 us to 0..1 for rotors */
msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
} else {
/* scale PWM out 900..2100 us to -1..1 for other channels */
msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
}
} else { } else {
/* scale PWM out 900..2100 us to -1..1 for other channels */ /* send 0 when disarmed and for disabled channels */
msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); msg.controls[i] = 0.0f;
} }
}
} else { } else {
/* send 0 when disarmed and for disabled channels */ /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
msg.controls[i] = 0.0f;
for (unsigned i = 0; i < 16; i++) {
if (act.output[i] > PWM_DEFAULT_MIN / 2) {
if (i != 3) {
/* scale PWM out 900..2100 us to -1..1 for normal channels */
msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
} else {
/* scale PWM out 900..2100 us to 0..1 for throttle */
msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
}
} else {
/* set 0 for disabled channels */
msg.controls[i] = 0.0f;
}
} }
} }
} else { msg.time_usec = hrt_absolute_time();
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ msg.mode = mavlink_base_mode;
msg.flags = 0;
for (unsigned i = 0; i < 16; i++) { mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg);
if (act.output[i] > PWM_DEFAULT_MIN / 2) {
if (i != 3) {
/* scale PWM out 900..2100 us to -1..1 for normal channels */
msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
} else { return true;
/* scale PWM out 900..2100 us to 0..1 for throttle */
msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
}
} else {
/* set 0 for disabled channels */
msg.controls[i] = 0.0f;
}
}
} }
msg.time_usec = hrt_absolute_time();
msg.mode = mavlink_base_mode;
msg.flags = 0;
mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg);
} }
return (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)); return false;
} }
}; };
@ -2781,6 +2802,7 @@ public:
private: private:
MavlinkOrbSubscription *_pos_sp_triplet_sub; MavlinkOrbSubscription *_pos_sp_triplet_sub;
uint64_t _pos_sp_triplet_timestamp{0};
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &); MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &);
@ -2793,9 +2815,9 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct position_setpoint_triplet_s pos_sp_triplet; position_setpoint_triplet_s pos_sp_triplet;
if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { if (_pos_sp_triplet_sub->update(&_pos_sp_triplet_timestamp, &pos_sp_triplet)) {
mavlink_position_target_global_int_t msg = {}; mavlink_position_target_global_int_t msg = {};
msg.time_boot_ms = hrt_absolute_time() / 1000; msg.time_boot_ms = hrt_absolute_time() / 1000;
@ -2863,7 +2885,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct vehicle_local_position_setpoint_s pos_sp; vehicle_local_position_setpoint_s pos_sp;
if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) {
mavlink_position_target_local_ned_t msg = {}; mavlink_position_target_local_ned_t msg = {};
@ -2927,8 +2949,8 @@ public:
private: private:
MavlinkOrbSubscription *_att_sp_sub; MavlinkOrbSubscription *_att_sp_sub;
MavlinkOrbSubscription *_att_rates_sp_sub; MavlinkOrbSubscription *_att_rates_sp_sub;
uint64_t _att_sp_time; uint64_t _att_sp_time;
uint64_t _att_rates_sp_time;
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &); MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &);
@ -2938,18 +2960,17 @@ protected:
explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink), explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink),
_att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), _att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
_att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))),
_att_sp_time(0), _att_sp_time(0)
_att_rates_sp_time(0)
{} {}
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct vehicle_attitude_setpoint_s att_sp; vehicle_attitude_setpoint_s att_sp;
if (_att_sp_sub->update(&_att_sp_time, &att_sp)) { if (_att_sp_sub->update(&_att_sp_time, &att_sp)) {
struct vehicle_rates_setpoint_s att_rates_sp = {}; vehicle_rates_setpoint_s att_rates_sp = {};
(void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); _att_rates_sp_sub->update(&att_rates_sp);
mavlink_attitude_target_t msg = {}; mavlink_attitude_target_t msg = {};
@ -3027,7 +3048,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct rc_input_values rc; rc_input_values rc;
if (_rc_sub->update(&_rc_time, &rc)) { if (_rc_sub->update(&_rc_time, &rc)) {
@ -3132,7 +3153,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct manual_control_setpoint_s manual; manual_control_setpoint_s manual;
if (_manual_sub->update(&_manual_time, &manual)) { if (_manual_sub->update(&_manual_time, &manual)) {
mavlink_manual_control_t msg = {}; mavlink_manual_control_t msg = {};
@ -3209,7 +3230,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct optical_flow_s flow; optical_flow_s flow;
if (_flow_sub->update(&_flow_time, &flow)) { if (_flow_sub->update(&_flow_time, &flow)) {
mavlink_optical_flow_rad_t msg = {}; mavlink_optical_flow_rad_t msg = {};
@ -3482,6 +3503,9 @@ private:
MavlinkOrbSubscription *_fw_pos_ctrl_status_sub; MavlinkOrbSubscription *_fw_pos_ctrl_status_sub;
MavlinkOrbSubscription *_tecs_status_sub; MavlinkOrbSubscription *_tecs_status_sub;
uint64_t _fw_pos_ctrl_status_timestamp{0};
uint64_t _tecs_status_timestamp{0};
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkStreamNavControllerOutput(MavlinkStreamNavControllerOutput &); MavlinkStreamNavControllerOutput(MavlinkStreamNavControllerOutput &);
MavlinkStreamNavControllerOutput &operator = (const MavlinkStreamNavControllerOutput &); MavlinkStreamNavControllerOutput &operator = (const MavlinkStreamNavControllerOutput &);
@ -3494,13 +3518,14 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct fw_pos_ctrl_status_s _fw_pos_ctrl_status; fw_pos_ctrl_status_s _fw_pos_ctrl_status = {};
struct tecs_status_s _tecs_status; tecs_status_s _tecs_status = {};
const bool updated_fw_pos_ctrl_status = _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status); bool updated = false;
const bool updated_tecs = _tecs_status_sub->update(&_tecs_status); updated |= _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_timestamp, &_fw_pos_ctrl_status);
updated |= _tecs_status_sub->update(&_tecs_status_timestamp, &_tecs_status);
if (updated_fw_pos_ctrl_status || updated_tecs) { if (updated) {
mavlink_nav_controller_output_t msg = {}; mavlink_nav_controller_output_t msg = {};
msg.nav_roll = math::degrees(_fw_pos_ctrl_status.nav_roll); msg.nav_roll = math::degrees(_fw_pos_ctrl_status.nav_roll);
@ -3568,7 +3593,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct vehicle_status_s status; vehicle_status_s status;
if (_status_sub->update(&status)) { if (_status_sub->update(&status)) {
@ -3644,7 +3669,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct distance_sensor_s dist_sensor; distance_sensor_s dist_sensor;
if (_distance_sensor_sub->update(&_dist_sensor_time, &dist_sensor)) { if (_distance_sensor_sub->update(&_dist_sensor_time, &dist_sensor)) {
@ -3746,10 +3771,10 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct vehicle_status_s status;
struct vehicle_land_detected_s land_detected;
bool updated = false; bool updated = false;
vehicle_status_s status;
if (_status_sub->update(&status)) { if (_status_sub->update(&status)) {
updated = true; updated = true;
@ -3769,6 +3794,8 @@ protected:
} }
} }
vehicle_land_detected_s land_detected;
if (_landed_sub->update(&land_detected)) { if (_landed_sub->update(&land_detected)) {
updated = true; updated = true;
@ -3778,8 +3805,8 @@ protected:
} else if (!land_detected.landed) { } else if (!land_detected.landed) {
_msg.landed_state = MAV_LANDED_STATE_IN_AIR; _msg.landed_state = MAV_LANDED_STATE_IN_AIR;
vehicle_control_mode_s control_mode = {}; vehicle_control_mode_s control_mode;
position_setpoint_triplet_s pos_sp_triplet = {}; position_setpoint_triplet_s pos_sp_triplet;
if (_control_mode_sub->update(&control_mode) && _pos_sp_triplet_sub->update(&pos_sp_triplet)) { if (_control_mode_sub->update(&control_mode) && _pos_sp_triplet_sub->update(&pos_sp_triplet)) {
if (control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { if (control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) {
@ -3836,7 +3863,6 @@ public:
} }
private: private:
MavlinkOrbSubscription *_global_pos_sub;
MavlinkOrbSubscription *_local_pos_sub; MavlinkOrbSubscription *_local_pos_sub;
MavlinkOrbSubscription *_home_sub; MavlinkOrbSubscription *_home_sub;
MavlinkOrbSubscription *_sensor_sub; MavlinkOrbSubscription *_sensor_sub;
@ -3849,7 +3875,6 @@ private:
protected: protected:
explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink), explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink),
_global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
_local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))),
_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))) _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined)))
@ -3857,7 +3882,7 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
mavlink_altitude_t msg; mavlink_altitude_t msg = {};
msg.altitude_monotonic = NAN; msg.altitude_monotonic = NAN;
msg.altitude_amsl = NAN; msg.altitude_amsl = NAN;
@ -3866,20 +3891,20 @@ protected:
msg.altitude_terrain = NAN; msg.altitude_terrain = NAN;
msg.bottom_clearance = NAN; msg.bottom_clearance = NAN;
bool updated = false;
// always update monotonic altitude // always update monotonic altitude
bool sensor_updated = false;
sensor_combined_s sensor = {}; sensor_combined_s sensor = {};
uint64_t sensor_time = 0; _sensor_sub->update(&sensor);
_sensor_sub->update(&sensor_time, &sensor);
if ((sensor.timestamp > 0) && (sensor_time > 0)) { if (sensor.timestamp > 0) {
msg.altitude_monotonic = sensor.baro_alt_meter; msg.altitude_monotonic = sensor.baro_alt_meter;
updated = true; sensor_updated = true;
} }
vehicle_local_position_s local_pos = {}; bool lpos_updated = false;
vehicle_local_position_s local_pos;
if (_local_pos_sub->update(&_local_pos_time, &local_pos)) { if (_local_pos_sub->update(&_local_pos_time, &local_pos)) {
@ -3893,7 +3918,7 @@ protected:
msg.altitude_local = -local_pos.z; msg.altitude_local = -local_pos.z;
home_position_s home; home_position_s home = {};
_home_sub->update(&home); _home_sub->update(&home);
if (home.valid_alt) { if (home.valid_alt) {
@ -3909,15 +3934,21 @@ protected:
} }
} }
updated = true; lpos_updated = true;
} }
if (updated) { // local position timeout after 10 ms
// avoid publishing only baro altitude_monotonic if possible
bool lpos_timeout = (hrt_elapsed_time(&_local_pos_time) > 10000);
if (lpos_updated || (sensor_updated && lpos_timeout)) {
msg.time_usec = hrt_absolute_time(); msg.time_usec = hrt_absolute_time();
mavlink_msg_altitude_send_struct(_mavlink->get_channel(), &msg); mavlink_msg_altitude_send_struct(_mavlink->get_channel(), &msg);
return true;
} }
return updated; return false;
} }
}; };
@ -3958,8 +3989,7 @@ private:
MavlinkOrbSubscription *_wind_estimate_sub; MavlinkOrbSubscription *_wind_estimate_sub;
uint64_t _wind_estimate_time; uint64_t _wind_estimate_time;
MavlinkOrbSubscription *_global_pos_sub; MavlinkOrbSubscription *_local_pos_sub;
uint64_t _global_pos_time;
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkStreamWind(MavlinkStreamWind &); MavlinkStreamWind(MavlinkStreamWind &);
@ -3969,13 +3999,12 @@ protected:
explicit MavlinkStreamWind(Mavlink *mavlink) : MavlinkStream(mavlink), explicit MavlinkStreamWind(Mavlink *mavlink) : MavlinkStream(mavlink),
_wind_estimate_sub(_mavlink->add_orb_subscription(ORB_ID(wind_estimate))), _wind_estimate_sub(_mavlink->add_orb_subscription(ORB_ID(wind_estimate))),
_wind_estimate_time(0), _wind_estimate_time(0),
_global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)))
_global_pos_time(0)
{} {}
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct wind_estimate_s wind_estimate; wind_estimate_s wind_estimate;
if (_wind_estimate_sub->update(&_wind_estimate_time, &wind_estimate)) { if (_wind_estimate_sub->update(&_wind_estimate_time, &wind_estimate)) {
@ -3990,10 +4019,9 @@ protected:
msg.var_horiz = wind_estimate.variance_north + wind_estimate.variance_east; msg.var_horiz = wind_estimate.variance_north + wind_estimate.variance_east;
msg.var_vert = 0.0f; msg.var_vert = 0.0f;
struct vehicle_global_position_s global_pos = {}; vehicle_local_position_s lpos = {};
_global_pos_sub->update(&_global_pos_time, &global_pos); _local_pos_sub->update(&lpos);
msg.wind_alt = (_global_pos_time > 0) ? global_pos.alt : NAN; msg.wind_alt = (lpos.z_valid && lpos.z_global) ? (-lpos.z + lpos.ref_alt) : NAN;
msg.horiz_accuracy = 0.0f; msg.horiz_accuracy = 0.0f;
msg.vert_accuracy = 0.0f; msg.vert_accuracy = 0.0f;
@ -4192,20 +4220,20 @@ protected:
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
struct actuator_controls_s actuator = {}; actuator_controls_s actuator = {};
struct airspeed_s airspeed = {}; airspeed_s airspeed = {};
struct battery_status_s battery = {}; battery_status_s battery = {};
struct fw_pos_ctrl_status_s fw_pos_ctrl_status = {}; fw_pos_ctrl_status_s fw_pos_ctrl_status = {};
struct home_position_s home = {}; home_position_s home = {};
struct mission_result_s mission_result = {}; mission_result_s mission_result = {};
struct sensor_combined_s sensor = {}; sensor_combined_s sensor = {};
struct tecs_status_s tecs_status = {}; tecs_status_s tecs_status = {};
struct vehicle_attitude_s attitude = {}; vehicle_attitude_s attitude = {};
struct vehicle_attitude_setpoint_s attitude_sp = {}; vehicle_attitude_setpoint_s attitude_sp = {};
struct vehicle_global_position_s global_pos = {}; vehicle_global_position_s global_pos = {};
struct vehicle_gps_position_s gps = {}; vehicle_gps_position_s gps = {};
struct vehicle_land_detected_s land_detected = {}; vehicle_land_detected_s land_detected = {};
struct vehicle_status_s status = {}; vehicle_status_s status = {};
bool updated = _status_sub->update(&_status_time, &status); bool updated = _status_sub->update(&_status_time, &status);
updated |= _actuator_sub->update(&_actuator_time, &actuator); updated |= _actuator_sub->update(&_actuator_time, &actuator);
@ -4322,8 +4350,6 @@ private:
MavlinkOrbSubscription *_gpos_sub; MavlinkOrbSubscription *_gpos_sub;
uint64_t _att_time; uint64_t _att_time;
uint64_t _gpos_time; uint64_t _gpos_time;
struct vehicle_attitude_s _att;
struct vehicle_global_position_s _gpos;
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkStreamGroundTruth(MavlinkStreamGroundTruth &); MavlinkStreamGroundTruth(MavlinkStreamGroundTruth &);
@ -4334,43 +4360,42 @@ protected:
_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_groundtruth))), _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_groundtruth))),
_gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position_groundtruth))), _gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position_groundtruth))),
_att_time(0), _att_time(0),
_gpos_time(0), _gpos_time(0)
_att(),
_gpos()
{} {}
bool send(const hrt_abstime t) bool send(const hrt_abstime t)
{ {
bool att_updated = _att_sub->update(&_att_time, &_att);
bool gpos_updated = _gpos_sub->update(&_gpos_time, &_gpos); vehicle_attitude_s att = {};
vehicle_global_position_s gpos = {};
bool att_updated = _att_sub->update(&_att_time, &att);
bool gpos_updated = _gpos_sub->update(&_gpos_time, &gpos);
if (att_updated || gpos_updated) { if (att_updated || gpos_updated) {
mavlink_hil_state_quaternion_t msg = {}; mavlink_hil_state_quaternion_t msg = {};
if (att_updated) { // vehicle_attitude -> hil_state_quaternion
msg.attitude_quaternion[0] = _att.q[0]; msg.attitude_quaternion[0] = att.q[0];
msg.attitude_quaternion[1] = _att.q[1]; msg.attitude_quaternion[1] = att.q[1];
msg.attitude_quaternion[2] = _att.q[2]; msg.attitude_quaternion[2] = att.q[2];
msg.attitude_quaternion[3] = _att.q[3]; msg.attitude_quaternion[3] = att.q[3];
msg.rollspeed = _att.rollspeed; msg.rollspeed = att.rollspeed;
msg.pitchspeed = _att.pitchspeed; msg.pitchspeed = att.pitchspeed;
msg.yawspeed = _att.yawspeed; msg.yawspeed = att.yawspeed;
}
if (gpos_updated) { // vehicle_global_position -> hil_state_quaternion
msg.lat = _gpos.lat; msg.lat = gpos.lat;
msg.lon = _gpos.lon; msg.lon = gpos.lon;
msg.alt = _gpos.alt; msg.alt = gpos.alt;
msg.vx = _gpos.vel_n; msg.vx = gpos.vel_n;
msg.vy = _gpos.vel_e; msg.vy = gpos.vel_e;
msg.vz = _gpos.vel_d; msg.vz = gpos.vel_d;
msg.ind_airspeed = 0; msg.ind_airspeed = 0;
msg.true_airspeed = 0; msg.true_airspeed = 0;
msg.xacc = 0; msg.xacc = 0;
msg.yacc = 0; msg.yacc = 0;
msg.zacc = 0; msg.zacc = 0;
}
mavlink_msg_hil_state_quaternion_send_struct(_mavlink->get_channel(), &msg); mavlink_msg_hil_state_quaternion_send_struct(_mavlink->get_channel(), &msg);

7
src/modules/mavlink/mavlink_orb_subscription.cpp
View File

@ -114,12 +114,7 @@ MavlinkOrbSubscription::update(void *data)
return false; return false;
} }
if (orb_copy(_topic, _fd, data)) { if (orb_copy(_topic, _fd, data) != PX4_OK) {
if (data != nullptr) {
/* error copying topic data */
memset(data, 0, _topic->o_size);
}
return false; return false;
} }