forked from Archive/PX4-Autopilot
commander cleanup battery failsafe handling
This commit is contained in:
Daniel Agar
parent
7f41ead238
commit
6396e486bd
|
|
@ -13,6 +13,7 @@ bool condition_local_position_valid # set to true by the commander app if the q
|
|||
bool condition_local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation
|
||||
bool condition_local_altitude_valid
|
||||
bool condition_power_input_valid # set if input power is valid
|
||||
bool condition_battery_healthy # set if battery is available and not low
|
||||
|
||||
bool circuit_breaker_engaged_power_check
|
||||
bool circuit_breaker_engaged_airspd_check
|
||||
|
|
|
|||
|
|
@ -152,7 +152,6 @@ static systemlib::Hysteresis auto_disarm_hysteresis(false);
|
|||
static float min_stick_change = 0.25f;
|
||||
|
||||
static struct vehicle_status_s status = {};
|
||||
static struct battery_status_s battery = {};
|
||||
static struct actuator_armed_s armed = {};
|
||||
static struct safety_s safety = {};
|
||||
static struct vehicle_control_mode_s control_mode = {};
|
||||
|
|
@ -203,8 +202,7 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]);
|
|||
*/
|
||||
void usage(const char *reason);
|
||||
|
||||
void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed,
|
||||
battery_status_s *battery_local, const cpuload_s *cpuload_local);
|
||||
void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, const uint8_t battery_warning, const cpuload_s *cpuload_local);
|
||||
|
||||
void get_circuit_breaker_params();
|
||||
|
||||
|
|
@ -376,7 +374,7 @@ int commander_main(int argc, char *argv[])
|
|||
bool preflight_check_res = Commander::preflight_check(true);
|
||||
PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED");
|
||||
|
||||
bool prearm_check_res = prearm_check(&mavlink_log_pub, status_flags, battery, safety, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
bool prearm_check_res = prearm_check(&mavlink_log_pub, status_flags, safety, arm_requirements);
|
||||
PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED");
|
||||
|
||||
return 0;
|
||||
|
|
@ -522,7 +520,6 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
|
|||
// Transition the armed state. By passing mavlink_log_pub to arming_state_transition it will
|
||||
// output appropriate error messages if the state cannot transition.
|
||||
arming_res = arming_state_transition(&status,
|
||||
battery,
|
||||
safety,
|
||||
arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
&armed,
|
||||
|
|
@ -545,6 +542,7 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
|
|||
Commander::Commander() :
|
||||
ModuleParams(nullptr)
|
||||
{
|
||||
_battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
}
|
||||
|
||||
bool
|
||||
|
|
@ -1140,7 +1138,6 @@ Commander::run()
|
|||
param_t _param_min_stick_change = param_find("COM_RC_STICK_OV");
|
||||
param_t _param_geofence_action = param_find("GF_ACTION");
|
||||
param_t _param_disarm_land = param_find("COM_DISARM_LAND");
|
||||
param_t _param_low_bat_act = param_find("COM_LOW_BAT_ACT");
|
||||
param_t _param_offboard_loss_timeout = param_find("COM_OF_LOSS_T");
|
||||
param_t _param_arm_without_gps = param_find("COM_ARM_WO_GPS");
|
||||
param_t _param_arm_switch_is_button = param_find("COM_ARM_SWISBTN");
|
||||
|
|
@ -1263,11 +1260,6 @@ Commander::run()
|
|||
int stick_off_counter = 0;
|
||||
int stick_on_counter = 0;
|
||||
|
||||
bool low_battery_voltage_actions_done = false;
|
||||
bool critical_battery_voltage_actions_done = false;
|
||||
bool emergency_battery_voltage_actions_done = false;
|
||||
bool dangerous_battery_level_requests_poweroff = false;
|
||||
|
||||
bool status_changed = true;
|
||||
bool param_init_forced = true;
|
||||
|
||||
|
|
@ -1302,10 +1294,6 @@ Commander::run()
|
|||
/* Subscribe to parameters changed topic */
|
||||
int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
||||
/* Subscribe to battery topic */
|
||||
int battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
memset(&battery, 0, sizeof(battery));
|
||||
|
||||
/* Subscribe to subsystem info topic */
|
||||
int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
|
||||
struct subsystem_info_s info;
|
||||
|
|
@ -1326,7 +1314,7 @@ Commander::run()
|
|||
int cpuload_sub = orb_subscribe(ORB_ID(cpuload));
|
||||
memset(&cpuload, 0, sizeof(cpuload));
|
||||
|
||||
control_status_leds(&status, &armed, true, &battery, &cpuload);
|
||||
control_status_leds(&status, &armed, true, _battery_warning, &cpuload);
|
||||
|
||||
thread_running = true;
|
||||
|
||||
|
|
@ -1395,7 +1383,6 @@ Commander::run()
|
|||
uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */
|
||||
|
||||
int32_t disarm_when_landed = 0;
|
||||
int32_t low_bat_action = 0;
|
||||
|
||||
/* check which state machines for changes, clear "changed" flag */
|
||||
bool main_state_changed = false;
|
||||
|
|
@ -1504,7 +1491,6 @@ Commander::run()
|
|||
auto_disarm_hysteresis.set_hysteresis_time_from(false, disarm_when_landed * 1_s);
|
||||
}
|
||||
|
||||
param_get(_param_low_bat_act, &low_bat_action);
|
||||
param_get(_param_offboard_loss_timeout, &offboard_loss_timeout);
|
||||
param_get(_param_offboard_loss_act, &offboard_loss_act);
|
||||
param_get(_param_offboard_loss_rc_act, &offboard_loss_rc_act);
|
||||
|
|
@ -1632,7 +1618,7 @@ Commander::run()
|
|||
if (armed.armed && (status.hil_state == vehicle_status_s::HIL_STATE_OFF)
|
||||
&& safety.safety_switch_available && !safety.safety_off) {
|
||||
|
||||
if (TRANSITION_CHANGED == arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
if (TRANSITION_CHANGED == arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
&armed, true /* fRunPreArmChecks */, &mavlink_log_pub,
|
||||
&status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))
|
||||
) {
|
||||
|
|
@ -1771,96 +1757,7 @@ Commander::run()
|
|||
orb_copy(ORB_ID(cpuload), cpuload_sub, &cpuload);
|
||||
}
|
||||
|
||||
/* update battery status */
|
||||
orb_check(battery_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
|
||||
|
||||
/* only consider battery voltage if system has been running 6s (usb most likely detected) and battery voltage is valid */
|
||||
if ((hrt_elapsed_time(&commander_boot_timestamp) > 6_s)
|
||||
&& battery.voltage_filtered_v > 2.0f * FLT_EPSILON) {
|
||||
|
||||
/* if battery voltage is getting lower, warn using buzzer, etc. */
|
||||
if (battery.warning == battery_status_s::BATTERY_WARNING_LOW &&
|
||||
!low_battery_voltage_actions_done) {
|
||||
|
||||
low_battery_voltage_actions_done = true;
|
||||
|
||||
if (armed.armed) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, RETURN TO LAND ADVISED");
|
||||
|
||||
} else {
|
||||
mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, TAKEOFF DISCOURAGED");
|
||||
}
|
||||
|
||||
status_changed = true;
|
||||
|
||||
} else if (battery.warning == battery_status_s::BATTERY_WARNING_CRITICAL &&
|
||||
!critical_battery_voltage_actions_done) {
|
||||
|
||||
critical_battery_voltage_actions_done = true;
|
||||
|
||||
if (!armed.armed) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "CRITICAL BATTERY, SHUT SYSTEM DOWN");
|
||||
|
||||
} else {
|
||||
if (low_bat_action == 1 || low_bat_action == 3) {
|
||||
// let us send the critical message even if already in RTL
|
||||
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &internal_state)) {
|
||||
warning_action_on = true;
|
||||
mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, RETURNING TO LAND");
|
||||
|
||||
} else {
|
||||
mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, RTL FAILED");
|
||||
}
|
||||
|
||||
} else if (low_bat_action == 2) {
|
||||
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, &internal_state)) {
|
||||
warning_action_on = true;
|
||||
mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LANDING AT CURRENT POSITION");
|
||||
|
||||
} else {
|
||||
mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LANDING FAILED");
|
||||
}
|
||||
|
||||
} else {
|
||||
mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, RETURN TO LAUNCH ADVISED!");
|
||||
}
|
||||
}
|
||||
|
||||
status_changed = true;
|
||||
|
||||
} else if (battery.warning == battery_status_s::BATTERY_WARNING_EMERGENCY &&
|
||||
!emergency_battery_voltage_actions_done) {
|
||||
|
||||
emergency_battery_voltage_actions_done = true;
|
||||
|
||||
if (!armed.armed) {
|
||||
// Request shutdown at the end of the cycle. This allows
|
||||
// the vehicle state to be published after emergency landing
|
||||
dangerous_battery_level_requests_poweroff = true;
|
||||
} else {
|
||||
if (low_bat_action == 2 || low_bat_action == 3) {
|
||||
if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, &internal_state)) {
|
||||
warning_action_on = true;
|
||||
mavlink_log_emergency(&mavlink_log_pub, "DANGEROUS BATTERY LEVEL, LANDING IMMEDIATELY");
|
||||
|
||||
} else {
|
||||
mavlink_log_emergency(&mavlink_log_pub, "DANGEROUS BATTERY LEVEL, LANDING FAILED");
|
||||
}
|
||||
|
||||
} else {
|
||||
mavlink_log_emergency(&mavlink_log_pub, "DANGEROUS BATTERY LEVEL, LANDING ADVISED!");
|
||||
}
|
||||
}
|
||||
|
||||
status_changed = true;
|
||||
}
|
||||
|
||||
/* End battery voltage check */
|
||||
}
|
||||
}
|
||||
battery_status_check();
|
||||
|
||||
/* update subsystem info which arrives from outside of commander*/
|
||||
do {
|
||||
|
|
@ -1875,7 +1772,7 @@ Commander::run()
|
|||
/* 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, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
|
||||
arming_ret = arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
|
||||
true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags,
|
||||
arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
|
||||
|
|
@ -2000,7 +1897,7 @@ Commander::run()
|
|||
|
||||
// revert geofence failsafe transition if sticks are moved and we were previously in a manual mode
|
||||
// but only if not in a low battery handling action
|
||||
if (rc_override != 0 && !critical_battery_voltage_actions_done && (warning_action_on &&
|
||||
if (rc_override != 0 && (_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL) && (warning_action_on &&
|
||||
(main_state_before_rtl == commander_state_s::MAIN_STATE_MANUAL ||
|
||||
main_state_before_rtl == commander_state_s::MAIN_STATE_ALTCTL ||
|
||||
main_state_before_rtl == commander_state_s::MAIN_STATE_POSCTL ||
|
||||
|
|
@ -2023,7 +1920,7 @@ Commander::run()
|
|||
|
||||
// abort landing or auto or loiter if sticks are moved significantly
|
||||
// but only if not in a low battery handling action
|
||||
if (rc_override != 0 && !critical_battery_voltage_actions_done &&
|
||||
if (rc_override != 0 && (_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL) &&
|
||||
(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
|
||||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION ||
|
||||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER)) {
|
||||
|
|
@ -2109,7 +2006,7 @@ Commander::run()
|
|||
print_reject_arm("NOT DISARMING: Not in manual mode or landed yet.");
|
||||
|
||||
} else if ((stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition) {
|
||||
arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
|
||||
arming_ret = arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
|
||||
true /* fRunPreArmChecks */,
|
||||
&mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
}
|
||||
|
|
@ -2159,7 +2056,7 @@ Commander::run()
|
|||
print_reject_arm("NOT ARMING: Geofence RTL requires valid home");
|
||||
|
||||
} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
|
||||
arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_ARMED, &armed,
|
||||
arming_ret = arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_ARMED, &armed,
|
||||
!in_arming_grace_period /* fRunPreArmChecks */,
|
||||
&mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
|
||||
|
|
@ -2253,7 +2150,7 @@ Commander::run()
|
|||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
|
||||
|
||||
const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
|
||||
const float current2throttle = battery.current_a / throttle;
|
||||
const float current2throttle = _battery_current / throttle;
|
||||
|
||||
if (((throttle > ef_throttle_thres) && (current2throttle < ef_current2throttle_thres))
|
||||
|| status.engine_failure) {
|
||||
|
|
@ -2536,12 +2433,12 @@ Commander::run()
|
|||
|
||||
} else if (!status_flags.usb_connected &&
|
||||
(status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
|
||||
(battery.warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
|
||||
(_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
|
||||
/* play tune on battery critical */
|
||||
set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
|
||||
|
||||
} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
|
||||
(battery.warning == battery_status_s::BATTERY_WARNING_LOW)) {
|
||||
(_battery_warning == battery_status_s::BATTERY_WARNING_LOW)) {
|
||||
/* play tune on battery warning */
|
||||
set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
|
||||
|
||||
|
|
@ -2568,6 +2465,7 @@ Commander::run()
|
|||
|
||||
if (!sensor_fail_tune_played && (!status_flags.condition_system_sensors_initialized
|
||||
&& status_flags.condition_system_hotplug_timeout)) {
|
||||
|
||||
set_tune_override(TONE_GPS_WARNING_TUNE);
|
||||
sensor_fail_tune_played = true;
|
||||
status_changed = true;
|
||||
|
|
@ -2581,12 +2479,12 @@ Commander::run()
|
|||
/* 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, &cpuload);
|
||||
control_status_leds(&status, &armed, true, _battery_warning, &cpuload);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* normal state */
|
||||
control_status_leds(&status, &armed, status_changed, &battery, &cpuload);
|
||||
control_status_leds(&status, &armed, status_changed, _battery_warning, &cpuload);
|
||||
}
|
||||
|
||||
status_changed = false;
|
||||
|
|
@ -2598,21 +2496,6 @@ Commander::run()
|
|||
|
||||
arm_auth_update(now, params_updated || param_init_forced);
|
||||
|
||||
// Handle shutdown request from emergency battery action
|
||||
if(!armed.armed && dangerous_battery_level_requests_poweroff){
|
||||
mavlink_log_critical(&mavlink_log_pub, "DANGEROUSLY LOW BATTERY, SHUT SYSTEM DOWN");
|
||||
usleep(200000);
|
||||
int ret_val = px4_shutdown_request(false, false);
|
||||
|
||||
if (ret_val) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "SYSTEM DOES NOT SUPPORT SHUTDOWN");
|
||||
dangerous_battery_level_requests_poweroff = false;
|
||||
|
||||
} else {
|
||||
while (1) { usleep(1); }
|
||||
}
|
||||
}
|
||||
|
||||
usleep(COMMANDER_MONITORING_INTERVAL);
|
||||
}
|
||||
|
||||
|
|
@ -2636,7 +2519,6 @@ Commander::run()
|
|||
px4_close(cmd_sub);
|
||||
px4_close(subsys_sub);
|
||||
px4_close(param_changed_sub);
|
||||
px4_close(battery_sub);
|
||||
px4_close(land_detector_sub);
|
||||
|
||||
thread_running = false;
|
||||
|
|
@ -2668,7 +2550,7 @@ Commander::check_valid(const hrt_abstime ×tamp, const hrt_abstime &timeout,
|
|||
|
||||
void
|
||||
control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed,
|
||||
bool changed, battery_status_s *battery_local, const cpuload_s *cpuload_local)
|
||||
bool changed, const uint8_t battery_warning, const cpuload_s *cpuload_local)
|
||||
{
|
||||
static hrt_abstime overload_start = 0;
|
||||
|
||||
|
|
@ -2724,10 +2606,10 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
|
|||
if (status.failsafe) {
|
||||
led_color = led_control_s::COLOR_PURPLE;
|
||||
|
||||
} else if (battery_local->warning == battery_status_s::BATTERY_WARNING_LOW) {
|
||||
} else if (battery_warning == battery_status_s::BATTERY_WARNING_LOW) {
|
||||
led_color = led_control_s::COLOR_AMBER;
|
||||
|
||||
} else if (battery_local->warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
|
||||
} else if (battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
|
||||
led_color = led_control_s::COLOR_RED;
|
||||
|
||||
} else {
|
||||
|
|
@ -3640,7 +3522,7 @@ void *commander_low_prio_loop(void *arg)
|
|||
int calib_ret = PX4_ERROR;
|
||||
|
||||
/* try to go to INIT/PREFLIGHT arming state */
|
||||
if (TRANSITION_DENIED == arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
|
||||
if (TRANSITION_DENIED == arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
|
||||
false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags,
|
||||
arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))) {
|
||||
|
||||
|
|
@ -3728,7 +3610,7 @@ void *commander_low_prio_loop(void *arg)
|
|||
|
||||
Commander::preflight_check(false);
|
||||
|
||||
arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
|
||||
arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
|
||||
false /* fRunPreArmChecks */,
|
||||
&mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
|
||||
|
|
@ -4113,6 +3995,60 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32
|
|||
}
|
||||
}
|
||||
|
||||
void Commander::battery_status_check()
|
||||
{
|
||||
bool updated = false;
|
||||
|
||||
/* update battery status */
|
||||
orb_check(_battery_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
|
||||
battery_status_s battery = {};
|
||||
if (orb_copy(ORB_ID(battery_status), _battery_sub, &battery) == PX4_OK) {
|
||||
|
||||
if ((hrt_elapsed_time(&battery.timestamp) < 5_s)
|
||||
&& battery.connected
|
||||
&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE)) {
|
||||
|
||||
status_flags.condition_battery_healthy = true;
|
||||
|
||||
} else {
|
||||
status_flags.condition_battery_healthy = false;
|
||||
}
|
||||
|
||||
// execute battery failsafe if the state has gotten worse
|
||||
if (armed.armed) {
|
||||
if (battery.warning > _battery_warning) {
|
||||
battery_failsafe(&mavlink_log_pub, status, status_flags, &internal_state, battery.warning, (low_battery_action_t)_low_bat_action.get());
|
||||
}
|
||||
}
|
||||
|
||||
// Handle shutdown request from emergency battery action
|
||||
if (!armed.armed && (battery.warning != _battery_warning)) {
|
||||
|
||||
if (battery.warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "DANGEROUSLY LOW BATTERY, SHUT SYSTEM DOWN");
|
||||
usleep(200000);
|
||||
|
||||
int ret_val = px4_shutdown_request(false, false);
|
||||
|
||||
if (ret_val) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "SYSTEM DOES NOT SUPPORT SHUTDOWN");
|
||||
|
||||
} else {
|
||||
while (1) { usleep(1); }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// save last value
|
||||
_battery_warning = battery.warning;
|
||||
_battery_current = battery.current_filtered_a;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Commander::estimator_check(bool *status_changed)
|
||||
{
|
||||
// Check if quality checking of position accuracy and consistency is to be performed
|
||||
|
|
|
|||
|
|
@ -104,7 +104,10 @@ private:
|
|||
|
||||
(ParamInt<px4::params::COM_POS_FS_DELAY>) _failsafe_pos_delay,
|
||||
(ParamInt<px4::params::COM_POS_FS_PROB>) _failsafe_pos_probation,
|
||||
(ParamInt<px4::params::COM_POS_FS_GAIN>) _failsafe_pos_gain
|
||||
(ParamInt<px4::params::COM_POS_FS_GAIN>) _failsafe_pos_gain,
|
||||
|
||||
(ParamInt<px4::params::COM_LOW_BAT_ACT>) _low_bat_action
|
||||
|
||||
)
|
||||
|
||||
const int64_t POSVEL_PROBATION_MIN = 1_s; /**< minimum probation duration (usec) */
|
||||
|
|
@ -173,6 +176,12 @@ private:
|
|||
|
||||
void estimator_check(bool *status_changed);
|
||||
|
||||
int _battery_sub{-1};
|
||||
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
|
||||
float _battery_current{0.0f};
|
||||
|
||||
void battery_status_check();
|
||||
|
||||
// Subscriptions
|
||||
Subscription<estimator_status_s> _estimator_status_sub{ORB_ID(estimator_status)};
|
||||
Subscription<iridiumsbd_status_s> _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
|
||||
|
|
|
|||
|
|
@ -343,6 +343,7 @@ void rgbled_set_color_and_mode(uint8_t color, uint8_t mode, uint8_t blinks, uint
|
|||
orb_publish(ORB_ID(led_control), led_control_pub, &led_control);
|
||||
}
|
||||
|
||||
void rgbled_set_color_and_mode(uint8_t color, uint8_t mode){
|
||||
void rgbled_set_color_and_mode(uint8_t color, uint8_t mode)
|
||||
{
|
||||
rgbled_set_color_and_mode(color, mode, 0, 0);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -41,9 +41,6 @@
|
|||
* @author Julian Oes <julian@px4.io>
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <parameters/param.h>
|
||||
|
||||
/**
|
||||
* Roll trim
|
||||
*
|
||||
|
|
|
|||
|
|
@ -235,7 +235,6 @@ bool StateMachineHelperTest::armingStateTransitionTest()
|
|||
struct vehicle_status_flags_s status_flags = {};
|
||||
struct safety_s safety = {};
|
||||
struct actuator_armed_s armed = {};
|
||||
struct battery_status_s battery = {};
|
||||
|
||||
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
|
||||
for (size_t i=0; i<cArmingTransitionTests; i++) {
|
||||
|
|
@ -255,7 +254,7 @@ bool StateMachineHelperTest::armingStateTransitionTest()
|
|||
armed.ready_to_arm = test->current_state.ready_to_arm;
|
||||
|
||||
// Attempt transition
|
||||
transition_result_t result = arming_state_transition(&status, battery, safety, test->requested_state, &armed,
|
||||
transition_result_t result = arming_state_transition(&status, safety, test->requested_state, &armed,
|
||||
false /* no pre-arm checks */,
|
||||
nullptr /* no mavlink_log_pub */,
|
||||
&status_flags,
|
||||
|
|
|
|||
|
|
@ -92,8 +92,8 @@ void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
|
|||
|
||||
void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, const link_loss_actions_t link_loss_act);
|
||||
|
||||
transition_result_t arming_state_transition(vehicle_status_s *status, const battery_status_s &battery,
|
||||
const safety_s &safety, const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks,
|
||||
transition_result_t arming_state_transition(vehicle_status_s *status, const safety_s &safety,
|
||||
const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks,
|
||||
orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags, const uint8_t arm_requirements,
|
||||
const hrt_abstime &time_since_boot)
|
||||
{
|
||||
|
|
@ -163,7 +163,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
|
|||
|
||||
if (preflight_check_ret) {
|
||||
// only bother running prearm if preflight was successful
|
||||
prearm_check_ret = prearm_check(mavlink_log_pub, *status_flags, battery, safety, arm_requirements, time_since_boot);
|
||||
prearm_check_ret = prearm_check(mavlink_log_pub, *status_flags, safety, arm_requirements);
|
||||
}
|
||||
|
||||
if (!(preflight_check_ret && prearm_check_ret)) {
|
||||
|
|
@ -917,9 +917,8 @@ void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, c
|
|||
}
|
||||
}
|
||||
|
||||
bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
|
||||
const battery_status_s &battery, const safety_s &safety, const uint8_t arm_requirements,
|
||||
const hrt_abstime &time_since_boot)
|
||||
bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const safety_s &safety,
|
||||
const uint8_t arm_requirements)
|
||||
{
|
||||
bool reportFailures = true;
|
||||
bool prearm_ok = true;
|
||||
|
|
@ -946,9 +945,9 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s
|
|||
}
|
||||
|
||||
// main battery level
|
||||
if (battery.warning >= battery_status_s::BATTERY_WARNING_LOW) {
|
||||
if (!status_flags.condition_battery_healthy) {
|
||||
if (prearm_ok && reportFailures) {
|
||||
mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: LOW BATTERY");
|
||||
mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: CHECK BATTERY");
|
||||
}
|
||||
|
||||
prearm_ok = false;
|
||||
|
|
@ -1008,3 +1007,88 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s
|
|||
|
||||
return prearm_ok;
|
||||
}
|
||||
|
||||
|
||||
void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
|
||||
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning,
|
||||
const low_battery_action_t low_battery_action)
|
||||
{
|
||||
switch (battery_warning) {
|
||||
case battery_status_s::BATTERY_WARNING_NONE:
|
||||
break;
|
||||
|
||||
case battery_status_s::BATTERY_WARNING_LOW:
|
||||
mavlink_log_critical(mavlink_log_pub, "LOW BATTERY, RETURN ADVISED");
|
||||
break;
|
||||
|
||||
case battery_status_s::BATTERY_WARNING_CRITICAL:
|
||||
|
||||
switch (low_battery_action) {
|
||||
case LOW_BAT_ACTION::WARNING:
|
||||
mavlink_log_critical(mavlink_log_pub, "CRITICAL BATTERY, RETURN ADVISED!");
|
||||
break;
|
||||
|
||||
case LOW_BAT_ACTION::RETURN:
|
||||
// FALLTHROUGH
|
||||
case LOW_BAT_ACTION::RETURN_OR_LAND:
|
||||
|
||||
// let us send the critical message even if already in RTL
|
||||
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state)) {
|
||||
mavlink_log_critical(mavlink_log_pub, "CRITICAL BATTERY, RETURNING");
|
||||
|
||||
} else {
|
||||
mavlink_log_emergency(mavlink_log_pub, "CRITICAL BATTERY, RETURN FAILED");
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case LOW_BAT_ACTION::LAND:
|
||||
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, internal_state)) {
|
||||
mavlink_log_critical(mavlink_log_pub, "CRITICAL BATTERY, LANDING AT CURRENT POSITION");
|
||||
|
||||
} else {
|
||||
mavlink_log_emergency(mavlink_log_pub, "CRITICAL BATTERY, LANDING FAILED");
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case battery_status_s::BATTERY_WARNING_EMERGENCY:
|
||||
|
||||
switch (low_battery_action) {
|
||||
case LOW_BAT_ACTION::WARNING:
|
||||
mavlink_log_emergency(mavlink_log_pub, "DANGEROUS BATTERY LEVEL, LANDING ADVISED!");
|
||||
break;
|
||||
|
||||
case LOW_BAT_ACTION::RETURN:
|
||||
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state)) {
|
||||
mavlink_log_emergency(mavlink_log_pub, "DANGEROUS BATTERY LEVEL, RETURNING");
|
||||
|
||||
} else {
|
||||
mavlink_log_emergency(mavlink_log_pub, "DANGEROUS BATTERY LEVEL, RETURN FAILED");
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case LOW_BAT_ACTION::RETURN_OR_LAND:
|
||||
// FALLTHROUGH
|
||||
case LOW_BAT_ACTION::LAND:
|
||||
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, internal_state)) {
|
||||
mavlink_log_emergency(mavlink_log_pub, "DANGEROUS BATTERY LEVEL, LANDING IMMEDIATELY");
|
||||
|
||||
} else {
|
||||
mavlink_log_emergency(mavlink_log_pub, "DANGEROUS BATTERY LEVEL, LANDING FAILED");
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case battery_status_s::BATTERY_WARNING_FAILED:
|
||||
mavlink_log_emergency(mavlink_log_pub, "BATTERY FAILED");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -79,10 +79,10 @@ extern const char *const arming_state_names[];
|
|||
|
||||
bool is_safe(const safety_s &safety, const actuator_armed_s &armed);
|
||||
|
||||
transition_result_t arming_state_transition(vehicle_status_s *status, const battery_status_s &battery,
|
||||
const safety_s &safety, const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks,
|
||||
orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags,
|
||||
const uint8_t arm_requirements, const hrt_abstime &time_since_boot);
|
||||
transition_result_t
|
||||
arming_state_transition(vehicle_status_s *status, const safety_s &safety, const arming_state_t new_arming_state,
|
||||
actuator_armed_s *armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
|
||||
vehicle_status_flags_s *status_flags, const uint8_t arm_requirements, const hrt_abstime &time_since_boot);
|
||||
|
||||
transition_result_t
|
||||
main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,
|
||||
|
|
@ -106,8 +106,20 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
|
|||
bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub,
|
||||
const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos);
|
||||
|
||||
bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
|
||||
const battery_status_s &battery, const safety_s &safety, const uint8_t arm_requirements,
|
||||
const hrt_abstime &time_since_boot);
|
||||
bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const safety_s &safety,
|
||||
const uint8_t arm_requirements);
|
||||
|
||||
|
||||
// COM_LOW_BAT_ACT parameter values
|
||||
typedef enum LOW_BAT_ACTION {
|
||||
WARNING = 0, // Warning
|
||||
RETURN = 1, // Return mode
|
||||
LAND = 2, // Land mode
|
||||
RETURN_OR_LAND = 3 // Return mode at critically low level, Land mode at current position if reaching dangerously low levels
|
||||
} low_battery_action_t;
|
||||
|
||||
void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
|
||||
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning,
|
||||
const low_battery_action_t low_bat_action);
|
||||
|
||||
#endif /* STATE_MACHINE_HELPER_H_ */
|
||||
|
|
|
|||
Loading…
Reference in New Issue