Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 02:36:37 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

commander: prune old code, do not run preflight checks when nothing relevant in the system is changing.

Browse Source
This commit is contained in:
Lorenz Meier
2015-04-26 17:39:00 +02:00
parent 362672ece8
commit b07964660e
2 changed files with 7 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+4 -32
View File
@@ -182,21 +182,6 @@ static struct safety_s safety;
static struct vehicle_control_mode_s control_mode; static struct vehicle_control_mode_s control_mode;
static struct offboard_control_mode_s offboard_control_mode; static struct offboard_control_mode_s offboard_control_mode;
/* tasks waiting for low prio thread */
typedef enum {
LOW_PRIO_TASK_NONE = 0,
LOW_PRIO_TASK_PARAM_SAVE,
LOW_PRIO_TASK_PARAM_LOAD,
LOW_PRIO_TASK_GYRO_CALIBRATION,
LOW_PRIO_TASK_MAG_CALIBRATION,
LOW_PRIO_TASK_ALTITUDE_CALIBRATION,
LOW_PRIO_TASK_RC_CALIBRATION,
LOW_PRIO_TASK_ACCEL_CALIBRATION,
LOW_PRIO_TASK_AIRSPEED_CALIBRATION
} low_prio_task_t;
static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE;
/** /**
* The daemon app only briefly exists to start * The daemon app only briefly exists to start
* the background job. The stack size assigned in the * the background job. The stack size assigned in the
@@ -552,8 +537,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} }
else { else {
//Refuse to arm if preflight checks have failed // Refuse to arm if preflight checks have failed
if(!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) { if (!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed."); mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed.");
cmd_result = VEHICLE_CMD_RESULT_DENIED; cmd_result = VEHICLE_CMD_RESULT_DENIED;
break; break;
@@ -1616,8 +1601,7 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */ /* End battery voltage check */
/* If in INIT state, try to proceed to STANDBY state */ /* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
/* TODO: check for sensors */
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
mavlink_fd); mavlink_fd);
@@ -1625,8 +1609,6 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = true; arming_state_changed = true;
} }
} else {
/* TODO: Add emergency stuff if sensors are lost */
} }
@@ -2674,7 +2656,7 @@ void *commander_low_prio_loop(void *arg)
/* try to go to INIT/PREFLIGHT arming state */ /* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) { false /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break; break;
} }
@@ -2755,16 +2737,6 @@ void *commander_low_prio_loop(void *arg)
case VEHICLE_CMD_PREFLIGHT_STORAGE: { case VEHICLE_CMD_PREFLIGHT_STORAGE: {
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
checkAirspeed = true;
}
// Update preflight check status
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
if (((int)(cmd.param1)) == 0) { if (((int)(cmd.param1)) == 0) {
int ret = param_load_default(); int ret = param_load_default();
src/modules/commander/state_machine_helper.cpp
+3 -1
View File
@@ -213,7 +213,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Sensors need to be initialized for STANDBY state // Sensors need to be initialized for STANDBY state
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational."); mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
feedback_provided = true; feedback_provided = true;
valid_transition = false; valid_transition = false;
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
@@ -235,6 +235,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
status->condition_system_sensors_initialized) { status->condition_system_sensors_initialized) {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete"); mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete");
feedback_provided = true; feedback_provided = true;
} else {
} }
} }