zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

State machine fixes for HIL

sbg
Lorenz Meier 12 years ago
parent
90c4664dce
commit
d1871bd7bb
5 changed files with 56 additions and 32 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 50
      src/modules/commander/commander.cpp
  2. 29
      src/modules/commander/state_machine_helper.cpp
  3. 2
      src/modules/commander/state_machine_helper.h
  4. 4
      src/modules/mavlink/waypoints.c
  5. 3
      src/modules/sensors/sensors.cpp

50
src/modules/commander/commander.cpp
Unescape View File

@ -359,23 +359,35 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
case VEHICLE_CMD_DO_SET_MODE: { case VEHICLE_CMD_DO_SET_MODE: {
uint8_t base_mode = (uint8_t) cmd->param1; uint8_t base_mode = (uint8_t) cmd->param1;
uint8_t custom_main_mode = (uint8_t) cmd->param2; uint8_t custom_main_mode = (uint8_t) cmd->param2;
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
/* set HIL state */ /* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
/* if HIL got enabled, reset battery status state */
if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
/* reset the arming mode to disarmed */
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
if (arming_res != TRANSITION_DENIED) {
mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
} else {
mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
}
}
// TODO remove debug code // TODO remove debug code
//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
/* set arming state */ /* set arming state */
transition_result_t arming_res = TRANSITION_NOT_CHANGED; arming_res = TRANSITION_NOT_CHANGED;
if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
if (safety->safety_switch_available && !safety->safety_off) { if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) {
print_reject_arm("NOT ARMING: Press safety switch first."); print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED; arming_res = TRANSITION_DENIED;
} else { } else {
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
} }
if (arming_res == TRANSITION_CHANGED) { if (arming_res == TRANSITION_CHANGED) {
@ -385,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} else { } else {
if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_res = arming_state_transition(status, safety, new_arming_state, armed); arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed);
if (arming_res == TRANSITION_CHANGED) { if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
@ -478,7 +490,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_DENIED; arming_res = TRANSITION_DENIED;
} else { } else {
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
} }
if (arming_res == TRANSITION_CHANGED) { if (arming_res == TRANSITION_CHANGED) {
@ -532,6 +544,9 @@ static struct actuator_armed_s armed;
static struct safety_s safety; static struct safety_s safety;
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
int commander_thread_main(int argc, char *argv[]) int commander_thread_main(int argc, char *argv[])
{ {
/* not yet initialized */ /* not yet initialized */
@ -584,10 +599,6 @@ int commander_thread_main(int argc, char *argv[])
/* Initialize armed with all false */ /* Initialize armed with all false */
memset(&armed, 0, sizeof(armed)); memset(&armed, 0, sizeof(armed));
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
/* Initialize all flags to false */ /* Initialize all flags to false */
memset(&control_mode, 0, sizeof(control_mode)); memset(&control_mode, 0, sizeof(control_mode));
@ -876,8 +887,8 @@ int commander_thread_main(int argc, char *argv[])
// warnx("bat v: %2.2f", battery.voltage_v); // warnx("bat v: %2.2f", battery.voltage_v);
/* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ /* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) {
status.battery_voltage = battery.voltage_v; status.battery_voltage = battery.voltage_v;
status.condition_battery_voltage_valid = true; status.condition_battery_voltage_valid = true;
status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
@ -958,9 +969,9 @@ int commander_thread_main(int argc, char *argv[])
battery_tune_played = false; battery_tune_played = false;
if (armed.armed) { if (armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
} else { } else {
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
} }
status_changed = true; status_changed = true;
@ -969,6 +980,7 @@ int commander_thread_main(int argc, char *argv[])
critical_voltage_counter++; critical_voltage_counter++;
} else { } else {
low_voltage_counter = 0; low_voltage_counter = 0;
critical_voltage_counter = 0; critical_voltage_counter = 0;
} }
@ -978,7 +990,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */ /* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
// XXX check for sensors // XXX check for sensors
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
} else { } else {
// XXX: Add emergency stuff if sensors are lost // XXX: Add emergency stuff if sensors are lost
@ -1082,7 +1094,7 @@ int commander_thread_main(int argc, char *argv[])
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
res = arming_state_transition(&status, &safety, new_arming_state, &armed); res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed);
stick_off_counter = 0; stick_off_counter = 0;
} else { } else {
@ -1104,7 +1116,7 @@ int commander_thread_main(int argc, char *argv[])
print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else { } else {
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
} }
stick_on_counter = 0; stick_on_counter = 0;
@ -1752,7 +1764,7 @@ void *commander_low_prio_loop(void *arg)
/* try to go to INIT/PREFLIGHT arming state */ /* try to go to INIT/PREFLIGHT arming state */
// XXX disable interrupts in arming_state_transition // XXX disable interrupts in arming_state_transition
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break; break;
} }
@ -1792,7 +1804,7 @@ void *commander_low_prio_loop(void *arg)
else else
tune_negative(); tune_negative();
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
break; break;
} }

