diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-26 17:39:00 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-26 17:39:00 +0200 |
commit | b07964660e8b959cad410064b1f74f839b2c268f (patch) | |
tree | 4a212c294e541e9196abb190fcd8da01be770119 | |
parent | 362672ece8210fe64537e434d17b3917a3a7e29a (diff) | |
download | px4-firmware-b07964660e8b959cad410064b1f74f839b2c268f.tar.gz px4-firmware-b07964660e8b959cad410064b1f74f839b2c268f.tar.bz2 px4-firmware-b07964660e8b959cad410064b1f74f839b2c268f.zip |
commander: prune old code, do not run preflight checks when nothing relevant in the system is changing.
-rw-r--r-- | src/modules/commander/commander.cpp | 36 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 4 |
2 files changed, 7 insertions, 33 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8cce01778..54de05192 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -182,21 +182,6 @@ static struct safety_s safety; static struct vehicle_control_mode_s 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 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 { - //Refuse to arm if preflight checks have failed - if(!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) { + // Refuse to arm if preflight checks have failed + 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."); cmd_result = VEHICLE_CMD_RESULT_DENIED; break; @@ -1616,8 +1601,7 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* 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) { - /* TODO: check for sensors */ + if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); @@ -1625,8 +1609,6 @@ int commander_thread_main(int argc, char *argv[]) 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 */ 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); break; } @@ -2755,16 +2737,6 @@ void *commander_low_prio_loop(void *arg) 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) { int ret = param_load_default(); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index bd31be7ac..844e8f2ab 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -213,7 +213,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Sensors need to be initialized for STANDBY state 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; valid_transition = false; 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) { mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete"); feedback_provided = true; + } else { + } } |