diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index f5b1666085..4e8ec96553 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -70,6 +70,3 @@ bool mission_failure # Set to true if mission could not continue/finish uint32 onboard_control_sensors_present uint32 onboard_control_sensors_enabled uint32 onboard_control_sensors_health - -float32 load # processor load from 0 to 1 - diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2318444e97..310e9dd264 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -99,6 +99,7 @@ #include #include #include +#include #include #include @@ -107,7 +108,6 @@ #include #include #include -#include #include #include #include @@ -214,6 +214,7 @@ struct manual_control_setpoint_s sp_man = {}; ///< the current manual control s static manual_control_setpoint_s _last_sp_man = {}; ///< the manual control setpoint valid at the last mode switch static struct vtol_vehicle_status_s vtol_status = {}; +static struct cpuload_s cpuload = {}; static uint8_t main_state_prev = 0; @@ -257,7 +258,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe int commander_thread_main(int argc, char *argv[]); void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, - battery_status_s *battery); + battery_status_s *battery, const cpuload_s *cpuload_local); void get_circuit_breaker_params(); @@ -1368,8 +1369,6 @@ int commander_thread_main(int argc, char *argv[]) bool low_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false; - hrt_abstime last_idle_time = 0; - bool status_changed = true; bool param_init_forced = true; @@ -1494,8 +1493,10 @@ int commander_thread_main(int argc, char *argv[]) memset(&vtol_status, 0, sizeof(vtol_status)); vtol_status.vtol_in_rw_mode = true; //default for vtol is rotary wing + int cpuload_sub = orb_subscribe(ORB_ID(cpuload)); + memset(&cpuload, 0, sizeof(cpuload)); - control_status_leds(&status, &armed, true, &battery); + control_status_leds(&status, &armed, true, &battery, &cpuload); /* now initialized */ commander_initialized = true; @@ -1997,6 +1998,12 @@ int commander_thread_main(int argc, char *argv[]) main_state_before_rtl = internal_state.main_state; } + orb_check(cpuload_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(cpuload), cpuload_sub, &cpuload); + } + /* update battery status */ orb_check(battery_sub, &updated); @@ -2095,17 +2102,6 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* compute system load */ - uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - - if (last_idle_time > 0) { - status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle - } - - last_idle_time = system_load.tasks[0].total_runtime; - } - /* If in INIT state, try to proceed to STANDBY state */ if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { arming_ret = arming_state_transition(&status, @@ -2803,12 +2799,12 @@ int commander_thread_main(int argc, char *argv[]) /* blinking LED message, don't touch LEDs */ if (blink_state == 2) { /* blinking LED message completed, restore normal state */ - control_status_leds(&status, &armed, true, &battery); + control_status_leds(&status, &armed, true, &battery, &cpuload); } } else { /* normal state */ - control_status_leds(&status, &armed, status_changed, &battery); + control_status_leds(&status, &armed, status_changed, &battery, &cpuload); } status_changed = false; @@ -2878,8 +2874,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } } -void -control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery_status) +control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery, const cpuload_s *cpuload_local) { /* driving rgbled */ if (changed) { @@ -2950,7 +2945,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu #endif /* give system warnings on error LED, XXX maybe add memory usage warning too */ - if (status_local->load > 0.95f) { + if (cpuload_local->load > 0.95f) { if (leds_counter % 2 == 0) { led_toggle(LED_AMBER); } @@ -3479,17 +3474,17 @@ set_control_mode() control_mode.flag_control_rattitude_enabled = false; - control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force && - !status.in_transition_mode; + control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force && + !status.in_transition_mode; control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position) && !status.in_transition_mode && + !offboard_control_mode.ignore_position) && !status.in_transition_mode && !control_mode.flag_control_acceleration_enabled; control_mode.flag_control_climb_rate_enabled = (!offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled; - control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode && + control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode && !control_mode.flag_control_acceleration_enabled; control_mode.flag_control_altitude_enabled = (!offboard_control_mode.ignore_velocity ||