aboutsummaryrefslogtreecommitdiff
path: root/src/modules/commander
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/commander')
-rw-r--r--src/modules/commander/accelerometer_calibration.cpp3
-rw-r--r--src/modules/commander/calibration_routines.cpp7
-rw-r--r--src/modules/commander/commander.cpp72
-rw-r--r--src/modules/commander/commander_helper.cpp6
-rw-r--r--src/modules/commander/state_machine_helper.cpp169
5 files changed, 224 insertions, 33 deletions
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 24da452b1..be465ab76 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -131,6 +131,7 @@
#include <fcntl.h>
#include <sys/prctl.h>
#include <math.h>
+#include <float.h>
#include <mathlib/mathlib.h>
#include <string.h>
#include <drivers/drv_hrt.h>
@@ -526,7 +527,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
- if (det == 0.0f) {
+ if (fabsf(det) < FLT_EPSILON) {
return ERROR; // Singular matrix
}
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index 9d79124e7..43f341ae7 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -40,6 +40,7 @@
*/
#include <math.h>
+#include <float.h>
#include "calibration_routines.h"
@@ -179,9 +180,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
- aA = (aA == 0.0f) ? 1.0f : aA;
- aB = (aB == 0.0f) ? 1.0f : aB;
- aC = (aC == 0.0f) ? 1.0f : aC;
+ aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA;
+ aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB;
+ aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC;
//Compute next iteration
nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index bb42889ea..efa26eb97 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -52,6 +52,7 @@
#include <fcntl.h>
#include <errno.h>
#include <systemlib/err.h>
+#include <systemlib/circuit_breaker.h>
#include <debug.h>
#include <sys/prctl.h>
#include <sys/stat.h>
@@ -76,6 +77,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/safety.h>
+#include <uORB/topics/system_power.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/telemetry_status.h>
@@ -371,16 +373,16 @@ 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_local, 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);
+ arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
- mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
+ mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
@@ -507,7 +509,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
- mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
+ mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f",
+ (double)cmd->param1,
+ (double)cmd->param2,
+ (double)cmd->param3,
+ (double)cmd->param4,
+ (double)cmd->param5,
+ (double)cmd->param6,
+ (double)cmd->param7);
}
}
break;
@@ -700,6 +709,12 @@ int commander_thread_main(int argc, char *argv[])
status.counter++;
status.timestamp = hrt_absolute_time();
+ status.condition_power_input_valid = true;
+ status.avionics_power_rail_voltage = -1.0f;
+
+ // CIRCUIT BREAKERS
+ status.circuit_breaker_engaged_power_check = false;
+
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -752,7 +767,6 @@ int commander_thread_main(int argc, char *argv[])
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;
@@ -846,6 +860,11 @@ int commander_thread_main(int argc, char *argv[])
struct position_setpoint_triplet_s pos_sp_triplet;
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
+ /* Subscribe to system power */
+ int system_power_sub = orb_subscribe(ORB_ID(system_power));
+ struct system_power_s system_power;
+ memset(&system_power, 0, sizeof(system_power));
+
control_status_leds(&status, &armed, true);
/* now initialized */
@@ -862,6 +881,7 @@ int commander_thread_main(int argc, char *argv[])
bool arming_state_changed = false;
bool main_state_changed = false;
bool failsafe_old = false;
+ bool system_checked = false;
while (!thread_should_exit) {
@@ -903,6 +923,9 @@ int commander_thread_main(int argc, char *argv[])
/* check and update system / component ID */
param_get(_param_system_id, &(status.system_id));
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_changed = true;
/* re-check RC calibration */
@@ -915,6 +938,15 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
}
+ /* Perform system checks (again) once params are loaded and MAVLink is up. */
+ if (!system_checked && mavlink_fd &&
+ (telemetry.heartbeat_time > 0) &&
+ (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) {
+
+ (void)rc_calibration_check(mavlink_fd);
+ system_checked = true;
+ }
+
orb_check(sp_man_sub, &updated);
if (updated) {
@@ -945,6 +977,26 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
}
+ orb_check(system_power_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
+
+ if (hrt_elapsed_time(&system_power.timestamp) < 200000) {
+ if (system_power.servo_valid &&
+ !system_power.brick_valid &&
+ !system_power.usb_connected) {
+ /* flying only on servo rail, this is unsafe */
+ status.condition_power_input_valid = false;
+ } else {
+ status.condition_power_input_valid = true;
+ }
+
+ /* copy avionics voltage */
+ status.avionics_power_rail_voltage = system_power.voltage5V_v;
+ }
+ }
+
check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
/* update safety topic */
@@ -1254,12 +1306,13 @@ int commander_thread_main(int argc, char *argv[])
if (status.arming_state == ARMING_STATE_STANDBY &&
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("#audio: NOT ARMING: Press safety switch first.");
- } else if (status.main_state != MAIN_STATE_MANUAL) {
+ /* we check outside of the transition function here because the requirement
+ * for being in manual mode only applies to manual arming actions.
+ * the system can be armed in auto if armed via the GCS.
+ */
+ if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
-
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1289,6 +1342,7 @@ int commander_thread_main(int argc, char *argv[])
} else if (arming_ret == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
+ tune_negative(true);
}
/* evaluate the main state machine according to mode switches */
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 940a04aa1..80e6861f6 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -209,12 +209,18 @@ int led_init()
/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
(void)ioctl(leds, LED_ON, LED_BLUE);
+ /* switch blue off */
+ led_off(LED_BLUE);
+
/* we consider the amber led mandatory */
if (ioctl(leds, LED_ON, LED_AMBER)) {
warnx("Amber LED: ioctl fail\n");
return ERROR;
}
+ /* switch amber off */
+ led_off(LED_AMBER);
+
/* then try RGB LEDs, this can fail on FMUv1*/
rgbleds = open(RGBLED_DEVICE_PATH, 0);
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 74885abf6..423ce2f23 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -46,20 +46,32 @@
#include <dirent.h>
#include <fcntl.h>
#include <string.h>
+#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/differential_pressure.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_airspeed.h>
#include <drivers/drv_device.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
#include "commander_helper.h"
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
+
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
@@ -98,18 +110,31 @@ arming_state_transition(struct vehicle_status_s *status, /// current
ASSERT(ARMING_STATE_INIT == 0);
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
- /*
- * Perform an atomic state update
- */
- irqstate_t flags = irqsave();
-
transition_result_t ret = TRANSITION_DENIED;
+ arming_state_t current_arming_state = status->arming_state;
+
/* only check transition if the new state is actually different from the current one */
- if (new_arming_state == status->arming_state) {
+ if (new_arming_state == current_arming_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
+
+ /*
+ * Get sensing state if necessary
+ */
+ int prearm_ret = OK;
+
+ /* only perform the check if we have to */
+ if (new_arming_state == ARMING_STATE_ARMED) {
+ prearm_ret = prearm_check(status, mavlink_fd);
+ }
+
+ /*
+ * Perform an atomic state update
+ */
+ irqstate_t flags = irqsave();
+
/* enforce lockdown in HIL */
if (status->hil_state == HIL_STATE_ON) {
armed->lockdown = true;
@@ -124,15 +149,44 @@ arming_state_transition(struct vehicle_status_s *status, /// current
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == ARMING_STATE_ARMED) {
- // Fail transition if we need safety switch press
- // Allow if coming from in air restore
+
+ // Do not perform pre-arm checks if coming from in air restore
// Allow if HIL_STATE_ON
- if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
- if (mavlink_fd) {
- mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first.");
+ if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE &&
+ status->hil_state == HIL_STATE_OFF) {
+
+ // Fail transition if pre-arm check fails
+ if (prearm_ret) {
+ valid_transition = false;
+
+ // Fail transition if we need safety switch press
+ } else if (safety->safety_switch_available && !safety->safety_off) {
+
+ mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!");
+
+ valid_transition = false;
+ }
+
+ // Perform power checks only if circuit breaker is not
+ // engaged for these checks
+ if (!status->circuit_breaker_engaged_power_check) {
+ // Fail transition if power is not good
+ if (!status->condition_power_input_valid) {
+
+ mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module.");
+ valid_transition = false;
+ }
+
+ // Fail transition if power levels on the avionics rail
+ // are measured but are insufficient
+ if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) &&
+ (status->avionics_power_rail_voltage < 4.9f)) {
+
+ mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
+ valid_transition = false;
+ }
}
- valid_transition = false;
}
} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
@@ -157,17 +211,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
}
- }
- /* end of atomic state update */
- irqrestore(flags);
+ /* end of atomic state update */
+ irqrestore(flags);
+ }
if (ret == TRANSITION_DENIED) {
- static const char *errMsg = "Invalid arming transition from %s to %s";
+ static const char *errMsg = "INVAL: %s - %s";
- if (mavlink_fd) {
- mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
- }
+ mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
@@ -539,3 +591,80 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
return status->nav_state != nav_state_old;
}
+int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
+{
+ int ret;
+
+ int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+ if (fd < 0) {
+ mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
+ ret = fd;
+ goto system_eval;
+ }
+
+ ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+
+ if (ret != OK) {
+ mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
+ goto system_eval;
+ }
+
+ /* check measurement result range */
+ struct accel_report acc;
+ ret = read(fd, &acc, sizeof(acc));
+
+ if (ret == sizeof(acc)) {
+ /* evaluate values */
+ float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
+
+ if (accel_scale < 9.78f || accel_scale > 9.83f) {
+ mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended.");
+ }
+
+ if (accel_scale > 30.0f /* m/s^2 */) {
+ mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
+ /* this is frickin' fatal */
+ ret = ERROR;
+ goto system_eval;
+ } else {
+ ret = OK;
+ }
+ } else {
+ mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
+ /* this is frickin' fatal */
+ ret = ERROR;
+ goto system_eval;
+ }
+
+ if (!status->is_rotary_wing) {
+ fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+
+ if (fd < 0) {
+ mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
+ ret = fd;
+ goto system_eval;
+ }
+
+ struct differential_pressure_s diff_pres;
+
+ ret = read(fd, &diff_pres, sizeof(diff_pres));
+
+ if (ret == sizeof(diff_pres)) {
+ if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) {
+ mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
+ // XXX do not make this fatal yet
+ ret = OK;
+ }
+ } else {
+ mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
+ /* this is frickin' fatal */
+ ret = ERROR;
+ goto system_eval;
+ }
+ }
+
+system_eval:
+ close(fd);
+ return ret;
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-13 03:30:43 +0000