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.cpp80
1 files changed, 46 insertions, 34 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 0c4d48dea..2b528eef6 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));
@@ -546,24 +548,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
break;
-#if 0
/* 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
- //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
- if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
- transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
- cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
-
+ case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
+ if (cmd->param1 > 0.5f) {
+ //XXX update state machine?
+ armed_local->force_failsafe = true;
+ warnx("forcing failsafe");
} else {
- /* reject parachute depoyment not armed */
- cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ armed_local->force_failsafe = false;
+ warnx("disabling failsafe");
}
-
+ cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}
break;
-#endif
case VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;
@@ -741,6 +738,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);
@@ -918,6 +916,11 @@ int commander_thread_main(int argc, char *argv[])
struct system_power_s system_power;
memset(&system_power, 0, sizeof(system_power));
+ /* Subscribe to actuator controls (outputs) */
+ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+ struct actuator_controls_s actuator_controls;
+ memset(&actuator_controls, 0, sizeof(actuator_controls));
+
control_status_leds(&status, &armed, true);
/* now initialized */
@@ -977,6 +980,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;
@@ -1106,32 +1110,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;
@@ -1161,8 +1165,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 {
@@ -1170,7 +1174,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 {
@@ -1199,13 +1203,17 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
/* 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(battery.voltage_filtered_v, battery.discharged_mah);
+
+ /* get throttle (if armed), as we only care about energy negative throttle also counts */
+ float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
+ status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle);
}
}
@@ -1267,13 +1275,13 @@ int commander_thread_main(int argc, char *argv[])
}
/* 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) {
+ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
- } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
+ } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
@@ -1412,8 +1420,12 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = true;
} else if (arming_ret == TRANSITION_DENIED) {
- /* DENIED here indicates bug in the commander */
- mavlink_log_critical(mavlink_fd, "arming state transition denied");
+ /*
+ * the arming transition can be denied to a number of reasons:
+ * - pre-flight check failed (sensors not ok or not calibrated)
+ * - safety not disabled
+ * - system not in manual mode
+ */
tune_negative(true);
}
@@ -1441,7 +1453,7 @@ int commander_thread_main(int argc, char *argv[])
/* data links check */
bool have_link = false;
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
- if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
+ if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
/* handle the case where data link was regained */
if (telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "data link %i regained", i);
@@ -1494,7 +1506,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;
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-28 00:25:40 +0000