29
src/modules/commander/state_machine_helper.cpp
Unescape View File

@ -67,7 +67,9 @@ static bool main_state_changed = true;
static bool navigation_state_changed = true; static bool navigation_state_changed = true;
transition_result_t transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
const struct vehicle_control_mode_s *control_mode,
arming_state_t new_arming_state, struct actuator_armed_s *armed)
{ {
/* /*
* Perform an atomic state update * Perform an atomic state update
@ -82,6 +84,13 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
} else { } else {
/* enforce lockdown in HIL */
if (control_mode->flag_system_hil_enabled) {
armed->lockdown = true;
} else {
armed->lockdown = false;
}
switch (new_arming_state) { switch (new_arming_state) {
case ARMING_STATE_INIT: case ARMING_STATE_INIT:
@ -98,7 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* allow coming from INIT and disarming from ARMED */ /* allow coming from INIT and disarming from ARMED */
if (status->arming_state == ARMING_STATE_INIT if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED) { || status->arming_state == ARMING_STATE_ARMED
|| control_mode->flag_system_hil_enabled) {
/* sensors need to be initialized for STANDBY state */ /* sensors need to be initialized for STANDBY state */
if (status->condition_system_sensors_initialized) { if (status->condition_system_sensors_initialized) {
@ -115,7 +125,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* allow arming from STANDBY and IN-AIR-RESTORE */ /* allow arming from STANDBY and IN-AIR-RESTORE */
if ((status->arming_state == ARMING_STATE_STANDBY if ((status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE) || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
&& (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */ && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
armed->armed = true; armed->armed = true;
armed->ready_to_arm = true; armed->ready_to_arm = true;
@ -466,20 +476,17 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
case HIL_STATE_OFF: case HIL_STATE_OFF:
if (current_status->arming_state == ARMING_STATE_INIT /* we're in HIL and unexpected things can happen if we disable HIL now */
|| current_status->arming_state == ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
valid_transition = false;
current_control_mode->flag_system_hil_enabled = false;
mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
valid_transition = true;
}
break; break;
case HIL_STATE_ON: case HIL_STATE_ON:
if (current_status->arming_state == ARMING_STATE_INIT if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) { || current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
current_control_mode->flag_system_hil_enabled = true; current_control_mode->flag_system_hil_enabled = true;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); mavlink_log_critical(mavlink_fd, "Switched to ON hil state");

2
src/modules/commander/state_machine_helper.h
Unescape View File

@ -58,7 +58,7 @@ typedef enum {
} transition_result_t; } transition_result_t;
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed); const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);

4
src/modules/mavlink/waypoints.c
Unescape View File

@ -307,7 +307,9 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
{ {
static uint16_t counter; static uint16_t counter;
if (!global_pos->valid && !local_pos->xy_valid) { if ((!global_pos->valid && !local_pos->xy_valid) ||
/* no waypoint */
wpm->size == 0) {
/* nothing to check here, return */ /* nothing to check here, return */
return; return;
} }

3
src/modules/sensors/sensors.cpp
Unescape View File

@ -1224,6 +1224,9 @@ Sensors::parameter_update_poll(bool forced)
void void
Sensors::adc_poll(struct sensor_combined_s &raw) Sensors::adc_poll(struct sensor_combined_s &raw)
{ {
/* only read if publishing */
if (!_publishing)
return;
/* rate limit to 100 Hz */ /* rate limit to 100 Hz */
if (hrt_absolute_time() - _last_adc >= 10000) { if (hrt_absolute_time() - _last_adc >= 10000) {

Write Preview
Loading…
Cancel
Save