diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 430f7dd768..c5d5ee9a16 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -91,6 +91,7 @@ uint8 nav_state # set navigation state machine to specified value uint8 arming_state # current arming state uint8 hil_state # current hil state bool failsafe # true if system is in failsafe state +bool calibration_enabled # true if current calibrating parts of the system. Also sets the system to ARMING_STATE_INIT. int32 system_type # system type, inspired by MAVLink's VEHICLE_TYPE enum int32 system_id # system id, inspired by MAVLink's system ID field diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 213de16562..87d9469a1d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -559,7 +559,7 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref int do_level_calibration(int mavlink_fd) { const unsigned cal_time = 5; - const unsigned cal_hz = 250; + const unsigned cal_hz = 100; const unsigned settle_time = 30; int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); struct vehicle_attitude_s att; @@ -614,6 +614,8 @@ int do_level_calibration(int mavlink_fd) { pitch_mean /= counter; } else { mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken"); + success = false; + goto out; } if (fabsf(roll_mean) > 0.8f ) { @@ -628,6 +630,7 @@ int do_level_calibration(int mavlink_fd) { success = true; } +out: if (success) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level"); return 0; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 298cd0289b..ab42cef7a9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1611,7 +1611,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) { + if (!status.calibration_enabled && 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); @@ -2698,6 +2698,8 @@ void *commander_low_prio_loop(void *arg) false /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; + } else { + status.calibration_enabled = true; } if ((int)(cmd.param1) == 1) { @@ -2757,12 +2759,14 @@ void *commander_low_prio_loop(void *arg) /* enable RC control input */ status.rc_input_blocked = false; mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); - calib_ret = OK; + calib_ret = OK; } /* this always succeeds */ calib_ret = OK; } + status.calibration_enabled = false; + if (calib_ret == OK) { tune_positive(true); @@ -2780,7 +2784,7 @@ void *commander_low_prio_loop(void *arg) status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check); - arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); } else { tune_negative(true); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index fc05cce6e9..5d0265d31d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -219,23 +219,13 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s valid_transition = true; } - // Sensors need to be initialized for STANDBY state, except for HIL - if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && - (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 need inspection"); - feedback_provided = true; - valid_transition = false; - status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; - } - // Check if we are trying to arm, checks look good but we are in STANDBY_ERROR if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { if (status->condition_system_sensors_initialized) { - mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); + mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot before arming"); } else { mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); } @@ -243,11 +233,24 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s } else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && 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, reboot to complete"); feedback_provided = true; } else { - + // Silent ignore + feedback_provided = true; } + + // Sensors need to be initialized for STANDBY state, except for HIL + } else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && + (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && + (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && + (!status->condition_system_sensors_initialized)) { + if (!fRunPreArmChecks) { + 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; } // Finish up the state transition