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.cpp52
1 files changed, 28 insertions, 24 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index eac8b0afc..2d2aeecfa 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -124,7 +124,7 @@ 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 (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */
+#define POSITION_TIMEOUT (2 * 1000 * 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 DL_TIMEOUT 5 * 1000* 1000
@@ -163,7 +163,8 @@ static bool on_usb_power = false;
static float takeoff_alt = 5.0f;
static int parachute_enabled = 0;
-static float eph_epv_threshold = 5.0f;
+static float eph_threshold = 5.0f;
+static float epv_threshold = 10.0f;
static struct vehicle_status_s status;
static struct actuator_armed_s armed;
@@ -335,6 +336,7 @@ void print_status()
{
warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE");
warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
+ warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage);
/* read all relevant states */
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -391,7 +393,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
// 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_local);
+ arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -758,6 +760,7 @@ int commander_thread_main(int argc, char *argv[])
// CIRCUIT BREAKERS
status.circuit_breaker_engaged_power_check = false;
+ status.circuit_breaker_engaged_airspd_check = false;
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -999,6 +1002,7 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_component_id, &(status.component_id));
status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+ status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status_changed = true;
@@ -1103,7 +1107,7 @@ int commander_thread_main(int argc, char *argv[])
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_fd)) {
+ if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
arming_state_changed = true;
}
@@ -1128,32 +1132,32 @@ int commander_thread_main(int argc, char *argv[])
/* update condition_global_position_valid */
/* hysteresis for EPH/EPV */
- bool eph_epv_good;
+ bool eph_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;
+ if (global_position.eph > eph_threshold * 2.5f) {
+ eph_good = false;
} else {
- eph_epv_good = true;
+ eph_good = true;
}
} else {
- if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
- eph_epv_good = true;
+ if (global_position.eph < eph_threshold) {
+ eph_good = true;
} else {
- eph_epv_good = false;
+ eph_good = false;
}
}
- check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
+ check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_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_threshold) && (global_position.epv < epv_threshold)) {
home.lat = global_position.lat;
home.lon = global_position.lon;
@@ -1183,8 +1187,8 @@ int commander_thread_main(int argc, char *argv[])
/* hysteresis for EPH */
bool local_eph_good;
- if (status.condition_global_position_valid) {
- if (local_position.eph > eph_epv_threshold * 2.0f) {
+ if (status.condition_local_position_valid) {
+ if (local_position.eph > eph_threshold * 2.5f) {
local_eph_good = false;
} else {
@@ -1192,7 +1196,7 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
- if (local_position.eph < eph_epv_threshold) {
+ if (local_position.eph < eph_threshold) {
local_eph_good = true;
} else {
@@ -1306,14 +1310,14 @@ int commander_thread_main(int argc, char *argv[])
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
- arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
+ arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
} else {
- arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
+ arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -1327,7 +1331,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
- arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+ arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -1386,7 +1390,7 @@ int commander_thread_main(int argc, char *argv[])
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);
- arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
+ arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@@ -1412,7 +1416,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
- arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
+ arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@@ -1524,7 +1528,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_threshold) && (global_position.epv < epv_threshold)) {
// TODO remove code duplication
home.lat = global_position.lat;
@@ -2184,7 +2188,7 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
- if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
+ if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@@ -2247,7 +2251,7 @@ void *commander_low_prio_loop(void *arg)
tune_negative(true);
}
- arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+ arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
break;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-21 19:41:10 +0000