aboutsummaryrefslogtreecommitdiff
path: root/src/modules/commander/commander.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/commander/commander.cpp')
-rw-r--r--src/modules/commander/commander.cpp381
1 files changed, 212 insertions, 169 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 1e5318121..f579fb52a 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -106,14 +106,9 @@ static const int ERROR = -1;
extern struct system_load_s system_load;
-#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
-#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
-
/* Decouple update interval and hysteris counters, all depends on intervals */
#define COMMANDER_MONITORING_INTERVAL 50000
#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
-#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define MAVLINK_OPEN_INTERVAL 50000
@@ -204,9 +199,9 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
-void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
+void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
-transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
+transition_result_t set_main_state_rc(struct vehicle_status_s *status);
void print_reject_mode(const char *msg);
@@ -376,6 +371,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
}
}
+
if (hil_ret == OK)
ret = true;
@@ -410,6 +406,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_NOT_CHANGED;
}
}
+
if (arming_res == TRANSITION_CHANGED)
ret = true;
@@ -452,6 +449,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
}
+
if (main_res == TRANSITION_CHANGED)
ret = true;
@@ -491,8 +489,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break;
case VEHICLE_CMD_OVERRIDE_GOTO: {
- // TODO listen vehicle_command topic directly from navigator (?)
+ // TODO listen vehicle_command topic directly from navigator (?)
unsigned int mav_goto = cmd->param1;
+
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status->set_nav_state = NAV_STATE_LOITER;
status->set_nav_state_timestamp = hrt_absolute_time();
@@ -513,11 +512,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
- /* Flight termination */
+ /* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
- transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
+ transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
@@ -566,7 +565,6 @@ int commander_thread_main(int argc, char *argv[])
{
/* not yet initialized */
commander_initialized = false;
- bool home_position_set = false;
bool battery_tune_played = false;
bool arm_tune_played = false;
@@ -580,6 +578,27 @@ int commander_thread_main(int argc, char *argv[])
/* welcome user */
warnx("starting");
+ char *main_states_str[MAIN_STATE_MAX];
+ main_states_str[0] = "MANUAL";
+ main_states_str[1] = "SEATBELT";
+ main_states_str[2] = "EASY";
+ main_states_str[3] = "AUTO";
+
+ char *arming_states_str[ARMING_STATE_MAX];
+ arming_states_str[0] = "INIT";
+ arming_states_str[1] = "STANDBY";
+ arming_states_str[2] = "ARMED";
+ arming_states_str[3] = "ARMED_ERROR";
+ arming_states_str[4] = "STANDBY_ERROR";
+ arming_states_str[5] = "REBOOT";
+ arming_states_str[6] = "IN_AIR_RESTORE";
+
+ char *failsafe_states_str[FAILSAFE_STATE_MAX];
+ failsafe_states_str[0] = "NORMAL";
+ failsafe_states_str[1] = "RTL";
+ failsafe_states_str[2] = "LAND";
+ failsafe_states_str[3] = "TERMINATION";
+
/* pthread for slow low prio thread */
pthread_t commander_low_prio_thread;
@@ -609,6 +628,7 @@ int commander_thread_main(int argc, char *argv[])
status.set_nav_state_timestamp = 0;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;
+ status.failsafe_state = FAILSAFE_STATE_NORMAL;
/* neither manual nor offboard control commands have been received */
status.offboard_control_signal_found_once = false;
@@ -666,8 +686,6 @@ int commander_thread_main(int argc, char *argv[])
/* Start monitoring loop */
unsigned counter = 0;
- unsigned low_voltage_counter = 0;
- unsigned critical_voltage_counter = 0;
unsigned stick_off_counter = 0;
unsigned stick_on_counter = 0;
@@ -745,7 +763,6 @@ int commander_thread_main(int argc, char *argv[])
int battery_sub = orb_subscribe(ORB_ID(battery_status));
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
- battery.voltage_v = 0.0f;
/* Subscribe to subsystem info topic */
int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
@@ -889,13 +906,12 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
- // warnx("bat v: %2.2f", battery.voltage_v);
-
- /* 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 > 4.0f) {
- status.battery_voltage = battery.voltage_v;
+ /* only consider battery voltage if system has been running 2s and battery voltage is valid */
+ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
+ status.battery_voltage = battery.voltage_filtered_v;
+ status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
- status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
+ status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
}
}
@@ -948,46 +964,29 @@ int commander_thread_main(int argc, char *argv[])
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
}
- // XXX remove later
- //warnx("bat remaining: %2.2f", status.battery_remaining);
-
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
- //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
- if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
- low_battery_voltage_actions_done = true;
- mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
- status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
- status_changed = true;
- battery_tune_played = false;
- }
-
- low_voltage_counter++;
+ low_battery_voltage_actions_done = true;
+ mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
+ status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
+ status_changed = true;
+ battery_tune_played = false;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
- if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
- critical_battery_voltage_actions_done = true;
- mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
- status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
- battery_tune_played = false;
-
- if (armed.armed) {
- arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+ critical_battery_voltage_actions_done = true;
+ mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
+ status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
+ battery_tune_played = false;
- } else {
- arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
- }
+ if (armed.armed) {
+ arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
- status_changed = true;
+ } else {
+ arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
}
- critical_voltage_counter++;
-
- } else {
-
- low_voltage_counter = 0;
- critical_voltage_counter = 0;
+ status_changed = true;
}
/* End battery voltage check */
@@ -1029,19 +1028,18 @@ int commander_thread_main(int argc, char *argv[])
* position to the current position.
*/
- if (!home_position_set && gps_position.fix_type >= 3 &&
- (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk
+ if (!status.condition_home_position_valid && gps_position.fix_type >= 3 &&
+ (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&
(hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
&& global_position.valid) {
- /* copy position data to uORB home message, store it locally as well */
+ /* copy position data to uORB home message, store it locally as well */
+ home.lat = global_position.lat;
+ home.lon = global_position.lon;
+ home.alt = global_position.alt;
- home.lat = (double)global_position.lat / 1e7d;
- home.lon = (double)global_position.lon / 1e7d;
- home.altitude = (float)global_position.alt;
-
- warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude);
- mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude);
+ warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt);
+ mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt);
/* announce new home position */
if (home_pub > 0) {
@@ -1052,129 +1050,163 @@ int commander_thread_main(int argc, char *argv[])
}
/* mark home position as set */
- home_position_set = true;
+ status.condition_home_position_valid = true;
tune_positive();
}
}
- /* ignore RC signals if in offboard control mode */
- if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
- /* start RC input check */
- if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
- /* handle the case where RC signal was regained */
- if (!status.rc_signal_found_once) {
- status.rc_signal_found_once = true;
- mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
- status_changed = true;
+ /* start RC input check */
+ if (sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
+ /* handle the case where RC signal was regained */
+ if (!status.rc_signal_found_once) {
+ status.rc_signal_found_once = true;
+ mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
+ status_changed = true;
- } else {
- if (status.rc_signal_lost) {
- mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
- status_changed = true;
- }
+ } else {
+ if (status.rc_signal_lost) {
+ mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
+ status_changed = true;
}
+ }
- status.rc_signal_lost = false;
-
- transition_result_t res; // store all transitions results here
+ status.rc_signal_lost = false;
- /* arm/disarm by RC */
- res = TRANSITION_NOT_CHANGED;
+ transition_result_t res; // store all transitions results here
- /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
- * do it only for rotary wings */
- if (status.is_rotary_wing &&
- (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
- (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
- sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+ /* arm/disarm by RC */
+ res = TRANSITION_NOT_CHANGED;
- if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
- /* 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);
- res = arming_state_transition(&status, &safety, new_arming_state, &armed);
- stick_off_counter = 0;
+ /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
+ * do it only for rotary wings */
+ if (status.is_rotary_wing &&
+ (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
+ (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
+ sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
- } else {
- stick_off_counter++;
- }
+ if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+ /* 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);
+ res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+ stick_off_counter = 0;
} else {
- stick_off_counter = 0;
+ stick_off_counter++;
}
- /* check if left stick is in lower right position and we're in MANUAL mode -> arm */
- if (status.arming_state == ARMING_STATE_STANDBY &&
- sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
- if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
- if (safety.safety_switch_available && !safety.safety_off) {
- print_reject_arm("NOT ARMING: Press safety switch first.");
+ } else {
+ stick_off_counter = 0;
+ }
- } else if (status.main_state != MAIN_STATE_MANUAL) {
- print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
+ /* check if left stick is in lower right position and we're in MANUAL mode -> arm */
+ if (status.arming_state == ARMING_STATE_STANDBY &&
+ sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+ if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+ if (safety.safety_switch_available && !safety.safety_off) {
+ print_reject_arm("NOT ARMING: Press safety switch first.");
- } else {
- res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
- }
-
- stick_on_counter = 0;
+ } else if (status.main_state != MAIN_STATE_MANUAL) {
+ print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
- stick_on_counter++;
+ res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
}
- } else {
stick_on_counter = 0;
+
+ } else {
+ stick_on_counter++;
}
- if (res == TRANSITION_CHANGED) {
- if (status.arming_state == ARMING_STATE_ARMED) {
- mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
+ } else {
+ stick_on_counter = 0;
+ }
- } else {
- mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
- }
+ if (res == TRANSITION_CHANGED) {
+ if (status.arming_state == ARMING_STATE_ARMED) {
+ mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
- } else if (res == TRANSITION_DENIED) {
- warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
- mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+ } else {
+ mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
}
- /* fill current_status according to mode switches */
- check_mode_switches(&sp_man, &status);
+ } else if (res == TRANSITION_DENIED) {
+ /* DENIED here indicates bug in the commander */
+ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
+ }
+
+ if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+ /* recover from failsafe */
+ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+ }
- /* evaluate the main state machine */
- res = check_main_state_machine(&status);
+ /* fill status according to mode switches */
+ check_mode_switches(&sp_man, &status);
- if (res == TRANSITION_CHANGED) {
- //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
- tune_positive();
+ /* evaluate the main state machine according to mode switches */
+ res = set_main_state_rc(&status);
+
+ if (res == TRANSITION_CHANGED) {
+ tune_positive();
+
+ } else if (res == TRANSITION_DENIED) {
+ /* DENIED here indicates bug in the commander */
+ mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
+ }
+
+ } else {
+ if (!status.rc_signal_lost) {
+ mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
+ status.rc_signal_lost = true;
+ status_changed = true;
+ }
+
+ if (armed.armed) {
+ if (status.main_state == MAIN_STATE_AUTO) {
+ /* check if AUTO mode still allowed */
+ transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
+
+ if (res == TRANSITION_DENIED) {
+ /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
+ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+
+ if (res == TRANSITION_DENIED) {
+ /* LAND not allowed, set TERMINATION state */
+ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+ }
+ }
+
+ } else {
+ /* failsafe for manual modes */
+ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
- } else if (res == TRANSITION_DENIED) {
- /* DENIED here indicates bug in the commander */
- warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
- mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+ if (res == TRANSITION_DENIED) {
+ /* RTL not allowed (no global position estimate), try LAND */
+ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+
+ if (res == TRANSITION_DENIED) {
+ /* LAND not allowed, set TERMINATION state */
+ res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+ }
+ }
}
} else {
- if (!status.rc_signal_lost) {
- mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
- status.rc_signal_lost = true;
- status_changed = true;
+ if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+ /* reset failsafe when disarmed */
+ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
}
}
- /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
- if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
- transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
- if (flighttermination_res == TRANSITION_CHANGED) {
+ // TODO remove this hack
+ /* flight termination in manual mode if assisted switch is on easy position */
+ if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+ if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive();
}
- } else {
- flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
}
-
/* handle commands last, as the system needs to be updated to handle them */
orb_check(cmd_sub, &updated);
@@ -1190,13 +1222,24 @@ int commander_thread_main(int argc, char *argv[])
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = check_arming_state_changed();
bool main_state_changed = check_main_state_changed();
- bool flighttermination_state_changed = check_flighttermination_state_changed();
+ bool failsafe_state_changed = check_failsafe_state_changed();
hrt_abstime t1 = hrt_absolute_time();
- if (arming_state_changed || main_state_changed) {
- mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state);
+ /* print new state */
+ if (arming_state_changed) {
+ status_changed = true;
+ mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]);
+ }
+
+ if (main_state_changed) {
+ status_changed = true;
+ mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
+ }
+
+ if (failsafe_state_changed) {
status_changed = true;
+ mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
}
/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
@@ -1375,72 +1418,72 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
}
void
-check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status)
+check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status)
{
/* main mode switch */
if (!isfinite(sp_man->mode_switch)) {
warnx("mode sw not finite");
- current_status->mode_switch = MODE_SWITCH_MANUAL;
+ status->mode_switch = MODE_SWITCH_MANUAL;
} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
- current_status->mode_switch = MODE_SWITCH_AUTO;
+ status->mode_switch = MODE_SWITCH_AUTO;
} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
- current_status->mode_switch = MODE_SWITCH_MANUAL;
+ status->mode_switch = MODE_SWITCH_MANUAL;
} else {
- current_status->mode_switch = MODE_SWITCH_ASSISTED;
+ status->mode_switch = MODE_SWITCH_ASSISTED;
}
/* return switch */
if (!isfinite(sp_man->return_switch)) {
- current_status->return_switch = RETURN_SWITCH_NONE;
+ status->return_switch = RETURN_SWITCH_NONE;
} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
- current_status->return_switch = RETURN_SWITCH_RETURN;
+ status->return_switch = RETURN_SWITCH_RETURN;
} else {
- current_status->return_switch = RETURN_SWITCH_NORMAL;
+ status->return_switch = RETURN_SWITCH_NORMAL;
}
/* assisted switch */
if (!isfinite(sp_man->assisted_switch)) {
- current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+ status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
- current_status->assisted_switch = ASSISTED_SWITCH_EASY;
+ status->assisted_switch = ASSISTED_SWITCH_EASY;
} else {
- current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+ status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
}
/* mission switch */
if (!isfinite(sp_man->mission_switch)) {
- current_status->mission_switch = MISSION_SWITCH_NONE;
+ status->mission_switch = MISSION_SWITCH_NONE;
} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
- current_status->mission_switch = MISSION_SWITCH_LOITER;
+ status->mission_switch = MISSION_SWITCH_LOITER;
} else {
- current_status->mission_switch = MISSION_SWITCH_MISSION;
+ status->mission_switch = MISSION_SWITCH_MISSION;
}
}
transition_result_t
-check_main_state_machine(struct vehicle_status_s *current_status)
+set_main_state_rc(struct vehicle_status_s *status)
{
/* evaluate the main state machine */
transition_result_t res = TRANSITION_DENIED;
- switch (current_status->mode_switch) {
+ switch (status->mode_switch) {
case MODE_SWITCH_MANUAL:
- res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+ res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
case MODE_SWITCH_ASSISTED:
- if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
- res = main_state_transition(current_status, MAIN_STATE_EASY);
+ if (status->assisted_switch == ASSISTED_SWITCH_EASY) {
+ res = main_state_transition(status, MAIN_STATE_EASY);
if (res != TRANSITION_DENIED)
break; // changed successfully or already in this state
@@ -1449,34 +1492,34 @@ check_main_state_machine(struct vehicle_status_s *current_status)
print_reject_mode("EASY");
}
- res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+ res = main_state_transition(status, MAIN_STATE_SEATBELT);
if (res != TRANSITION_DENIED)
break; // changed successfully or already in this mode
- if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages
+ if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages
print_reject_mode("SEATBELT");
// else fallback to MANUAL
- res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+ res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
case MODE_SWITCH_AUTO:
- res = main_state_transition(current_status, MAIN_STATE_AUTO);
+ res = main_state_transition(status, MAIN_STATE_AUTO);
if (res != TRANSITION_DENIED)
break; // changed successfully or already in this state
// else fallback to SEATBELT (EASY likely will not work too)
print_reject_mode("AUTO");
- res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+ res = main_state_transition(status, MAIN_STATE_SEATBELT);
if (res != TRANSITION_DENIED)
break; // changed successfully or already in this state
// else fallback to MANUAL
- res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+ res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-13 19:06:25 +0000