aboutsummaryrefslogtreecommitdiff
path: root/src/modules/commander/commander.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/commander/commander.c')
-rw-r--r--src/modules/commander/commander.c1429
1 files changed, 829 insertions, 600 deletions
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c
index 937c515e6..edcdf7e54 100644
--- a/src/modules/commander/commander.c
+++ b/src/modules/commander/commander.c
@@ -60,13 +60,9 @@
#include <debug.h>
#include <sys/prctl.h>
#include <string.h>
-#include <drivers/drv_led.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_tone_alarm.h>
-#include "state_machine_helper.h"
-#include "systemlib/systemlib.h"
#include <math.h>
#include <poll.h>
+
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/battery_status.h>
@@ -77,18 +73,27 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_safety.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/safety.h>
#include <mavlink/mavlink_log.h>
+#include <drivers/drv_led.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
+#include <systemlib/cpuload.h>
+
+#include "state_machine_helper.h"
-/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
@@ -103,7 +108,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
-#include <systemlib/cpuload.h>
+
extern struct system_load_s system_load;
/* Decouple update interval and hysteris counters, all depends on intervals */
@@ -122,25 +127,39 @@ extern struct system_load_s system_load;
#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */
+
/* File descriptors */
static int leds;
static int buzzer;
static int mavlink_fd;
-static bool commander_initialized = false;
-static struct vehicle_status_s current_status; /**< Main state machine */
-static orb_advert_t stat_pub;
-
-// static uint16_t nofix_counter = 0;
-// static uint16_t gotfix_counter = 0;
-
-static unsigned int failsafe_lowlevel_timeout_ms;
+/* flags */
+static bool commander_initialized = false;
static bool thread_should_exit = false; /**< daemon exit flag */
static bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
+/* timout until lowlevel failsafe */
+static unsigned int failsafe_lowlevel_timeout_ms;
+
+/* tasks waiting for low prio thread */
+enum {
+ LOW_PRIO_TASK_NONE = 0,
+ LOW_PRIO_TASK_PARAM_SAVE,
+ LOW_PRIO_TASK_PARAM_LOAD,
+ LOW_PRIO_TASK_GYRO_CALIBRATION,
+ LOW_PRIO_TASK_MAG_CALIBRATION,
+ LOW_PRIO_TASK_ALTITUDE_CALIBRATION,
+ LOW_PRIO_TASK_RC_CALIBRATION,
+ LOW_PRIO_TASK_ACCEL_CALIBRATION,
+ LOW_PRIO_TASK_AIRSPEED_CALIBRATION
+} low_prio_task;
+
+
/* pthread loops */
-static void *orb_receive_loop(void *arg);
+void *orb_receive_loop(void *arg);
+void *commander_low_prio_loop(void *arg);
__EXPORT int commander_main(int argc, char *argv[]);
@@ -149,17 +168,25 @@ __EXPORT int commander_main(int argc, char *argv[]);
*/
int commander_thread_main(int argc, char *argv[]);
-static int buzzer_init(void);
-static void buzzer_deinit(void);
-static int led_init(void);
-static void led_deinit(void);
-static int led_toggle(int led);
-static int led_on(int led);
-static int led_off(int led);
-static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_rc_calibration(int status_pub, struct vehicle_status_s *status);
-static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
+int buzzer_init(void);
+void buzzer_deinit(void);
+int led_init(void);
+void led_deinit(void);
+int led_toggle(int led);
+int led_on(int led);
+int led_off(int led);
+void tune_error(void);
+void tune_positive(void);
+void tune_neutral(void);
+void tune_negative(void);
+void do_reboot(void);
+void do_gyro_calibration(void);
+void do_mag_calibration(void);
+void do_rc_calibration(void);
+void do_airspeed_calibration(void);
+
+
+void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety);
int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
@@ -168,7 +195,7 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u
/**
* Print the correct usage.
*/
-static void usage(const char *reason);
+void usage(const char *reason);
/**
* Sort calibration values.
@@ -180,7 +207,7 @@ static void usage(const char *reason);
*/
// static void cal_bsort(float a[], int n);
-static int buzzer_init()
+int buzzer_init()
{
buzzer = open("/dev/tone_alarm", O_WRONLY);
@@ -192,13 +219,13 @@ static int buzzer_init()
return 0;
}
-static void buzzer_deinit()
+void buzzer_deinit()
{
close(buzzer);
}
-static int led_init()
+int led_init()
{
leds = open(LED_DEVICE_PATH, 0);
@@ -215,12 +242,12 @@ static int led_init()
return 0;
}
-static void led_deinit()
+void led_deinit()
{
close(leds);
}
-static int led_toggle(int led)
+int led_toggle(int led)
{
static int last_blue = LED_ON;
static int last_amber = LED_ON;
@@ -232,68 +259,61 @@ static int led_toggle(int led)
return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
}
-static int led_on(int led)
+int led_on(int led)
{
return ioctl(leds, LED_ON, led);
}
-static int led_off(int led)
+int led_off(int led)
{
return ioctl(leds, LED_OFF, led);
}
-enum AUDIO_PATTERN {
- AUDIO_PATTERN_ERROR = 2,
- AUDIO_PATTERN_NOTIFY_POSITIVE = 3,
- AUDIO_PATTERN_NOTIFY_NEUTRAL = 4,
- AUDIO_PATTERN_NOTIFY_NEGATIVE = 5,
- AUDIO_PATTERN_NOTIFY_CHARGE = 6
-};
-int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state)
+void tune_error()
{
-
- /* Trigger alarm if going into any error state */
- if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) ||
- ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) {
- ioctl(buzzer, TONE_SET_ALARM, 0);
- ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR);
- }
-
- /* Trigger neutral on arming / disarming */
- if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) {
- ioctl(buzzer, TONE_SET_ALARM, 0);
- ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL);
- }
-
- /* Trigger Tetris on being bored */
-
- return 0;
+ ioctl(buzzer, TONE_SET_ALARM, 2);
}
-void tune_confirm(void)
+void tune_positive()
{
ioctl(buzzer, TONE_SET_ALARM, 3);
}
-void tune_error(void)
+void tune_neutral()
{
ioctl(buzzer, TONE_SET_ALARM, 4);
}
-void do_rc_calibration(int status_pub, struct vehicle_status_s *status)
+void tune_negative()
{
- if (current_status.offboard_control_signal_lost) {
- mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
- return;
- }
+ ioctl(buzzer, TONE_SET_ALARM, 5);
+}
+
+void do_reboot()
+{
+ mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
+ usleep(500000);
+ up_systemreset();
+ /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
+}
+
+
+void do_rc_calibration()
+{
+ mavlink_log_info(mavlink_fd, "trim calibration starting");
+
+ /* XXX fix this */
+ // if (current_status.offboard_control_signal_lost) {
+ // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
+ // return;
+ // }
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp;
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
-
float p = sp.roll;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.pitch;
@@ -302,22 +322,21 @@ void do_rc_calibration(int status_pub, struct vehicle_status_s *status)
param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */
- /* auto-save to EEPROM */
+ /* auto-save */
int save_ret = param_save_default();
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
}
+ tune_positive();
+
mavlink_log_info(mavlink_fd, "trim calibration done");
}
-void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
+void do_mag_calibration()
{
-
- /* set to mag calibration mode */
- status->flag_preflight_mag_calibration = true;
- state_machine_publish(status_pub, status, mavlink_fd);
+ mavlink_log_info(mavlink_fd, "mag calibration starting, hold still");
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
struct mag_report mag;
@@ -332,15 +351,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
/* limit update rate to get equally spaced measurements over time (in ms) */
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
- // XXX old cal
- // * FLT_MIN is not the most negative float number,
- // * but the smallest number by magnitude float can
- // * represent. Use -FLT_MAX to initialize the most
- // * negative number
-
- // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
- // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
-
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
/* erase old calibration */
@@ -386,10 +396,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
return;
}
- tune_confirm();
- sleep(2);
- tune_confirm();
-
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter < calibration_maxcount) {
@@ -403,9 +409,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
axis_index++;
char buf[50];
- sprintf(buf, "Please rotate around %c", axislabels[axis_index]);
+ sprintf(buf, "please rotate around %c", axislabels[axis_index]);
mavlink_log_info(mavlink_fd, buf);
- tune_confirm();
+ tune_neutral();
axis_deadline += calibration_interval / 3;
}
@@ -541,28 +547,19 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
mavlink_log_info(mavlink_fd, "mag calibration done");
- tune_confirm();
- sleep(2);
- tune_confirm();
- sleep(2);
+ tune_positive();
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
}
- /* disable calibration mode */
- status->flag_preflight_mag_calibration = false;
- state_machine_publish(status_pub, status, mavlink_fd);
-
close(sub_mag);
}
-void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
+void do_gyro_calibration()
{
- /* set to gyro calibration mode */
- status->flag_preflight_gyro_calibration = true;
- state_machine_publish(status_pub, status, mavlink_fd);
+ mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still");
const int calibration_count = 5000;
@@ -613,10 +610,6 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
gyro_offset[1] = gyro_offset[1] / calibration_count;
gyro_offset[2] = gyro_offset[2] / calibration_count;
- /* exit gyro calibration mode */
- status->flag_preflight_gyro_calibration = false;
- state_machine_publish(status_pub, status, mavlink_fd);
-
if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
@@ -653,10 +646,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
// mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "gyro calibration done");
- tune_confirm();
- sleep(2);
- tune_confirm();
- sleep(2);
+ tune_positive();
/* third beep by cal end routine */
} else {
@@ -666,14 +656,11 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
close(sub_sensor_combined);
}
-void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
-{
- /* announce change */
- mavlink_log_info(mavlink_fd, "keep it still");
- /* set to accel calibration mode */
- status->flag_preflight_airspeed_calibration = true;
- state_machine_publish(status_pub, status, mavlink_fd);
+void do_airspeed_calibration()
+{
+ /* give directions */
+ mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
const int calibration_count = 2500;
@@ -722,340 +709,277 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
//mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "airspeed calibration done");
- tune_confirm();
- sleep(2);
- tune_confirm();
- sleep(2);
- /* third beep by cal end routine */
+ tune_positive();
} else {
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
}
- /* exit airspeed calibration mode */
- status->flag_preflight_airspeed_calibration = false;
- state_machine_publish(status_pub, status, mavlink_fd);
-
close(diff_pres_sub);
}
-
-
-void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
+void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety)
{
/* result of the command */
uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
- /* announce command handling */
- tune_confirm();
-
-
- /* supported command handling start */
-
/* request to set different system mode */
switch (cmd->command) {
- case VEHICLE_CMD_DO_SET_MODE: {
- if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) {
- result = VEHICLE_CMD_RESULT_ACCEPTED;
+ case VEHICLE_CMD_DO_SET_MODE:
- } else {
- result = VEHICLE_CMD_RESULT_DENIED;
- }
- }
- break;
+ /* request to activate HIL */
+ if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) {
- case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
- /* request to arm */
- if ((int)cmd->param1 == 1) {
- if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+ if (OK == hil_state_transition(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
-
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
+ }
- /* request to disarm */
-
- } else if ((int)cmd->param1 == 0) {
- if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+ if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) {
+ if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
-
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
- }
- }
- break;
-
- /* request for an autopilot reboot */
- case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
- if ((int)cmd->param1 == 1) {
- if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) {
- /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
+ } else {
+ if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
-
} else {
- /* system may return here */
result = VEHICLE_CMD_RESULT_DENIED;
}
}
- }
- break;
-// /* request to land */
-// case VEHICLE_CMD_NAV_LAND:
-// {
-// //TODO: add check if landing possible
-// //TODO: add landing maneuver
-//
-// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) {
-// result = VEHICLE_CMD_RESULT_ACCEPTED;
-// } }
-// break;
-//
-// /* request to takeoff */
-// case VEHICLE_CMD_NAV_TAKEOFF:
-// {
-// //TODO: add check if takeoff possible
-// //TODO: add takeoff maneuver
-//
-// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) {
-// result = VEHICLE_CMD_RESULT_ACCEPTED;
-// }
-// }
-// break;
-//
- /* preflight calibration */
- case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
- bool handled = false;
+ break;
- /* gyro calibration */
- if ((int)(cmd->param1) == 1) {
- /* transition to calibration state */
- do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "starting gyro cal");
- tune_confirm();
- do_gyro_calibration(status_pub, &current_status);
- mavlink_log_info(mavlink_fd, "finished gyro cal");
- tune_confirm();
- do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
- result = VEHICLE_CMD_RESULT_ACCEPTED;
+ case VEHICLE_CMD_COMPONENT_ARM_DISARM:
+ /* request to arm */
+ if ((int)cmd->param1 == 1) {
+ if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) {
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
- mavlink_log_critical(mavlink_fd, "REJECTING gyro cal");
result = VEHICLE_CMD_RESULT_DENIED;
}
-
- handled = true;
- }
-
- /* magnetometer calibration */
- if ((int)(cmd->param2) == 1) {
- /* transition to calibration state */
- do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "starting mag cal");
- tune_confirm();
- do_mag_calibration(status_pub, &current_status);
- mavlink_log_info(mavlink_fd, "finished mag cal");
- tune_confirm();
- do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
+ /* request to disarm */
+ } else if ((int)cmd->param1 == 0) {
+ if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
-
} else {
- mavlink_log_critical(mavlink_fd, "REJECTING mag cal");
result = VEHICLE_CMD_RESULT_DENIED;
}
-
- handled = true;
}
- /* zero-altitude pressure calibration */
- if ((int)(cmd->param3) == 1) {
- /* transition to calibration state */
- do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
+ break;
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
- tune_confirm();
+ case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
- } else {
- mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
- result = VEHICLE_CMD_RESULT_DENIED;
- }
+ /* request for an autopilot reboot */
+ if ((int)cmd->param1 == 1) {
+ if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, safety_pub, safety, mavlink_fd)) {
+ /* reboot is executed later, after positive tune is sent */
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ tune_positive();
+ /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
+ do_reboot();
- handled = true;
+ } else {
+ /* system may return here */
+ result = VEHICLE_CMD_RESULT_DENIED;
+ tune_negative();
+ }
+ }
}
+ break;
- /* trim calibration */
- if ((int)(cmd->param4) == 1) {
- /* transition to calibration state */
- do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "starting trim cal");
- tune_confirm();
- do_rc_calibration(status_pub, &current_status);
- mavlink_log_info(mavlink_fd, "finished trim cal");
- tune_confirm();
- do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else {
- mavlink_log_critical(mavlink_fd, "REJECTING trim cal");
- result = VEHICLE_CMD_RESULT_DENIED;
- }
+ // /* request to land */
+ // case VEHICLE_CMD_NAV_LAND:
+ // {
+ // //TODO: add check if landing possible
+ // //TODO: add landing maneuver
+ //
+ // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) {
+ // result = VEHICLE_CMD_RESULT_ACCEPTED;
+ // } }
+ // break;
+ //
+ // /* request to takeoff */
+ // case VEHICLE_CMD_NAV_TAKEOFF:
+ // {
+ // //TODO: add check if takeoff possible
+ // //TODO: add takeoff maneuver
+ //
+ // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) {
+ // result = VEHICLE_CMD_RESULT_ACCEPTED;
+ // }
+ // }
+ // break;
+ //
+ /* preflight calibration */
+ case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
- handled = true;
- }
+ /* gyro calibration */
+ if ((int)(cmd->param1) == 1) {
- /* accel calibration */
- if ((int)(cmd->param5) == 1) {
- /* transition to calibration state */
- do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "CMD starting accel cal");
- tune_confirm();
- do_accel_calibration(status_pub, &current_status, mavlink_fd);
- tune_confirm();
- mavlink_log_info(mavlink_fd, "CMD finished accel cal");
- do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
- result = VEHICLE_CMD_RESULT_ACCEPTED;
+ /* check if no other task is scheduled */
+ if(low_prio_task == LOW_PRIO_TASK_NONE) {
+ /* try to go to INIT/PREFLIGHT arming state */
+ if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ /* now set the task for the low prio thread */
+ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION;
+ } else {
+ result = VEHICLE_CMD_RESULT_DENIED;
+ }
} else {
- mavlink_log_critical(mavlink_fd, "REJECTING accel cal");
- result = VEHICLE_CMD_RESULT_DENIED;
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
-
- handled = true;
}
- /* airspeed calibration */
- if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol
- /* transition to calibration state */
- do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "CMD starting airspeed cal");
- tune_confirm();
- do_airspeed_calibration(status_pub, &current_status);
- tune_confirm();
- mavlink_log_info(mavlink_fd, "CMD finished airspeed cal");
- do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
- result = VEHICLE_CMD_RESULT_ACCEPTED;
+ /* magnetometer calibration */
+ if ((int)(cmd->param2) == 1) {
+ /* check if no other task is scheduled */
+ if(low_prio_task == LOW_PRIO_TASK_NONE) {
+
+ /* try to go to INIT/PREFLIGHT arming state */
+ if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ /* now set the task for the low prio thread */
+ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION;
+ } else {
+ result = VEHICLE_CMD_RESULT_DENIED;
+ }
} else {
- mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal");
- result = VEHICLE_CMD_RESULT_DENIED;
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
-
- handled = true;
}
- /* none found */
- if (!handled) {
- //warnx("refusing unsupported calibration request\n");
- mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request");
- result = VEHICLE_CMD_RESULT_UNSUPPORTED;
- }
- }
- break;
- case VEHICLE_CMD_PREFLIGHT_STORAGE: {
- if (current_status.flag_system_armed &&
- ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
- (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
- (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) {
- /* do not perform expensive memory tasks on multirotors in flight */
- // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed
- mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed");
+ // /* zero-altitude pressure calibration */
+ // if ((int)(cmd->param3) == 1) {
- } else {
+ // /* check if no other task is scheduled */
+ // if(low_prio_task == LOW_PRIO_TASK_NONE) {
- // XXX move this to LOW PRIO THREAD of commander app
- /* Read all parameters from EEPROM to RAM */
+ // /* try to go to INIT/PREFLIGHT arming state */
+ // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
+ // result = VEHICLE_CMD_RESULT_ACCEPTED;
+ // /* now set the task for the low prio thread */
+ // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION;
+ // } else {
+ // result = VEHICLE_CMD_RESULT_DENIED;
+ // }
+ // } else {
+ // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ // }
+ // }
- if (((int)(cmd->param1)) == 0) {
- /* read all parameters from EEPROM to RAM */
- int read_ret = param_load_default();
- if (read_ret == OK) {
- //warnx("[mavlink pm] Loaded EEPROM params in RAM\n");
- mavlink_log_info(mavlink_fd, "OK loading params from");
- mavlink_log_info(mavlink_fd, param_get_default_file());
- result = VEHICLE_CMD_RESULT_ACCEPTED;
+ // /* trim calibration */
+ // if ((int)(cmd->param4) == 1) {
- } else if (read_ret == 1) {
- mavlink_log_info(mavlink_fd, "OK no changes in");
- mavlink_log_info(mavlink_fd, param_get_default_file());
- result = VEHICLE_CMD_RESULT_ACCEPTED;
+ // /* check if no other task is scheduled */
+ // if(low_prio_task == LOW_PRIO_TASK_NONE) {
- } else {
- if (read_ret < -1) {
- mavlink_log_info(mavlink_fd, "ERR loading params from");
- mavlink_log_info(mavlink_fd, param_get_default_file());
+ // /* try to go to INIT/PREFLIGHT arming state */
+ // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
+ // result = VEHICLE_CMD_RESULT_ACCEPTED;
+ // /* now set the task for the low prio thread */
+ // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION;
+ // } else {
+ // result = VEHICLE_CMD_RESULT_DENIED;
+ // }
+ // } else {
+ // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ // }
+ // }
- } else {
- mavlink_log_info(mavlink_fd, "ERR no param file named");
- mavlink_log_info(mavlink_fd, param_get_default_file());
- }
- result = VEHICLE_CMD_RESULT_FAILED;
+ /* accel calibration */
+ if ((int)(cmd->param5) == 1) {
+
+ /* check if no other task is scheduled */
+ if(low_prio_task == LOW_PRIO_TASK_NONE) {
+
+ /* try to go to INIT/PREFLIGHT arming state */
+ if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ /* now set the task for the low prio thread */
+ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION;
+ } else {
+ result = VEHICLE_CMD_RESULT_DENIED;
}
+ } else {
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ }
+ }
- } else if (((int)(cmd->param1)) == 1) {
+ /* airspeed calibration */
+ if ((int)(cmd->param6) == 1) {
- /* write all parameters from RAM to EEPROM */
- int write_ret = param_save_default();
+ /* check if no other task is scheduled */
+ if(low_prio_task == LOW_PRIO_TASK_NONE) {
- if (write_ret == OK) {
- mavlink_log_info(mavlink_fd, "OK saved param file");
- mavlink_log_info(mavlink_fd, param_get_default_file());
+ /* try to go to INIT/PREFLIGHT arming state */
+ if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
-
+ /* now set the task for the low prio thread */
+ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION;
} else {
- if (write_ret < -1) {
- mavlink_log_info(mavlink_fd, "ERR params file does not exit:");
- mavlink_log_info(mavlink_fd, param_get_default_file());
+ result = VEHICLE_CMD_RESULT_DENIED;
+ }
+ } else {
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ }
+ }
+ break;
- } else {
- mavlink_log_info(mavlink_fd, "ERR writing params to");
- mavlink_log_info(mavlink_fd, param_get_default_file());
- }
+ case VEHICLE_CMD_PREFLIGHT_STORAGE:
- result = VEHICLE_CMD_RESULT_FAILED;
- }
+ if (((int)(cmd->param1)) == 0) {
+
+ /* check if no other task is scheduled */
+ if(low_prio_task == LOW_PRIO_TASK_NONE) {
+ low_prio_task = LOW_PRIO_TASK_PARAM_LOAD;
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ } else {
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ }
+
+
+ } else if (((int)(cmd->param1)) == 1) {
+ /* check if no other task is scheduled */
+ if(low_prio_task == LOW_PRIO_TASK_NONE) {
+ low_prio_task = LOW_PRIO_TASK_PARAM_SAVE;
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
- mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request");
- result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
- }
break;
- default: {
- mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
- result = VEHICLE_CMD_RESULT_UNSUPPORTED;
- /* announce command rejection */
- ioctl(buzzer, TONE_SET_ALARM, 4);
- }
+ default:
+ mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
+ result = VEHICLE_CMD_RESULT_UNSUPPORTED;
break;
}
/* supported command handling stop */
if (result == VEHICLE_CMD_RESULT_FAILED ||
result == VEHICLE_CMD_RESULT_DENIED ||
- result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
- ioctl(buzzer, TONE_SET_ALARM, 5);
+ result == VEHICLE_CMD_RESULT_UNSUPPORTED ||
+ result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
+
+ tune_negative();
} else if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
- tune_confirm();
+
+ tune_positive();
}
/* send any requested ACKs */
@@ -1066,7 +990,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
}
-static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal
+void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal
{
/* Set thread name */
prctl(PR_SET_NAME, "commander orb rcv", getpid());
@@ -1169,8 +1093,7 @@ float battery_remaining_estimate_voltage(float voltage)
return ret;
}
-static void
-usage(const char *reason)
+void usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
@@ -1245,62 +1168,100 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_component_id = param_find("MAV_COMP_ID");
/* welcome user */
- warnx("I am in command now!\n");
+ warnx("[commander] starting");
/* pthreads for command and subsystem info handling */
// pthread_t command_handling_thread;
pthread_t subsystem_info_thread;
+ pthread_t commander_low_prio_thread;
/* initialize */
if (led_init() != 0) {
- warnx("ERROR: Failed to initialize leds\n");
+ warnx("ERROR: Failed to initialize leds");
}
if (buzzer_init() != 0) {
- warnx("ERROR: Failed to initialize buzzer\n");
+ warnx("ERROR: Failed to initialize buzzer");
}
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
- warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
+ warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.");
}
-
+
+ /* Main state machine */
+ struct vehicle_status_s current_status;
+ orb_advert_t status_pub;
/* make sure we are in preflight state */
memset(&current_status, 0, sizeof(current_status));
- current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
- current_status.flag_system_armed = false;
+
+ /* safety topic */
+ struct actuator_safety_s safety;
+ orb_advert_t safety_pub;
+ /* Initialize safety with all false */
+ memset(&safety, 0, sizeof(safety));
+
+
+ /* flags for control apps */
+ struct vehicle_control_mode_s control_mode;
+ orb_advert_t control_mode_pub;
+
+ /* Initialize all flags to false */
+ memset(&control_mode, 0, sizeof(control_mode));
+
+ current_status.navigation_state = NAVIGATION_STATE_INIT;
+ current_status.arming_state = ARMING_STATE_INIT;
+ current_status.hil_state = HIL_STATE_OFF;
+
/* neither manual nor offboard control commands have been received */
current_status.offboard_control_signal_found_once = false;
current_status.rc_signal_found_once = false;
+
/* mark all signals lost as long as they haven't been found */
current_status.rc_signal_lost = true;
current_status.offboard_control_signal_lost = true;
+
/* allow manual override initially */
current_status.flag_external_manual_override_ok = true;
+
/* flag position info as bad, do not allow auto mode */
- current_status.flag_vector_flight_mode_ok = false;
+ // current_status.flag_vector_flight_mode_ok = false;
+
/* set battery warning flag */
current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
/* set safety device detection flag */
- current_status.flag_safety_present = false;
+
+ /* XXX do we need this? */
+ //current_status.flag_safety_present = false;
+
+ // XXX for now just set sensors as initialized
+ current_status.condition_system_sensors_initialized = true;
+
+ // XXX just disable offboard control for now
+ control_mode.flag_control_offboard_enabled = false;
/* advertise to ORB */
- stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
+ status_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
/* publish current state machine */
- state_machine_publish(stat_pub, &current_status, mavlink_fd);
+ state_machine_publish(status_pub, &current_status, mavlink_fd);
+
+ safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety);
+
+ control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
/* home position */
orb_advert_t home_pub = -1;
struct home_position_s home;
memset(&home, 0, sizeof(home));
- if (stat_pub < 0) {
+ if (status_pub < 0) {
warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
warnx("exiting.");
exit(ERROR);
}
+ // XXX needed?
mavlink_log_info(mavlink_fd, "system is running");
/* create pthreads */
@@ -1309,9 +1270,18 @@ int commander_thread_main(int argc, char *argv[])
pthread_attr_setstacksize(&subsystem_info_attr, 2048);
pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, &current_status);
+ pthread_attr_t commander_low_prio_attr;
+ pthread_attr_init(&commander_low_prio_attr);
+ pthread_attr_setstacksize(&commander_low_prio_attr, 2048);
+
+ struct sched_param param;
+ /* low priority */
+ param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
+ (void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
+ pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
+
/* Start monitoring loop */
uint16_t counter = 0;
- uint8_t flight_env;
/* Initialize to 0.0V */
float battery_voltage = 0.0f;
@@ -1320,12 +1290,14 @@ int commander_thread_main(int argc, char *argv[])
bool critical_battery_voltage_actions_done = false;
uint8_t low_voltage_counter = 0;
uint16_t critical_voltage_counter = 0;
- int16_t mode_switch_rc_value;
float bat_remain = 1.0f;
uint16_t stick_off_counter = 0;
uint16_t stick_on_counter = 0;
+ /* XXX what exactly is this for? */
+ uint64_t last_print_time = 0;
+
/* Subscribe to manual control data */
int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp_man;
@@ -1336,11 +1308,13 @@ int commander_thread_main(int argc, char *argv[])
struct offboard_control_setpoint_s sp_offboard;
memset(&sp_offboard, 0, sizeof(sp_offboard));
+ /* Subscribe to global position */
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));
uint64_t last_global_position_time = 0;
+ /* Subscribe to local position data */
int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
struct vehicle_local_position_s local_position;
memset(&local_position, 0, sizeof(local_position));
@@ -1351,10 +1325,13 @@ int commander_thread_main(int argc, char *argv[])
* position estimator and commander. RAW GPS is more than good enough for a
* non-flying vehicle.
*/
+
+ /* Subscribe to GPS topic */
int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
struct vehicle_gps_position_s gps_position;
memset(&gps_position, 0, sizeof(gps_position));
+ /* Subscribe to sensor topic */
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s sensors;
memset(&sensors, 0, sizeof(sensors));
@@ -1380,12 +1357,6 @@ int commander_thread_main(int argc, char *argv[])
memset(&battery, 0, sizeof(battery));
battery.voltage_v = 0.0f;
- /* subscribe to safety topic */
- int safety_sub = orb_subscribe(ORB_ID(safety));
- struct safety_s safety;
- memset(&safety, 0, sizeof(safety));
- safety.status = SAFETY_STATUS_NOT_PRESENT;
-
// uint8_t vehicle_state_previous = current_status.state_machine;
float voltage_previous = 0.0f;
@@ -1396,7 +1367,9 @@ int commander_thread_main(int argc, char *argv[])
thread_running = true;
uint64_t start_time = hrt_absolute_time();
- uint64_t failsave_ll_start_time = 0;
+
+ /* XXX use this! */
+ //uint64_t failsave_ll_start_time = 0;
bool state_changed = true;
bool param_init_forced = true;
@@ -1416,7 +1389,7 @@ int commander_thread_main(int argc, char *argv[])
/* update parameters */
- if (!current_status.flag_system_armed) {
+ if (!safety.armed) {
if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
warnx("failed setting new system type");
}
@@ -1470,20 +1443,38 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
- handle_command(stat_pub, &current_status, &cmd);
+ handle_command(status_pub, &current_status, &cmd, safety_pub, &safety);
}
+
- orb_check(safety_sub, &new_data);
+ /* update parameters */
+ orb_check(param_changed_sub, &new_data);
- if (new_data) {
- /* got safety change */
- orb_copy(ORB_ID(safety), safety_sub, &safety);
+ if (new_data || param_init_forced) {
+ param_init_forced = false;
+ /* parameters changed */
+ orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
- /* handle it */
- current_status.flag_safety_present = (safety.status != SAFETY_STATUS_NOT_PRESENT);
-
- if (current_status.flag_safety_present)
- current_status.flag_safety_safe = (safety.status == SAFETY_STATUS_SAFE);
+ /* update parameters */
+ if (!safety.armed) {
+ if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
+ warnx("failed setting new system type");
+ }
+
+ /* disable manual override for all systems that rely on electronic stabilization */
+ if (current_status.system_type == VEHICLE_TYPE_QUADROTOR ||
+ current_status.system_type == VEHICLE_TYPE_HEXAROTOR ||
+ current_status.system_type == VEHICLE_TYPE_OCTOROTOR) {
+ current_status.flag_external_manual_override_ok = false;
+
+ } else {
+ current_status.flag_external_manual_override_ok = true;
+ }
+
+ /* check and update system / component ID */
+ param_get(_param_system_id, &(current_status.system_id));
+ param_get(_param_component_id, &(current_status.component_id));
+ }
}
/* update global position estimate */
@@ -1504,6 +1495,13 @@ int commander_thread_main(int argc, char *argv[])
last_local_position_time = local_position.timestamp;
}
+ /* set the condition to valid if there has recently been a local position estimate */
+ if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) {
+ current_status.condition_local_position_valid = true;
+ } else {
+ current_status.condition_local_position_valid = false;
+ }
+
/* update battery status */
orb_check(battery_sub, &new_data);
if (new_data) {
@@ -1522,10 +1520,11 @@ int commander_thread_main(int argc, char *argv[])
/* Slow but important 8 Hz checks */
if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
- /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
- if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
+
+ /* XXX if armed */
+ if ((false /*current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
current_status.state_machine == SYSTEM_STATE_AUTO ||
- current_status.state_machine == SYSTEM_STATE_MANUAL)) {
+ current_status.state_machine == SYSTEM_STATE_MANUAL*/)) {
/* armed, solid */
led_on(LED_AMBER);
@@ -1560,26 +1559,18 @@ int commander_thread_main(int argc, char *argv[])
} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
/* toggle arming (red) at 5 Hz on low battery or error */
led_toggle(LED_AMBER);
-
- } else {
- // /* Constant error indication in standby mode without GPS */
- // if (!current_status.gps_valid) {
- // led_on(LED_AMBER);
-
- // } else {
- // led_off(LED_AMBER);
- // }
}
- if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
- /* compute system load */
- uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
+ }
- if (last_idle_time > 0)
- current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle
+ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+ /* compute system load */
+ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
- last_idle_time = system_load.tasks[0].total_runtime;
- }
+ if (last_idle_time > 0)
+ current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle
+
+ last_idle_time = system_load.tasks[0].total_runtime;
}
// // XXX Export patterns and threshold to parameters
@@ -1595,7 +1586,7 @@ int commander_thread_main(int argc, char *argv[])
} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) {
/* For less than 20%, start be slightly annoying at 1 Hz */
ioctl(buzzer, TONE_SET_ALARM, 0);
- tune_confirm();
+ tune_positive();
} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) {
ioctl(buzzer, TONE_SET_ALARM, 0);
@@ -1622,13 +1613,14 @@ int commander_thread_main(int argc, char *argv[])
low_voltage_counter++;
}
- /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
+ /* Critical, this is rather an emergency, change state machine */
else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
- state_machine_emergency(stat_pub, &current_status, mavlink_fd);
+ // XXX implement this
+ // state_machine_emergency(status_pub, &current_status, mavlink_fd);
}
critical_voltage_counter++;
@@ -1640,6 +1632,14 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */
+ /* If in INIT state, try to proceed to STANDBY state */
+ if (current_status.arming_state == ARMING_STATE_INIT) {
+ // XXX check for sensors
+ arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
+ } else {
+ // XXX: Add emergency stuff if sensors are lost
+ }
+
/*
* Check for valid position information.
@@ -1651,53 +1651,53 @@ int commander_thread_main(int argc, char *argv[])
*/
/* store current state to reason later about a state change */
- bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
- bool global_pos_valid = current_status.flag_global_position_valid;
- bool local_pos_valid = current_status.flag_local_position_valid;
- bool airspeed_valid = current_status.flag_airspeed_valid;
+ // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
+ bool global_pos_valid = current_status.condition_global_position_valid;
+ bool local_pos_valid = current_status.condition_local_position_valid;
+ bool airspeed_valid = current_status.condition_airspeed_valid;
+
/* check for global or local position updates, set a timeout of 2s */
if (hrt_absolute_time() - last_global_position_time < 2000000) {
- current_status.flag_global_position_valid = true;
+ current_status.condition_global_position_valid = true;
// XXX check for controller status and home position as well
} else {
- current_status.flag_global_position_valid = false;
+ current_status.condition_global_position_valid = false;
}
if (hrt_absolute_time() - last_local_position_time < 2000000) {
- current_status.flag_local_position_valid = true;
+ current_status.condition_local_position_valid = true;
// XXX check for controller status and home position as well
} else {
- current_status.flag_local_position_valid = false;
+ current_status.condition_local_position_valid = false;
}
/* Check for valid airspeed/differential pressure measurements */
if (hrt_absolute_time() - last_diff_pres_time < 2000000) {
- current_status.flag_airspeed_valid = true;
+ current_status.condition_airspeed_valid = true;
} else {
- current_status.flag_airspeed_valid = false;
+ current_status.condition_airspeed_valid = false;
}
/*
* Consolidate global position and local position valid flags
* for vector flight mode.
*/
- if (current_status.flag_local_position_valid ||
- current_status.flag_global_position_valid) {
- current_status.flag_vector_flight_mode_ok = true;
+ // if (current_status.condition_local_position_valid ||
+ // current_status.condition_global_position_valid) {
+ // current_status.flag_vector_flight_mode_ok = true;
- } else {
- current_status.flag_vector_flight_mode_ok = false;
- }
+ // } else {
+ // current_status.flag_vector_flight_mode_ok = false;
+ // }
/* consolidate state change, flag as changed if required */
- if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
- global_pos_valid != current_status.flag_global_position_valid ||
- local_pos_valid != current_status.flag_local_position_valid ||
- airspeed_valid != current_status.flag_airspeed_valid) {
+ if (global_pos_valid != current_status.condition_global_position_valid ||
+ local_pos_valid != current_status.condition_local_position_valid ||
+ airspeed_valid != current_status.condition_airspeed_valid) {
state_changed = true;
}
@@ -1711,20 +1711,21 @@ int commander_thread_main(int argc, char *argv[])
* 2) The system has not aquired position lock before
* 3) The system is not armed (on the ground)
*/
- if (!current_status.flag_valid_launch_position &&
- !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok &&
- !current_status.flag_system_armed) {
- /* first time a valid position, store it and emit it */
-
- // XXX implement storage and publication of RTL position
- current_status.flag_valid_launch_position = true;
- current_status.flag_auto_flight_mode_ok = true;
- state_changed = true;
- }
+ // if (!current_status.flag_valid_launch_position &&
+ // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok &&
+ // !current_status.flag_system_armed) {
+ // first time a valid position, store it and emit it
+
+ // // XXX implement storage and publication of RTL position
+ // current_status.flag_valid_launch_position = true;
+ // current_status.flag_auto_flight_mode_ok = true;
+ // state_changed = true;
+ // }
orb_check(ORB_ID(vehicle_gps_position), &new_data);
if (new_data) {
+
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
/* check for first, long-term and valid GPS lock -> set home position */
@@ -1748,10 +1749,10 @@ int commander_thread_main(int argc, char *argv[])
if (gps_position.fix_type == GPS_FIX_TYPE_3D
&& (hdop_m < hdop_threshold_m)
- && (vdop_m < vdop_threshold_m)
+ && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk
&& !home_position_set
&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
- && !current_status.flag_system_armed) {
+ && !safety.armed) {
warnx("setting home position");
/* copy position data to uORB home message, store it locally as well */
@@ -1774,7 +1775,7 @@ int commander_thread_main(int argc, char *argv[])
/* mark home position as set */
home_position_set = true;
- tune_confirm();
+ tune_positive();
}
}
@@ -1784,100 +1785,266 @@ int commander_thread_main(int argc, char *argv[])
if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
- // /*
- // * Check if manual control modes have to be switched
- // */
- // if (!isfinite(sp_man.manual_mode_switch)) {
- // warnx("man mode sw not finite\n");
-
- // /* this switch is not properly mapped, set default */
- // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
- // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
- // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
- // /* assuming a rotary wing, fall back to SAS */
- // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
- // current_status.flag_control_attitude_enabled = true;
- // current_status.flag_control_rates_enabled = true;
- // } else {
-
- // /* assuming a fixed wing, fall back to direct pass-through */
- // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
- // current_status.flag_control_attitude_enabled = false;
- // current_status.flag_control_rates_enabled = false;
- // }
+ /*
+ * Check if manual control modes have to be switched
+ */
+ if (!isfinite(sp_man.mode_switch)) {
- // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) {
+ warnx("mode sw not finite");
+ /* no valid stick position, go to default */
+ current_status.mode_switch = MODE_SWITCH_MANUAL;
- // /* bottom stick position, set direct mode for vehicles supporting it */
- // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
- // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
- // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
+ } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) {
- // /* assuming a rotary wing, fall back to SAS */
- // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
- // current_status.flag_control_attitude_enabled = true;
- // current_status.flag_control_rates_enabled = true;
- // } else {
+ /* bottom stick position, go to manual mode */
+ current_status.mode_switch = MODE_SWITCH_MANUAL;
- // /* assuming a fixed wing, set to direct pass-through as requested */
- // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
- // current_status.flag_control_attitude_enabled = false;
- // current_status.flag_control_rates_enabled = false;
- // }
+ } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) {
- // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) {
+ /* top stick position, set auto/mission for all vehicle types */
+ current_status.mode_switch = MODE_SWITCH_AUTO;
- // /* top stick position, set SAS for all vehicle types */
- // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
- // current_status.flag_control_attitude_enabled = true;
- // current_status.flag_control_rates_enabled = true;
+ } else {
- // } else {
- // /* center stick position, set rate control */
- // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
- // current_status.flag_control_attitude_enabled = false;
- // current_status.flag_control_rates_enabled = true;
- // }
+ /* center stick position, set seatbelt/simple control */
+ current_status.mode_switch = MODE_SWITCH_SEATBELT;
+ }
// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
/*
- * Check if manual stability control modes have to be switched
- */
- if (!isfinite(sp_man.manual_sas_switch)) {
+ * Check if land/RTL is requested
+ */
+ if (!isfinite(sp_man.return_switch)) {
/* this switch is not properly mapped, set default */
- current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
+ current_status.return_switch = RETURN_SWITCH_NONE;
- } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
+ } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, set altitude hold */
- current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
+ current_status.return_switch = RETURN_SWITCH_NONE;
- } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
+ } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) {
/* top stick position */
- current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
+ current_status.return_switch = RETURN_SWITCH_RETURN;
} else {
/* center stick position, set default */
- current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
+ current_status.return_switch = RETURN_SWITCH_NONE;
+ }
+
+ /* check mission switch */
+ if (!isfinite(sp_man.mission_switch)) {
+
+ /* this switch is not properly mapped, set default */
+ current_status.mission_switch = MISSION_SWITCH_NONE;
+
+ } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) {
+
+ /* top switch position */
+ current_status.mission_switch = MISSION_SWITCH_MISSION;
+
+ } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) {
+
+ /* bottom switch position */
+ current_status.mission_switch = MISSION_SWITCH_NONE;
+
+ } else {
+
+ /* center switch position, set default */
+ current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default?
+ }
+
+ /* Now it's time to handle the stick inputs */
+
+ switch (current_status.arming_state) {
+
+ /* evaluate the navigation state when disarmed */
+ case ARMING_STATE_STANDBY:
+
+ /* just manual, XXX this might depend on the return switch */
+ if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
+
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // These is not supposed to happen
+ warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
+ }
+
+ /* Try seatbelt or fallback to manual */
+ } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) {
+
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // fallback to MANUAL_STANDBY
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // These is not supposed to happen
+ warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
+ }
+ }
+
+ /* Try auto or fallback to seatbelt or even manual */
+ } else if (current_status.mode_switch == MODE_SWITCH_AUTO) {
+
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // first fallback to SEATBELT_STANDY
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // or fallback to MANUAL_STANDBY
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // These is not supposed to happen
+ warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
+ }
+ }
+ }
+ }
+
+ break;
+
+ /* evaluate the navigation state when armed */
+ case ARMING_STATE_ARMED:
+
+ /* Always accept manual mode */
+ if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
+
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // These is not supposed to happen
+ warnx("ERROR: Navigation state MANUAL rejected");
+ }
+
+ /* SEATBELT_STANDBY (fallback: MANUAL) */
+ } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT
+ && current_status.return_switch == RETURN_SWITCH_NONE) {
+
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // fallback to MANUAL_STANDBY
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // These is not supposed to happen
+ warnx("ERROR: Navigation state MANUAL rejected");
+ }
+ }
+
+ /* SEATBELT_DESCENT (fallback: MANUAL) */
+ } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT
+ && current_status.return_switch == RETURN_SWITCH_RETURN) {
+
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // fallback to MANUAL_STANDBY
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // These is not supposed to happen
+ warnx("ERROR: Navigation state MANUAL rejected");
+ }
+ }
+
+ /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */
+ } else if (current_status.mode_switch == MODE_SWITCH_AUTO
+ && current_status.return_switch == RETURN_SWITCH_NONE
+ && current_status.mission_switch == MISSION_SWITCH_NONE) {
+
+ /* we might come from the disarmed state AUTO_STANDBY */
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // fallback to MANUAL_STANDBY
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // These is not supposed to happen
+ warnx("ERROR: Navigation state MANUAL rejected");
+ }
+ }
+ /* or from some other armed state like SEATBELT or MANUAL */
+ } else if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // fallback to MANUAL_STANDBY
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // These is not supposed to happen
+ warnx("ERROR: Navigation state MANUAL rejected");
+ }
+ }
+ }
+
+ /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */
+ } else if (current_status.mode_switch == MODE_SWITCH_AUTO
+ && current_status.return_switch == RETURN_SWITCH_NONE
+ && current_status.mission_switch == MISSION_SWITCH_MISSION) {
+
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // fallback to MANUAL_STANDBY
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // These is not supposed to happen
+ warnx("ERROR: Navigation state MANUAL rejected");
+ }
+ }
+ }
+
+ /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */
+ } else if (current_status.mode_switch == MODE_SWITCH_AUTO
+ && current_status.return_switch == RETURN_SWITCH_RETURN
+ && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) {
+
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // fallback to MANUAL_STANDBY
+ if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+ // These is not supposed to happen
+ warnx("ERROR: Navigation state MANUAL rejected");
+ }
+ }
+ }
+ }
+ break;
+
+ // XXX we might be missing something that triggers a transition from RTL to LAND
+
+ case ARMING_STATE_ARMED_ERROR:
+
+ // XXX work out fail-safe scenarios
+ break;
+
+ case ARMING_STATE_STANDBY_ERROR:
+
+ // XXX work out fail-safe scenarios
+ break;
+
+ case ARMING_STATE_REBOOT:
+
+ // XXX I don't think we should end up here
+ break;
+
+ case ARMING_STATE_IN_AIR_RESTORE:
+
+ // XXX not sure what to do here
+ break;
+ default:
+ break;
+ }
+ /* handle the case where RC signal was regained */
+ if (!current_status.rc_signal_found_once) {
+ current_status.rc_signal_found_once = true;
+ mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME.");
+
+ } else {
+ if (current_status.rc_signal_lost) {
+ mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
+ }
}
/*
- * Check if left stick is in lower left position --> switch to standby state.
- * Do this only for multirotors, not for fixed wing aircraft.
- */
- if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
- (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
- (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
- ) &&
- ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) &&
- (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
+ * Check if left stick is in lower left position --> switch to standby state.
+ * Do this only for multirotors, not for fixed wing aircraft.
+ */
+ // printf("checking\n");
+ if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
- update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
- stick_on_counter = 0;
+
+ if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
+ (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
+ (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
+ ) {
+ arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
+
+ } else {
+ mavlink_log_critical(mavlink_fd, "STICK DISARM not allowed");
+ }
+ stick_off_counter = 0;
} else {
stick_off_counter++;
@@ -1888,7 +2055,7 @@ int commander_thread_main(int argc, char *argv[])
/* check if left stick is in lower right position --> arm */
if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
- update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
+ arming_state_transition(status_pub, &current_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd);
stick_on_counter = 0;
} else {
@@ -1897,49 +2064,24 @@ int commander_thread_main(int argc, char *argv[])
}
}
- /* check manual override switch - switch to manual or auto mode */
- if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) {
- /* enable manual override */
- update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
-
- } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
- // /* check auto mode switch for correct mode */
- // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
- // /* enable guided mode */
- // update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
-
- // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
- // XXX hardcode to auto for now
- update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
-
- // }
-
- } else {
- /* center stick position, set SAS for all vehicle types */
- update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
- }
-
- /* handle the case where RC signal was regained */
- if (!current_status.rc_signal_found_once) {
- current_status.rc_signal_found_once = true;
- mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME.");
-
- } else {
- if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
- }
-
current_status.rc_signal_cutting_off = false;
current_status.rc_signal_lost = false;
current_status.rc_signal_lost_interval = 0;
} else {
- static uint64_t last_print_time = 0;
/* print error message for first RC glitch and then every 5 s / 5000 ms) */
if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) {
/* only complain if the offboard control is NOT active */
current_status.rc_signal_cutting_off = true;
mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!");
+
+ if (!current_status.rc_signal_cutting_off) {
+ printf("Reason: not rc_signal_cutting_off\n");
+ } else {
+ printf("last print time: %llu\n", last_print_time);
+ }
+
last_print_time = hrt_absolute_time();
}
@@ -1971,60 +2113,60 @@ int commander_thread_main(int argc, char *argv[])
if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) {
if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {
- /* decide about attitude control flag, enable in att/pos/vel */
- bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
- sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
- sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
+ // /* decide about attitude control flag, enable in att/pos/vel */
+ // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
+ // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
+ // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
- /* decide about rate control flag, enable it always XXX (for now) */
- bool rates_ctrl_enabled = true;
+ // /* decide about rate control flag, enable it always XXX (for now) */
+ // bool rates_ctrl_enabled = true;
- /* set up control mode */
- if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
- current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
- state_changed = true;
- }
+ // /* set up control mode */
+ // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
+ // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
+ // state_changed = true;
+ // }
- if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
- current_status.flag_control_rates_enabled = rates_ctrl_enabled;
- state_changed = true;
- }
+ // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
+ // current_status.flag_control_rates_enabled = rates_ctrl_enabled;
+ // state_changed = true;
+ // }
- /* handle the case where offboard control signal was regained */
- if (!current_status.offboard_control_signal_found_once) {
- current_status.offboard_control_signal_found_once = true;
- /* enable offboard control, disable manual input */
- current_status.flag_control_manual_enabled = false;
- current_status.flag_control_offboard_enabled = true;
- state_changed = true;
- tune_confirm();
+ // /* handle the case where offboard control signal was regained */
+ // if (!current_status.offboard_control_signal_found_once) {
+ // current_status.offboard_control_signal_found_once = true;
+ // /* enable offboard control, disable manual input */
+ // current_status.flag_control_manual_enabled = false;
+ // current_status.flag_control_offboard_enabled = true;
+ // state_changed = true;
+ // tune_positive();
- mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST");
+ // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST");
- } else {
- if (current_status.offboard_control_signal_lost) {
- mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL");
- state_changed = true;
- tune_confirm();
- }
- }
+ // } else {
+ // if (current_status.offboard_control_signal_lost) {
+ // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL");
+ // state_changed = true;
+ // tune_positive();
+ // }
+ // }
current_status.offboard_control_signal_weak = false;
current_status.offboard_control_signal_lost = false;
current_status.offboard_control_signal_lost_interval = 0;
+ // XXX check if this is correct
/* arm / disarm on request */
- if (sp_offboard.armed && !current_status.flag_system_armed) {
- update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
- /* switch to stabilized mode = takeoff */
- update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
+ if (sp_offboard.armed && !safety.armed) {
+
+ arming_state_transition(status_pub, &current_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd);
+
+ } else if (!sp_offboard.armed && safety.armed) {
- } else if (!sp_offboard.armed && current_status.flag_system_armed) {
- update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
+ arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
}
} else {
- static uint64_t last_print_time = 0;
/* print error message for first RC glitch and then every 5 s / 5000 ms) */
if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) {
@@ -2040,7 +2182,7 @@ int commander_thread_main(int argc, char *argv[])
if (current_status.offboard_control_signal_lost_interval > 100000) {
current_status.offboard_control_signal_lost = true;
current_status.failsave_lowlevel_start_time = hrt_absolute_time();
- tune_confirm();
+ tune_positive();
/* kill motors after timeout */
if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) {
@@ -2056,25 +2198,28 @@ int commander_thread_main(int argc, char *argv[])
current_status.timestamp = hrt_absolute_time();
+ // XXX this is missing
/* If full run came back clean, transition to standby */
- if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
- current_status.flag_preflight_gyro_calibration == false &&
- current_status.flag_preflight_mag_calibration == false &&
- current_status.flag_preflight_accel_calibration == false) {
- /* All ok, no calibration going on, go to standby */
- do_state_update(stat_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
- }
+ // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
+ // current_status.flag_preflight_gyro_calibration == false &&
+ // current_status.flag_preflight_mag_calibration == false &&
+ // current_status.flag_preflight_accel_calibration == false) {
+ // /* All ok, no calibration going on, go to standby */
+ // do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
+ // }
/* publish at least with 1 Hz */
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) {
- publish_armed_status(&current_status);
- orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
+
+ orb_publish(ORB_ID(vehicle_status), status_pub, &current_status);
state_changed = false;
}
+
+
/* Store old modes to detect and act on state transitions */
voltage_previous = current_status.voltage_battery;
-
+ /* XXX use this voltage_previous */
fflush(stdout);
counter++;
usleep(COMMANDER_MONITORING_INTERVAL);
@@ -2083,6 +2228,7 @@ int commander_thread_main(int argc, char *argv[])
/* wait for threads to complete */
// pthread_join(command_handling_thread, NULL);
pthread_join(subsystem_info_thread, NULL);
+ pthread_join(commander_low_prio_thread, NULL);
/* close fds */
led_deinit();
@@ -2093,10 +2239,93 @@ int commander_thread_main(int argc, char *argv[])
close(sensor_sub);
close(cmd_sub);
- warnx("exiting..\n");
+ warnx("exiting");
fflush(stdout);
thread_running = false;
return 0;
}
+
+
+void *commander_low_prio_loop(void *arg)
+{
+ /* Set thread name */
+ prctl(PR_SET_NAME, "commander low prio", getpid());
+
+ while (!thread_should_exit) {
+
+ switch (low_prio_task) {
+
+ case LOW_PRIO_TASK_PARAM_LOAD:
+
+ if (0 == param_load_default()) {
+ mavlink_log_info(mavlink_fd, "Param load success");
+ } else {
+ mavlink_log_critical(mavlink_fd, "Param load ERROR");
+ tune_error();
+ }
+ low_prio_task = LOW_PRIO_TASK_NONE;
+ break;
+
+ case LOW_PRIO_TASK_PARAM_SAVE:
+
+ if (0 == param_save_default()) {
+ mavlink_log_info(mavlink_fd, "Param save success");
+ } else {
+ mavlink_log_critical(mavlink_fd, "Param save ERROR");
+ tune_error();
+ }
+ low_prio_task = LOW_PRIO_TASK_NONE;
+ break;
+
+ case LOW_PRIO_TASK_GYRO_CALIBRATION:
+
+ do_gyro_calibration();
+
+ low_prio_task = LOW_PRIO_TASK_NONE;
+ break;
+
+ case LOW_PRIO_TASK_MAG_CALIBRATION:
+
+ do_mag_calibration();
+
+ low_prio_task = LOW_PRIO_TASK_NONE;
+ break;
+
+ case LOW_PRIO_TASK_ALTITUDE_CALIBRATION:
+
+ // do_baro_calibration();
+
+ case LOW_PRIO_TASK_RC_CALIBRATION:
+
+ // do_rc_calibration();
+
+ low_prio_task = LOW_PRIO_TASK_NONE;
+ break;
+
+ case LOW_PRIO_TASK_ACCEL_CALIBRATION:
+
+ do_accel_calibration(mavlink_fd);
+
+ low_prio_task = LOW_PRIO_TASK_NONE;
+ break;
+
+ case LOW_PRIO_TASK_AIRSPEED_CALIBRATION:
+
+ do_airspeed_calibration();
+
+ low_prio_task = LOW_PRIO_TASK_NONE;
+ break;
+
+ case LOW_PRIO_TASK_NONE:
+ default:
+ /* slow down to 10Hz */
+ usleep(100000);
+ break;
+ }
+
+ }
+
+ return 0;
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-24 03:27:59 +0000