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.cpp338
1 files changed, 219 insertions, 119 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 53ed34f46..5c0628a16 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -117,9 +117,10 @@ extern struct system_load_s system_load;
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-#define POSITION_TIMEOUT 100000 /**< consider the local or global position estimate invalid after 100ms */
-#define RC_TIMEOUT 500000
-#define DIFFPRESS_TIMEOUT 2000000
+#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */
+#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
+#define RC_TIMEOUT 500000
+#define DIFFPRESS_TIMEOUT 2000000
#define PRINT_INTERVAL 5000000
#define PRINT_MODE_REJECT_INTERVAL 2000000
@@ -220,7 +221,7 @@ void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
-transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy);
+transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
@@ -232,8 +233,9 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
int commander_main(int argc, char *argv[])
{
- if (argc < 1)
+ if (argc < 1) {
usage("missing command");
+ }
if (!strcmp(argv[1], "start")) {
@@ -247,7 +249,7 @@ int commander_main(int argc, char *argv[])
daemon_task = task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
- 3000,
+ 2950,
commander_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
@@ -260,8 +262,9 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
- if (!thread_running)
+ if (!thread_running) {
errx(0, "commander already stopped");
+ }
thread_should_exit = true;
@@ -303,8 +306,9 @@ int commander_main(int argc, char *argv[])
void usage(const char *reason)
{
- if (reason)
+ if (reason) {
fprintf(stderr, "%s\n", reason);
+ }
fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
@@ -363,20 +367,22 @@ void print_status()
static orb_advert_t status_pub;
-transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy)
+transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy)
{
- transition_result_t arming_res = TRANSITION_NOT_CHANGED;
-
- // Transition the armed state. By passing mavlink_fd to arming_state_transition it will
- // output appropriate error messages if the state cannot transition.
- arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
- if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
- mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
- } else if (arming_res == TRANSITION_DENIED) {
- tune_negative(true);
- }
-
- return arming_res;
+ transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+
+ // Transition the armed state. By passing mavlink_fd to arming_state_transition it will
+ // output appropriate error messages if the state cannot transition.
+ arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
+
+ if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
+ mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
+
+ } else if (arming_res == TRANSITION_DENIED) {
+ tune_negative(true);
+ }
+
+ return arming_res;
}
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
@@ -416,14 +422,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
- if (hil_ret == OK)
+ if (hil_ret == OK) {
ret = true;
+ }
- // Transition the arming state
- arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
+ // Transition the arming state
+ arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
- if (arming_res == TRANSITION_CHANGED)
+ if (arming_res == TRANSITION_CHANGED) {
ret = true;
+ }
/* set main state */
transition_result_t main_res = TRANSITION_DENIED;
@@ -434,17 +442,21 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
- } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
- /* SEATBELT */
- main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
+ } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
+ /* ALTCTL */
+ main_res = main_state_transition(status, MAIN_STATE_ALTCTL);
- } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
- /* EASY */
- main_res = main_state_transition(status, MAIN_STATE_EASY);
+ } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
+ /* POSCTL */
+ main_res = main_state_transition(status, MAIN_STATE_POSCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
+
+ } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
+ /* ACRO */
+ main_res = main_state_transition(status, MAIN_STATE_ACRO);
}
} else {
@@ -455,8 +467,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
- /* EASY */
- main_res = main_state_transition(status, MAIN_STATE_EASY);
+ /* POSCTL */
+ main_res = main_state_transition(status, MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
@@ -465,8 +477,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
- if (main_res == TRANSITION_CHANGED)
+ if (main_res == TRANSITION_CHANGED) {
ret = true;
+ }
if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -479,19 +492,28 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
- // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
- // We use an float epsilon delta to test float equality.
- if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
+ // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
+ // We use an float epsilon delta to test float equality.
+ if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
- } else {
- transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
- if (arming_res == TRANSITION_DENIED) {
- mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
- result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
- } else {
- result = VEHICLE_CMD_RESULT_ACCEPTED;
- }
- }
+
+ } else {
+
+ // Flick to inair restore first if this comes from an onboard system
+ if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) {
+ status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
+ }
+
+ transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
+
+ if (arming_res == TRANSITION_DENIED) {
+ mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+
+ } else {
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ }
+ }
}
break;
@@ -519,7 +541,7 @@ 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
//XXX: to enable the parachute, a param needs to be set
@@ -539,6 +561,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
case VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;
+
if (use_current) {
/* use current position */
if (status->condition_global_position_valid) {
@@ -582,6 +605,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
break;
+
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@@ -595,6 +619,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
break;
}
+
if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(*cmd, result);
@@ -628,9 +653,10 @@ int commander_thread_main(int argc, char *argv[])
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[1] = "ALTCTL";
+ main_states_str[2] = "POSCTL";
main_states_str[3] = "AUTO";
+ main_states_str[4] = "ACRO";
char *arming_states_str[ARMING_STATE_MAX];
arming_states_str[0] = "INIT";
@@ -722,7 +748,7 @@ int commander_thread_main(int argc, char *argv[])
pthread_attr_t commander_low_prio_attr;
pthread_attr_init(&commander_low_prio_attr);
- pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
+ pthread_attr_setstacksize(&commander_low_prio_attr, 2900);
struct sched_param param;
(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@@ -741,8 +767,9 @@ int commander_thread_main(int argc, char *argv[])
bool low_battery_voltage_actions_done = false;
bool critical_battery_voltage_actions_done = false;
- uint64_t last_idle_time = 0;
- uint64_t start_time = 0;
+ hrt_abstime last_idle_time = 0;
+ hrt_abstime start_time = 0;
+ hrt_abstime last_auto_state_valid = 0;
bool status_changed = true;
bool param_init_forced = true;
@@ -771,6 +798,9 @@ int commander_thread_main(int argc, char *argv[])
int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct vehicle_global_position_s global_position;
memset(&global_position, 0, sizeof(global_position));
+ /* Init EPH and EPV */
+ global_position.eph = 1000.0f;
+ global_position.epv = 1000.0f;
/* Subscribe to local position data */
int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
@@ -873,6 +903,7 @@ int commander_thread_main(int argc, char *argv[])
/* re-check RC calibration */
rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
}
+
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
param_get(_param_enable_parachute, &parachute_enabled);
@@ -913,6 +944,7 @@ int commander_thread_main(int argc, char *argv[])
/* disarm if safety is now on and still armed */
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
}
@@ -930,23 +962,31 @@ int commander_thread_main(int argc, char *argv[])
/* update condition_global_position_valid */
/* hysteresis for EPH/EPV */
bool eph_epv_good;
+
if (status.condition_global_position_valid) {
if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) {
eph_epv_good = false;
+
+ } else {
+ eph_epv_good = true;
}
} else {
if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
eph_epv_good = true;
+
+ } else {
+ eph_epv_good = false;
}
}
+
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
/* check if GPS fix is ok */
/* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
- (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
+ (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
home.lat = global_position.lat;
home.lon = global_position.lon;
@@ -977,10 +1017,30 @@ int commander_thread_main(int argc, char *argv[])
}
/* update condition_local_position_valid and condition_local_altitude_valid */
- check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed);
+ /* hysteresis for EPH */
+ bool local_eph_good;
+
+ if (status.condition_global_position_valid) {
+ if (local_position.eph > eph_epv_threshold * 2.0f) {
+ local_eph_good = false;
+
+ } else {
+ local_eph_good = true;
+ }
+
+ } else {
+ if (local_position.eph < eph_epv_threshold) {
+ local_eph_good = true;
+
+ } else {
+ local_eph_good = false;
+ }
+ }
+ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
static bool published_condition_landed_fw = false;
+
if (status.is_rotary_wing && status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) {
status.condition_landed = local_position.landed;
@@ -994,6 +1054,7 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
}
}
+
} else {
if (!published_condition_landed_fw) {
status.condition_landed = false; // Fixedwing does not have a landing detector currently
@@ -1063,8 +1124,9 @@ int commander_thread_main(int argc, char *argv[])
/* compute system load */
uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
- if (last_idle_time > 0)
- status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle
+ if (last_idle_time > 0) {
+ status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle
+ }
last_idle_time = system_load.tasks[0].total_runtime;
@@ -1140,22 +1202,22 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = false;
- transition_result_t res; // store all transitions results here
+ transition_result_t arming_res; // store all transitions results here
/* arm/disarm by RC */
- res = TRANSITION_NOT_CHANGED;
+ arming_res = TRANSITION_NOT_CHANGED;
- /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
+ /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST 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 < 0.1f) {
+ (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) &&
+ sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
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);
+ arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed);
stick_off_counter = 0;
} else {
@@ -1168,16 +1230,16 @@ int commander_thread_main(int argc, char *argv[])
/* 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 < 0.1f) {
+ sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
- print_reject_arm("NOT ARMING: Press safety switch first.");
+ print_reject_arm("#audio: NOT ARMING: Press safety switch first.");
} else if (status.main_state != MAIN_STATE_MANUAL) {
- print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
+ print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
} else {
- res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+ arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
}
stick_on_counter = 0;
@@ -1190,7 +1252,7 @@ int commander_thread_main(int argc, char *argv[])
stick_on_counter = 0;
}
- if (res == TRANSITION_CHANGED) {
+ if (arming_res == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
@@ -1198,24 +1260,24 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
}
- } else if (res == TRANSITION_DENIED) {
+ } else if (arming_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);
+ (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
/* evaluate the main state machine according to mode switches */
- res = set_main_state_rc(&status, &sp_man);
+ transition_result_t main_res = set_main_state_rc(&status, &sp_man);
/* play tune on mode change only if armed, blink LED always */
- if (res == TRANSITION_CHANGED) {
+ if (main_res == TRANSITION_CHANGED) {
tune_positive(armed.armed);
- } else if (res == TRANSITION_DENIED) {
+ } else if (main_res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
}
@@ -1228,7 +1290,8 @@ int commander_thread_main(int argc, char *argv[])
status.set_nav_state_timestamp = hrt_absolute_time();
} else {
- /* MISSION switch */
+
+ /* LOITER switch */
if (sp_man.loiter_switch == SWITCH_POS_ON) {
/* stick is in LOITER position */
status.set_nav_state = NAV_STATE_LOITER;
@@ -1240,7 +1303,7 @@ int commander_thread_main(int argc, char *argv[])
status.set_nav_state_timestamp = hrt_absolute_time();
} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
- pos_sp_triplet.nav_state == NAV_STATE_RTL) {
+ pos_sp_triplet.nav_state == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
status.set_nav_state = NAV_STATE_MISSION;
status.set_nav_state_timestamp = hrt_absolute_time();
@@ -1257,34 +1320,39 @@ int commander_thread_main(int argc, char *argv[])
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);
+ transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO);
+
+ if (auto_res == TRANSITION_NOT_CHANGED) {
+ last_auto_state_valid = hrt_absolute_time();
+ }
- if (res == TRANSITION_DENIED) {
+ /* still invalid state after the timeout interval, execute failsafe */
+ if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) {
/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
- res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+ auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
- if (res == TRANSITION_DENIED) {
+ if (auto_res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
- transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+ (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
}
} else {
/* failsafe for manual modes */
- transition_result_t res = TRANSITION_DENIED;
+ transition_result_t manual_res = TRANSITION_DENIED;
if (!status.condition_landed) {
/* vehicle is not landed, try to perform RTL */
- res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+ manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
}
- if (res == TRANSITION_DENIED) {
+ if (manual_res == TRANSITION_DENIED) {
/* RTL not allowed (no global position estimate) or not wanted, try LAND */
- res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+ manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
- if (res == TRANSITION_DENIED) {
+ if (manual_res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
- res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+ (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
}
}
@@ -1292,14 +1360,14 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* reset failsafe when disarmed */
- transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+ (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
}
}
// TODO remove this hack
- /* flight termination in manual mode if assisted switch is on easy position */
- if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) {
+ /* flight termination in manual mode if assist switch is on POSCTL position */
+ if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive(armed.armed);
}
@@ -1313,8 +1381,9 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
- if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub))
+ if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) {
status_changed = true;
+ }
}
/* check which state machines for changes, clear "changed" flag */
@@ -1331,7 +1400,7 @@ int commander_thread_main(int argc, char *argv[])
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
- (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
+ (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
// TODO remove code duplication
home.lat = global_position.lat;
@@ -1339,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[])
home.alt = global_position.alt;
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
- mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
+ mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
/* announce new home position */
if (home_pub > 0) {
@@ -1353,6 +1422,7 @@ int commander_thread_main(int argc, char *argv[])
status.condition_home_position_valid = true;
}
}
+
was_armed = armed.armed;
if (main_state_changed) {
@@ -1516,21 +1586,24 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
} else if (actuator_armed->ready_to_arm) {
/* ready to arm, blink at 1Hz */
- if (leds_counter % 20 == 0)
+ if (leds_counter % 20 == 0) {
led_toggle(LED_BLUE);
+ }
} else {
/* not ready to arm, blink at 10Hz */
- if (leds_counter % 2 == 0)
+ if (leds_counter % 2 == 0) {
led_toggle(LED_BLUE);
+ }
}
#endif
/* give system warnings on error LED, XXX maybe add memory usage warning too */
if (status->load > 0.95f) {
- if (leds_counter % 2 == 0)
+ if (leds_counter % 2 == 0) {
led_toggle(LED_AMBER);
+ }
} else {
led_off(LED_AMBER);
@@ -1551,30 +1624,35 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
break;
case SWITCH_POS_OFF: // MANUAL
- res = main_state_transition(status, MAIN_STATE_MANUAL);
+ if (sp_man->acro_switch == SWITCH_POS_ON) {
+ res = main_state_transition(status, MAIN_STATE_ACRO);
+
+ } else {
+ res = main_state_transition(status, MAIN_STATE_MANUAL);
+ }
// TRANSITION_DENIED is not possible here
break;
- case SWITCH_POS_MIDDLE: // ASSISTED
- if (sp_man->assisted_switch == SWITCH_POS_ON) {
- res = main_state_transition(status, MAIN_STATE_EASY);
+ case SWITCH_POS_MIDDLE: // ASSIST
+ if (sp_man->posctl_switch == SWITCH_POS_ON) {
+ res = main_state_transition(status, MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
- // else fallback to SEATBELT
- print_reject_mode(status, "EASY");
+ // else fallback to ALTCTL
+ print_reject_mode(status, "POSCTL");
}
- res = main_state_transition(status, MAIN_STATE_SEATBELT);
+ res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
}
- if (sp_man->assisted_switch != SWITCH_POS_ON) {
- print_reject_mode(status, "SEATBELT");
+ if (sp_man->posctl_switch != SWITCH_POS_ON) {
+ print_reject_mode(status, "ALTCTL");
}
// else fallback to MANUAL
@@ -1589,9 +1667,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
break; // changed successfully or already in this state
}
- // else fallback to SEATBELT (EASY likely will not work too)
+ // else fallback to ALTCTL (POSCTL likely will not work too)
print_reject_mode(status, "AUTO");
- res = main_state_transition(status, MAIN_STATE_SEATBELT);
+ res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -1637,7 +1715,7 @@ set_control_mode()
control_mode.flag_control_velocity_enabled = false;
break;
- case MAIN_STATE_SEATBELT:
+ case MAIN_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -1648,7 +1726,7 @@ set_control_mode()
control_mode.flag_control_velocity_enabled = false;
break;
- case MAIN_STATE_EASY:
+ case MAIN_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -1661,6 +1739,18 @@ set_control_mode()
case MAIN_STATE_AUTO:
navigator_enabled = true;
+ break;
+
+ case MAIN_STATE_ACRO:
+ control_mode.flag_control_manual_enabled = true;
+ control_mode.flag_control_auto_enabled = false;
+ control_mode.flag_control_rates_enabled = true;
+ control_mode.flag_control_attitude_enabled = false;
+ control_mode.flag_control_altitude_enabled = false;
+ control_mode.flag_control_climb_rate_enabled = false;
+ control_mode.flag_control_position_enabled = false;
+ control_mode.flag_control_velocity_enabled = false;
+ break;
default:
break;
@@ -1744,7 +1834,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
- tune_positive(true);
+ tune_positive(true);
break;
case VEHICLE_CMD_RESULT_DENIED:
@@ -1758,7 +1848,8 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
- mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
+ /* this needs additional hints to the user - so let other messages pass and be spoken */
+ mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command);
tune_negative(true);
break;
@@ -1794,8 +1885,9 @@ void *commander_low_prio_loop(void *arg)
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
/* timed out - periodic check for thread_should_exit, etc. */
- if (pret == 0)
+ if (pret == 0) {
continue;
+ }
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
@@ -1810,8 +1902,9 @@ void *commander_low_prio_loop(void *arg)
if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
- cmd.command == VEHICLE_CMD_DO_SET_SERVO)
+ cmd.command == VEHICLE_CMD_DO_SET_SERVO) {
continue;
+ }
/* only handle low-priority commands here */
switch (cmd.command) {
@@ -1889,6 +1982,7 @@ void *commander_low_prio_loop(void *arg)
/* airspeed calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_airspeed_calibration(mavlink_fd);
+
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status.rc_input_blocked) {
@@ -1903,10 +1997,12 @@ void *commander_low_prio_loop(void *arg)
}
- if (calib_ret == OK)
+ if (calib_ret == OK) {
tune_positive(true);
- else
+
+ } else {
tune_negative(true);
+ }
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
@@ -1926,11 +2022,13 @@ void *commander_low_prio_loop(void *arg)
mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR");
/* convenience as many parts of NuttX use negative errno */
- if (ret < 0)
+ if (ret < 0) {
ret = -ret;
+ }
- if (ret < 1000)
+ if (ret < 1000) {
mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
+ }
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
@@ -1946,11 +2044,13 @@ void *commander_low_prio_loop(void *arg)
mavlink_log_critical(mavlink_fd, "#audio: parameters save error");
/* convenience as many parts of NuttX use negative errno */
- if (ret < 0)
+ if (ret < 0) {
ret = -ret;
+ }
- if (ret < 1000)
+ if (ret < 1000) {
mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
+ }
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
@@ -1960,8 +2060,8 @@ void *commander_low_prio_loop(void *arg)
}
case VEHICLE_CMD_START_RX_PAIR:
- /* handled in the IO driver */
- break;
+ /* handled in the IO driver */
+ break;
default:
/* don't answer on unsupported commands, it will be done in main loop */
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-09 20:27:53 +0000