fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: remove load from vehicle_status message

Browse Source
This commit is contained in:
Jonathan Challinger 2016-02-26 18:14:03 -08:00 committed by Lorenz Meier
parent 749b598af1
commit 535cea4e77
2 changed files with 20 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/vehicle_status.msg
View File

@ -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

45
src/modules/commander/commander.cpp
View File

@ -99,6 +99,7 @@
#include <uORB/topics/input_rc.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/cpuload.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@ -107,7 +108,6 @@
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/cpuload.h>
#include <systemlib/rc_check.h>
#include <geo/geo.h>
#include <systemlib/state_table.h>
@ -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 ||