diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index afcde463da..3de8994ec1 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -427,9 +427,16 @@ VtolAttitudeControl::Run() _vehicle_thrust_setpoint0_pub.publish(_thrust_setpoint_0); _vehicle_thrust_setpoint1_pub.publish(_thrust_setpoint_1); - // Advertise/Publish vtol vehicle status - _vtol_vehicle_status.timestamp = hrt_absolute_time(); - _vtol_vehicle_status_pub.publish(_vtol_vehicle_status); + // Advertise/publish vtol vehicle status -- immediately if changed, otherwise at 1 Hz + const bool vtol_vehicle_status_changed = + (_vtol_vehicle_status.vehicle_vtol_state != _prev_published_vtol_vehicle_status.vehicle_vtol_state) || + (_vtol_vehicle_status.fixed_wing_system_failure != _prev_published_vtol_vehicle_status.fixed_wing_system_failure); + + if (vtol_vehicle_status_changed || hrt_elapsed_time(&_prev_published_vtol_vehicle_status.timestamp) >= 1_s) { + _vtol_vehicle_status.timestamp = hrt_absolute_time(); + _vtol_vehicle_status_pub.publish(_vtol_vehicle_status); + _prev_published_vtol_vehicle_status = _vtol_vehicle_status; + } // Publish flaps/spoiler setpoint with configured deflection in Hover if in Auto. // In Manual always published in FW rate controller, and in Auto FW in FW Position Controller. diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index aa34129e57..a69a614743 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -205,6 +205,7 @@ private: vehicle_local_position_setpoint_s _local_pos_sp{}; vehicle_status_s _vehicle_status{}; vtol_vehicle_status_s _vtol_vehicle_status{}; + vtol_vehicle_status_s _prev_published_vtol_vehicle_status{}; float _home_position_z{NAN}; float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos}; // [kg/m^3]