fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 23:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: pass battery status to preflight check

Browse Source 
A low battery check was recently added to the preflight check, therefore
we need to pass on the information about the battery.
This commit is contained in:
Julian Oes
2016-04-11 18:01:49 +02:00
parent 68a69c9d46
commit 974223bdd1
3 changed files with 25 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+14 -11
View File
@@ -175,12 +175,6 @@ enum MAV_MODE_FLAG {
};
enum class vehicle_battery_warning {
NONE = 0, // no battery low voltage warning active
LOW = 1, // warning of low voltage
CRITICAL = 2, // alerting of critical voltage
};
/* Mavlink log uORB handle */
static orb_advert_t mavlink_log_pub = 0;
@@ -205,6 +199,7 @@ static float eph_threshold = 5.0f;
static float epv_threshold = 10.0f;
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;
@@ -407,9 +402,9 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "check")) {
int checkres = 0;
checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags);
checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery);
warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags);
checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery);
warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
return 0;
}
@@ -635,7 +630,9 @@ 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, &safety,
arming_res = arming_state_transition(&status,
&battery,
&safety,
arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY,
&armed,
true /* fRunPreArmChecks */,
@@ -1432,7 +1429,6 @@ int commander_thread_main(int argc, char *argv[])
/* Subscribe to battery topic */
int battery_sub = orb_subscribe(ORB_ID(battery_status));
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
/* Subscribe to subsystem info topic */
@@ -1786,6 +1782,7 @@ int commander_thread_main(int argc, char *argv[])
vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status,
&battery,
&safety,
new_arming_state,
&armed,
@@ -2044,6 +2041,7 @@ int commander_thread_main(int argc, char *argv[])
/* 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,
@@ -2300,7 +2298,9 @@ int commander_thread_main(int argc, char *argv[])
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety,
arming_ret = arming_state_transition(&status,
&battery,
&safety,
new_arming_state,
&armed,
true /* fRunPreArmChecks */,
@@ -2343,6 +2343,7 @@ int commander_thread_main(int argc, char *argv[])
} 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,
@@ -3546,6 +3547,7 @@ void *commander_low_prio_loop(void *arg)
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status,
&battery,
&safety,
vehicle_status_s::ARMING_STATE_INIT,
&armed,
@@ -3639,6 +3641,7 @@ void *commander_low_prio_loop(void *arg)
!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !status_flags.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
arming_state_transition(&status,
&battery,
&safety,
vehicle_status_s::ARMING_STATE_STANDBY,
&armed,
src/modules/commander/state_machine_helper.cpp
+7 -21
View File
@@ -110,6 +110,7 @@ static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked
static int last_prearm_ret = 1; ///< initialize to fail
transition_result_t arming_state_transition(struct vehicle_status_s *status,
struct battery_status_s *battery,
const struct safety_s *safety,
arming_state_t new_arming_state,
struct actuator_armed_s *armed,
@@ -142,7 +143,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */,
status_flags);
status_flags, battery);
}
/* re-run the pre-flight check as long as sensors are failing */
if (!status_flags->condition_system_sensors_initialized
@@ -152,7 +153,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */,
status_flags);
status_flags, battery);
status_flags->condition_system_sensors_initialized = !prearm_ret;
last_preflight_check = hrt_absolute_time();
last_prearm_ret = prearm_ret;
@@ -255,26 +256,16 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
<<<<<<< 4ed69db0abb4b3b2f191ba77949e9c6817eaf169
if (status->condition_system_sensors_initialized) {
mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot before arming");
=======
if (status_flags->condition_system_sensors_initialized) {
// mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot before arming");
>>>>>>> commander: internalize system status bools
mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot before arming");
} else {
mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check failed, refusing to arm");
}
feedback_provided = true;
} else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
<<<<<<< 4ed69db0abb4b3b2f191ba77949e9c6817eaf169
status->condition_system_sensors_initialized) {
mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot to complete");
=======
status_flags->condition_system_sensors_initialized) {
// mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot to complete");
>>>>>>> commander: internalize system status bools
mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot to complete");
feedback_provided = true;
} else {
// Silent ignore
@@ -289,13 +280,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
if ((!status_flags->condition_system_prearm_error_reported &&
status_flags->condition_system_hotplug_timeout) ||
(new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
<<<<<<< 4ed69db0abb4b3b2f191ba77949e9c6817eaf169
mavlink_and_console_log_critical(mavlink_log_pub, "Not ready to fly: Sensors not set up correctly");
status->condition_system_prearm_error_reported = true;
=======
mavlink_and_console_log_critical(mavlink_log_pub, "Not ready to fly: Sensors not initialized");
status_flags->condition_system_prearm_error_reported = true;
>>>>>>> commander: internalize system status bools
}
feedback_provided = true;
valid_transition = false;
@@ -879,7 +865,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
return status->nav_state != nav_state_old;
}
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags)
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery)
{
/*
*/
@@ -904,7 +890,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
}
}
if (status->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
if (battery->warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
preflight_ok = false;
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_log_pub, "ARMING DENIED: VERY LOW BATTERY");
src/modules/commander/state_machine_helper.h
+4 -3
View File
@@ -44,6 +44,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/commander_state.h>
@@ -94,11 +95,12 @@ struct status_flags_s {
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
transition_result_t arming_state_transition(struct vehicle_status_s *status,
struct battery_status_s *battery,
const struct safety_s *safety,
arming_state_t new_arming_state,
struct actuator_armed_s *armed,
bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub,
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
status_flags_s *status_flags,
float avionics_power_rail_voltage);
@@ -112,7 +114,6 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
const bool data_link_loss_enabled, const bool mission_finished,
const bool stay_in_failsafe, status_flags_s *status_flags, bool landed);
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm,
status_flags_s *status_flags);
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, status_flags_s *status_flags, battery_status_s *battery);
#endif /* STATE_MACHINE_HELPER_H_ */