diff options
author | px4dev <px4@purgatory.org> | 2013-04-26 16:14:32 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-04-26 16:14:32 -0700 |
commit | 01e427b17c161d8adaa38d6bdb91aecb434451f2 (patch) | |
tree | e33f4f6b78ef133c91ad92f1a413c2b16f17a5d5 /apps/commander | |
parent | ce0e4a3afd28b97d5a540e02bef86c52a335f243 (diff) | |
download | px4-firmware-01e427b17c161d8adaa38d6bdb91aecb434451f2.tar.gz px4-firmware-01e427b17c161d8adaa38d6bdb91aecb434451f2.tar.bz2 px4-firmware-01e427b17c161d8adaa38d6bdb91aecb434451f2.zip |
Merge working changes into export-build branch.
Diffstat (limited to 'apps/commander')
-rw-r--r-- | apps/commander/.context | 0 | ||||
-rw-r--r-- | apps/commander/Makefile | 45 | ||||
-rw-r--r-- | apps/commander/calibration_routines.c | 219 | ||||
-rw-r--r-- | apps/commander/calibration_routines.h | 61 | ||||
-rw-r--r-- | apps/commander/commander.c | 2181 | ||||
-rw-r--r-- | apps/commander/commander.h | 58 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 752 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.h | 209 |
8 files changed, 0 insertions, 3525 deletions
diff --git a/apps/commander/.context b/apps/commander/.context deleted file mode 100644 index e69de29bb..000000000 --- a/apps/commander/.context +++ /dev/null diff --git a/apps/commander/Makefile b/apps/commander/Makefile deleted file mode 100644 index d12697274..000000000 --- a/apps/commander/Makefile +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 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 -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Commander application -# - -APPNAME = commander -PRIORITY = SCHED_PRIORITY_MAX - 30 -STACKSIZE = 2048 - -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink - -include $(APPDIR)/mk/app.mk - diff --git a/apps/commander/calibration_routines.c b/apps/commander/calibration_routines.c deleted file mode 100644 index a26938637..000000000 --- a/apps/commander/calibration_routines.c +++ /dev/null @@ -1,219 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file calibration_routines.c - * Calibration routines implementations. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#include <math.h> - -#include "calibration_routines.h" - - -int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) -{ - - float x_sumplain = 0.0f; - float x_sumsq = 0.0f; - float x_sumcube = 0.0f; - - float y_sumplain = 0.0f; - float y_sumsq = 0.0f; - float y_sumcube = 0.0f; - - float z_sumplain = 0.0f; - float z_sumsq = 0.0f; - float z_sumcube = 0.0f; - - float xy_sum = 0.0f; - float xz_sum = 0.0f; - float yz_sum = 0.0f; - - float x2y_sum = 0.0f; - float x2z_sum = 0.0f; - float y2x_sum = 0.0f; - float y2z_sum = 0.0f; - float z2x_sum = 0.0f; - float z2y_sum = 0.0f; - - for (unsigned int i = 0; i < size; i++) { - - float x2 = x[i] * x[i]; - float y2 = y[i] * y[i]; - float z2 = z[i] * z[i]; - - x_sumplain += x[i]; - x_sumsq += x2; - x_sumcube += x2 * x[i]; - - y_sumplain += y[i]; - y_sumsq += y2; - y_sumcube += y2 * y[i]; - - z_sumplain += z[i]; - z_sumsq += z2; - z_sumcube += z2 * z[i]; - - xy_sum += x[i] * y[i]; - xz_sum += x[i] * z[i]; - yz_sum += y[i] * z[i]; - - x2y_sum += x2 * y[i]; - x2z_sum += x2 * z[i]; - - y2x_sum += y2 * x[i]; - y2z_sum += y2 * z[i]; - - z2x_sum += z2 * x[i]; - z2y_sum += z2 * y[i]; - } - - // - //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data - // - // P is a structure that has been computed with the data earlier. - // P.npoints is the number of elements; the length of X,Y,Z are identical. - // P's members are logically named. - // - // X[n] is the x component of point n - // Y[n] is the y component of point n - // Z[n] is the z component of point n - // - // A is the x coordiante of the sphere - // B is the y coordiante of the sphere - // C is the z coordiante of the sphere - // Rsq is the radius squared of the sphere. - // - //This method should converge; maybe 5-100 iterations or more. - // - float x_sum = x_sumplain / size; //sum( X[n] ) - float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) - float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) - float y_sum = y_sumplain / size; //sum( Y[n] ) - float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) - float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) - float z_sum = z_sumplain / size; //sum( Z[n] ) - float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) - float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) - - float XY = xy_sum / size; //sum( X[n] * Y[n] ) - float XZ = xz_sum / size; //sum( X[n] * Z[n] ) - float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) - float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) - float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) - float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) - float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) - float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) - float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) - - //Reduction of multiplications - float F0 = x_sum2 + y_sum2 + z_sum2; - float F1 = 0.5f * F0; - float F2 = -8.0f * (x_sum3 + Y2X + Z2X); - float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); - float F4 = -8.0f * (X2Z + Y2Z + z_sum3); - - //Set initial conditions: - float A = x_sum; - float B = y_sum; - float C = z_sum; - - //First iteration computation: - float A2 = A * A; - float B2 = B * B; - float C2 = C * C; - float QS = A2 + B2 + C2; - float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); - - //Set initial conditions: - float Rsq = F0 + QB + QS; - - //First iteration computation: - float Q0 = 0.5f * (QS - Rsq); - float Q1 = F1 + Q0; - float Q2 = 8.0f * (QS - Rsq + QB + F0); - float aA, aB, aC, nA, nB, nC, dA, dB, dC; - - //Iterate N times, ignore stop condition. - int n = 0; - - while (n < max_iterations) { - n++; - - //Compute denominator: - 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; - - //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); - nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB); - nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC); - - //Check for stop condition - dA = (nA - A); - dB = (nB - B); - dC = (nC - C); - - if ((dA * dA + dB * dB + dC * dC) <= delta) { break; } - - //Compute next iteration's values - A = nA; - B = nB; - C = nC; - A2 = A * A; - B2 = B * B; - C2 = C * C; - QS = A2 + B2 + C2; - QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); - Rsq = F0 + QB + QS; - Q0 = 0.5f * (QS - Rsq); - Q1 = F1 + Q0; - Q2 = 8.0f * (QS - Rsq + QB + F0); - } - - *sphere_x = A; - *sphere_y = B; - *sphere_z = C; - *sphere_radius = sqrtf(Rsq); - - return 0; -} diff --git a/apps/commander/calibration_routines.h b/apps/commander/calibration_routines.h deleted file mode 100644 index e3e7fbafd..000000000 --- a/apps/commander/calibration_routines.h +++ /dev/null @@ -1,61 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file calibration_routines.h - * Calibration routines definitions. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -/** - * Least-squares fit of a sphere to a set of points. - * - * Fits a sphere to a set of points on the sphere surface. - * - * @param x point coordinates on the X axis - * @param y point coordinates on the Y axis - * @param z point coordinates on the Z axis - * @param size number of points - * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100. - * @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times. - * @param sphere_x coordinate of the sphere center on the X axis - * @param sphere_y coordinate of the sphere center on the Y axis - * @param sphere_z coordinate of the sphere center on the Z axis - * @param sphere_radius sphere radius - * - * @return 0 on success, 1 on failure - */ -int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
\ No newline at end of file diff --git a/apps/commander/commander.c b/apps/commander/commander.c deleted file mode 100644 index 7c0a25f86..000000000 --- a/apps/commander/commander.c +++ /dev/null @@ -1,2181 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file commander.c - * Main system state machine implementation. - * - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * - */ - -#include "commander.h" - -#include <nuttx/config.h> -#include <pthread.h> -#include <stdio.h> -#include <stdlib.h> -#include <stdbool.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#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> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/offboard_control_setpoint.h> -#include <uORB/topics/home_position.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/vehicle_command.h> -#include <uORB/topics/subsystem_info.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/parameter_update.h> -#include <uORB/topics/differential_pressure.h> -#include <mavlink/mavlink_log.h> - -#include <systemlib/param/param.h> -#include <systemlib/systemlib.h> -#include <systemlib/err.h> - -/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ -#include <drivers/drv_accel.h> -#include <drivers/drv_gyro.h> -#include <drivers/drv_mag.h> -#include <drivers/drv_baro.h> - -#include "calibration_routines.h" - - -PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ -//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ -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 */ -#define COMMANDER_MONITORING_INTERVAL 50000 -#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) -#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define STICK_ON_OFF_LIMIT 0.75f -#define STICK_THRUST_RANGE 1.0f -#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 -#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define GPS_FIX_TYPE_2D 2 -#define GPS_FIX_TYPE_3D 3 -#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 -#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -/* 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; - -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 */ - -/* pthread loops */ -static void *orb_receive_loop(void *arg); - -__EXPORT int commander_main(int argc, char *argv[]); - -/** - * Mainloop of commander. - */ -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 do_accel_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 trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); - - - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/** - * Sort calibration values. - * - * Sorts the calibration values with bubble sort. - * - * @param a The array to sort - * @param n The number of entries in the array - */ -// static void cal_bsort(float a[], int n); - -static int buzzer_init() -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return 0; -} - -static void buzzer_deinit() -{ - close(buzzer); -} - - -static int led_init() -{ - leds = open(LED_DEVICE_PATH, 0); - - if (leds < 0) { - warnx("LED: open fail\n"); - return ERROR; - } - - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - warnx("LED: ioctl fail\n"); - return ERROR; - } - - return 0; -} - -static void led_deinit() -{ - close(leds); -} - -static int led_toggle(int led) -{ - static int last_blue = LED_ON; - static int last_amber = LED_ON; - - if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; - - if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; - - return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); -} - -static int led_on(int led) -{ - return ioctl(leds, LED_ON, led); -} - -static 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) -{ - - /* 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; -} - -void tune_confirm(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 3); -} - -void tune_error(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 4); -} - -void do_rc_calibration(int status_pub, struct vehicle_status_s *status) -{ - 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; - param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; - param_set(param_find("TRIM_YAW"), &p); - - /* store to permanent storage */ - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); - } - - mavlink_log_info(mavlink_fd, "trim calibration done"); -} - -void do_mag_calibration(int status_pub, struct vehicle_status_s *status) -{ - - /* set to mag calibration mode */ - status->flag_preflight_mag_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - struct mag_report mag; - - /* 45 seconds */ - uint64_t calibration_interval = 45 * 1000 * 1000; - - /* maximum 2000 values */ - const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; - - /* 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 */ - struct mag_scale mscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); - } - - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); - } - - close(fd); - - /* calibrate offsets */ - - // uint64_t calibration_start = hrt_absolute_time(); - - uint64_t axis_deadline = hrt_absolute_time(); - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - - const char axislabels[3] = { 'X', 'Y', 'Z'}; - int axis_index = -1; - - float *x = (float *)malloc(sizeof(float) * calibration_maxcount); - float *y = (float *)malloc(sizeof(float) * calibration_maxcount); - float *z = (float *)malloc(sizeof(float) * calibration_maxcount); - - if (x == NULL || y == NULL || z == NULL) { - warnx("mag cal failed: out of memory"); - mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - warnx("x:%p y:%p z:%p\n", x, y, z); - return; - } - - tune_confirm(); - sleep(2); - tune_confirm(); - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; - - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { - - axis_index++; - - char buf[50]; - sprintf(buf, "Please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); - tune_confirm(); - - axis_deadline += calibration_interval / 3; - } - - if (!(axis_index < 3)) { - break; - } - - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); - break; - } - } - - float sphere_x; - float sphere_y; - float sphere_z; - float sphere_radius; - - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - - free(x); - free(y); - free(z); - - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { - - fd = open(MAG_DEVICE_PATH, 0); - - struct mag_scale mscale; - - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); - - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); - - close(fd); - - /* announce and set new offset */ - - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - warnx("Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - warnx("Setting X mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - warnx("Setting Y mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - warnx("Setting Z mag scale failed!\n"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); - } - - warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); - - char buf[52]; - sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, - (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_log_info(mavlink_fd, buf); - - sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, - (double)mscale.y_scale, (double)mscale.z_scale); - mavlink_log_info(mavlink_fd, buf); - - mavlink_log_info(mavlink_fd, "mag calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* 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) -{ - /* set to gyro calibration mode */ - status->flag_preflight_gyro_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - const int calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - int calibration_counter = 0; - float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; - - /* set offsets to zero */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_rad_s[0]; - gyro_offset[1] += raw.gyro_rad_s[1]; - gyro_offset[2] += raw.gyro_rad_s[2]; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - return; - } - } - - gyro_offset[0] = gyro_offset[0] / calibration_count; - 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])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); - } - - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - // char buf[50]; - // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "gyro calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - } - - close(sub_sensor_combined); -} - -void do_accel_calibration(int status_pub, struct vehicle_status_s *status) -{ - /* announce change */ - - mavlink_log_info(mavlink_fd, "keep it level and still"); - /* set to accel calibration mode */ - status->flag_preflight_accel_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - const int calibration_count = 2500; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - int calibration_counter = 0; - float accel_offset[3] = {0.0f, 0.0f, 0.0f}; - - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null)) - warn("WARNING: failed to set scale / offsets for accel"); - - close(fd); - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - accel_offset[0] += raw.accelerometer_m_s2[0]; - accel_offset[1] += raw.accelerometer_m_s2[1]; - accel_offset[2] += raw.accelerometer_m_s2[2]; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "acceleration calibration aborted"); - return; - } - } - - accel_offset[0] = accel_offset[0] / calibration_count; - accel_offset[1] = accel_offset[1] / calibration_count; - accel_offset[2] = accel_offset[2] / calibration_count; - - if (isfinite(accel_offset[0]) && isfinite(accel_offset[1]) && isfinite(accel_offset[2])) { - - /* add the removed length from x / y to z, since we induce a scaling issue else */ - float total_len = sqrtf(accel_offset[0] * accel_offset[0] + accel_offset[1] * accel_offset[1] + accel_offset[2] * accel_offset[2]); - - /* if length is correct, zero results here */ - accel_offset[2] = accel_offset[2] + total_len; - - float scale = 9.80665f / total_len; - - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0])) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1])) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2])) - || param_set(param_find("SENS_ACC_XSCALE"), &(scale)) - || param_set(param_find("SENS_ACC_YSCALE"), &(scale)) - || param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!"); - } - - fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale = { - accel_offset[0], - scale, - accel_offset[1], - scale, - accel_offset[2], - scale, - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) - warn("WARNING: failed to set scale / offsets for accel"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "accel calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)"); - } - - /* exit accel calibration mode */ - status->flag_preflight_accel_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - - 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); - - const int calibration_count = 2500; - - int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s differential_pressure; - - int calibration_counter = 0; - float airspeed_offset = 0.0f; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure); - airspeed_offset += differential_pressure.voltage; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; - } - } - - airspeed_offset = airspeed_offset / calibration_count; - - if (isfinite(airspeed_offset)) { - - if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //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 */ - - } 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(sub_differential_pressure); -} - - - -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) -{ - /* 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; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - break; - - 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)) { - 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)) { - 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 */ - 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; - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_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, ¤t_status); - mavlink_log_info(mavlink_fd, "finished gyro cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - 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, ¤t_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, ¤t_status); - mavlink_log_info(mavlink_fd, "finished mag cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - 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, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); - tune_confirm(); - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* trim calibration */ - if ((int)(cmd->param4) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_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, ¤t_status); - mavlink_log_info(mavlink_fd, "finished trim cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_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; - } - - handled = true; - } - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_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, ¤t_status); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - 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, ¤t_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, ¤t_status); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished airspeed cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - 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"); - - } else { - - // XXX move this to LOW PRIO THREAD of commander app - /* Read all parameters from EEPROM to RAM */ - - 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; - - } 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; - - } else { - if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } 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; - } - - } else if (((int)(cmd->param1)) == 1) { - - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); - - if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "OK saved param file"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } 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()); - - } else { - mavlink_log_info(mavlink_fd, "ERR writing params to"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else { - mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } - } - } - 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); - } - 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); - - } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_confirm(); - } - - /* send any requested ACKs */ - if (cmd->confirmation > 0) { - /* send acknowledge command */ - // XXX TODO - } - -} - -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 -{ - /* Set thread name */ - prctl(PR_SET_NAME, "commander orb rcv", getpid()); - - /* Subscribe to command topic */ - int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); - struct subsystem_info_s info; - - struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg; - - while (!thread_should_exit) { - struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } }; - - if (poll(fds, 1, 5000) == 0) { - /* timeout, but this is no problem, silently ignore */ - } else { - /* got command */ - orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - - warnx("Subsys changed: %d\n", (int)info.subsystem_type); - - /* mark / unmark as present */ - if (info.present) { - vstatus->onboard_control_sensors_present |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_present &= ~info.subsystem_type; - } - - /* mark / unmark as enabled */ - if (info.enabled) { - vstatus->onboard_control_sensors_enabled |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type; - } - - /* mark / unmark as ok */ - if (info.ok) { - vstatus->onboard_control_sensors_health |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_health &= ~info.subsystem_type; - } - } - } - - close(subsys_sub); - - return NULL; -} - -/* - * Provides a coarse estimate of remaining battery power. - * - * The estimate is very basic and based on decharging voltage curves. - * - * @return the estimated remaining capacity in 0..1 - */ -float battery_remaining_estimate_voltage(float voltage); - -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); - -float battery_remaining_estimate_voltage(float voltage) -{ - float ret = 0; - static param_t bat_volt_empty; - static param_t bat_volt_full; - static param_t bat_n_cells; - static bool initialized = false; - static unsigned int counter = 0; - static float ncells = 3; - // XXX change cells to int (and param to INT32) - - if (!initialized) { - bat_volt_empty = param_find("BAT_V_EMPTY"); - bat_volt_full = param_find("BAT_V_FULL"); - bat_n_cells = param_find("BAT_N_CELLS"); - initialized = true; - } - - static float chemistry_voltage_empty = 3.2f; - static float chemistry_voltage_full = 4.05f; - - if (counter % 100 == 0) { - param_get(bat_volt_empty, &chemistry_voltage_empty); - param_get(bat_volt_full, &chemistry_voltage_full); - param_get(bat_n_cells, &ncells); - } - - counter++; - - ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); - - /* limit to sane values */ - ret = (ret < 0) ? 0 : ret; - ret = (ret > 1) ? 1 : ret; - return ret; -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); - exit(1); -} - -/** - * The daemon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int commander_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("commander already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - daemon_task = task_spawn("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3000, - commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\tcommander is running\n"); - - } else { - warnx("\tcommander not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int commander_thread_main(int argc, char *argv[]) -{ - /* not yet initialized */ - commander_initialized = false; - bool home_position_set = false; - - /* set parameters */ - failsafe_lowlevel_timeout_ms = 0; - param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); - - param_t _param_sys_type = param_find("MAV_TYPE"); - param_t _param_system_id = param_find("MAV_SYS_ID"); - param_t _param_component_id = param_find("MAV_COMP_ID"); - - /* welcome user */ - warnx("I am in command now!\n"); - - /* pthreads for command and subsystem info handling */ - // pthread_t command_handling_thread; - pthread_t subsystem_info_thread; - - /* initialize */ - if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds\n"); - } - - if (buzzer_init() != 0) { - warnx("ERROR: Failed to initialize buzzer\n"); - } - - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); - } - - /* make sure we are in preflight state */ - memset(¤t_status, 0, sizeof(current_status)); - current_status.state_machine = SYSTEM_STATE_PREFLIGHT; - current_status.flag_system_armed = false; - /* 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; - /* set battery warning flag */ - current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - - /* advertise to ORB */ - stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); - /* publish current state machine */ - state_machine_publish(stat_pub, ¤t_status, mavlink_fd); - - /* home position */ - orb_advert_t home_pub = -1; - struct home_position_s home; - memset(&home, 0, sizeof(home)); - - if (stat_pub < 0) { - warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); - warnx("exiting."); - exit(ERROR); - } - - mavlink_log_info(mavlink_fd, "system is running"); - - /* create pthreads */ - pthread_attr_t subsystem_info_attr; - pthread_attr_init(&subsystem_info_attr); - pthread_attr_setstacksize(&subsystem_info_attr, 2048); - pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); - - /* Start monitoring loop */ - uint16_t counter = 0; - uint8_t flight_env; - - /* Initialize to 0.0V */ - float battery_voltage = 0.0f; - bool battery_voltage_valid = false; - bool low_battery_voltage_actions_done = false; - 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; - - /* Subscribe to manual control data */ - int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp_man; - memset(&sp_man, 0, sizeof(sp_man)); - - /* Subscribe to offboard control data */ - int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s sp_offboard; - memset(&sp_offboard, 0, sizeof(sp_offboard)); - - 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; - - 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)); - uint64_t last_local_position_time = 0; - - /* - * The home position is set based on GPS only, to prevent a dependency between - * position estimator and commander. RAW GPS is more than good enough for a - * non-flying vehicle. - */ - int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps_position; - memset(&gps_position, 0, sizeof(gps_position)); - - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s sensors; - memset(&sensors, 0, sizeof(sensors)); - - int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s differential_pressure; - memset(&differential_pressure, 0, sizeof(differential_pressure)); - uint64_t last_differential_pressure_time = 0; - - /* Subscribe to command topic */ - int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - struct vehicle_command_s cmd; - memset(&cmd, 0, sizeof(cmd)); - - /* Subscribe to parameters changed topic */ - int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); - struct parameter_update_s param_changed; - memset(¶m_changed, 0, sizeof(param_changed)); - - /* subscribe to battery topic */ - int battery_sub = orb_subscribe(ORB_ID(battery_status)); - struct battery_status_s battery; - memset(&battery, 0, sizeof(battery)); - battery.voltage_v = 0.0f; - - // uint8_t vehicle_state_previous = current_status.state_machine; - float voltage_previous = 0.0f; - - uint64_t last_idle_time = 0; - - /* now initialized */ - commander_initialized = true; - thread_running = true; - - uint64_t start_time = hrt_absolute_time(); - uint64_t failsave_ll_start_time = 0; - - bool state_changed = true; - bool param_init_forced = true; - - while (!thread_should_exit) { - - /* Get current values */ - bool new_data; - orb_check(sp_man_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); - } - - orb_check(sp_offboard_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); - } - - orb_check(sensor_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); - } - - orb_check(differential_pressure_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure); - last_differential_pressure_time = differential_pressure.timestamp; - } - - orb_check(cmd_sub, &new_data); - - if (new_data) { - /* got command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* handle it */ - handle_command(stat_pub, ¤t_status, &cmd); - } - - /* update parameters */ - orb_check(param_changed_sub, &new_data); - - if (new_data || param_init_forced) { - param_init_forced = false; - /* parameters changed */ - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - - - /* update parameters */ - if (!current_status.flag_system_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 */ - orb_check(global_position_sub, &new_data); - - if (new_data) { - /* position changed */ - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); - last_global_position_time = global_position.timestamp; - } - - /* update local position estimate */ - orb_check(local_position_sub, &new_data); - - if (new_data) { - /* position changed */ - orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - last_local_position_time = local_position.timestamp; - } - - /* update battery status */ - orb_check(battery_sub, &new_data); - if (new_data) { - orb_copy(ORB_ID(battery_status), battery_sub, &battery); - battery_voltage = battery.voltage_v; - battery_voltage_valid = true; - - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - if (hrt_absolute_time() - start_time > 2500000) { - bat_remain = battery_remaining_estimate_voltage(battery_voltage); - } - } - - /* 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 || - current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL)) { - /* armed */ - led_toggle(LED_BLUE); - - } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not armed */ - led_toggle(LED_BLUE); - } - - /* toggle error led at 5 Hz in HIL mode */ - if (current_status.flag_hil_enabled) { - /* hil enabled */ - led_toggle(LED_AMBER); - - } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - /* toggle error (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 - - last_idle_time = system_load.tasks[0].total_runtime; - } - } - - // // XXX Export patterns and threshold to parameters - /* Trigger audio event for low battery */ - if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) { - /* For less than 10%, start be really annoying at 5 Hz */ - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, 3); - - } else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - - } 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(); - - } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - } - - /* Check battery voltage */ - /* write to sys_status */ - if (battery_voltage_valid) { - current_status.voltage_battery = battery_voltage; - - } else { - current_status.voltage_battery = 0.0f; - } - - /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - - if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { - low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; - } - - low_voltage_counter++; - } - - /* Critical, this is rather an emergency, kill signal to sdlog and 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, ¤t_status, mavlink_fd); - } - - critical_voltage_counter++; - - } else { - low_voltage_counter = 0; - critical_voltage_counter = 0; - } - - /* End battery voltage check */ - - - /* - * Check for valid position information. - * - * If the system has a valid position source from an onboard - * position estimator, it is safe to operate it autonomously. - * The flag_vector_flight_mode_ok flag indicates that a minimum - * set of position measurements is available. - */ - - /* 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; - - /* 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; - // XXX check for controller status and home position as well - - } else { - current_status.flag_global_position_valid = false; - } - - if (hrt_absolute_time() - last_local_position_time < 2000000) { - current_status.flag_local_position_valid = true; - // XXX check for controller status and home position as well - - } else { - current_status.flag_local_position_valid = false; - } - - /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_differential_pressure_time < 2000000) { - current_status.flag_airspeed_valid = true; - - } else { - current_status.flag_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; - - } 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) { - state_changed = true; - } - - /* - * Mark the position of the first position lock as return to launch (RTL) - * position. The MAV will return here on command or emergency. - * - * Conditions: - * - * 1) The system aquired position lock just now - * 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 (orb_check(ORB_ID(vehicle_gps_position), &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 */ - float hdop_m = gps_position.eph_m; - float vdop_m = gps_position.epv_m; - - /* check if gps fix is ok */ - // XXX magic number - float hdop_threshold_m = 4.0f; - float vdop_threshold_m = 8.0f; - - /* - * If horizontal dilution of precision (hdop / eph) - * and vertical diluation of precision (vdop / epv) - * are below a certain threshold (e.g. 4 m), AND - * home position is not yet set AND the last GPS - * GPS measurement is not older than two seconds AND - * the system is currently not armed, set home - * position to the current position. - */ - - if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !current_status.flag_system_armed) { - warnx("setting home position"); - - /* copy position data to uORB home message, store it locally as well */ - home.lat = gps_position.lat; - home.lon = gps_position.lon; - home.alt = gps_position.alt; - - home.eph_m = gps_position.eph_m; - home.epv_m = gps_position.epv_m; - - home.s_variance_m_s = gps_position.s_variance_m_s; - home.p_variance_m = gps_position.p_variance_m; - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - home_position_set = true; - tune_confirm(); - } - } - - /* ignore RC signals if in offboard control mode */ - if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* Start RC state check */ - - 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; - // } - - // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { - - // /* 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)) { - - // /* 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, 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.manual_mode_switch > STICK_ON_OFF_LIMIT) { - - // /* 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 { - // /* 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; - // } - - // 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)) { - - /* this switch is not properly mapped, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; - - } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; - - } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; - - } else { - /* center stick position, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; - } - - /* - * 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)) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); - stick_on_counter = 0; - - } else { - stick_off_counter++; - stick_on_counter = 0; - } - } - - /* 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, ¤t_status, mavlink_fd); - stick_on_counter = 0; - - } else { - stick_on_counter++; - stick_off_counter = 0; - } - } - - /* 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, ¤t_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, ¤t_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, ¤t_status, mavlink_fd); - - // } - - } else { - /* center stick position, set SAS for all vehicle types */ - update_state_machine_mode_stabilized(stat_pub, ¤t_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!"); - last_print_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; - - /* if the RC signal is gone for a full second, consider it lost */ - if (current_status.rc_signal_lost_interval > 1000000) { - current_status.rc_signal_lost = true; - current_status.failsave_lowlevel = true; - state_changed = true; - } - - // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { - // publish_armed_status(¤t_status); - // } - } - } - - - - - /* End mode switch */ - - /* END RC state check */ - - - /* State machine update for offboard control */ - 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 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; - } - - 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(); - - 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(); - } - } - - current_status.offboard_control_signal_weak = false; - current_status.offboard_control_signal_lost = false; - current_status.offboard_control_signal_lost_interval = 0; - - /* arm / disarm on request */ - if (sp_offboard.armed && !current_status.flag_system_armed) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - /* switch to stabilized mode = takeoff */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - - } else if (!sp_offboard.armed && current_status.flag_system_armed) { - update_state_machine_disarm(stat_pub, ¤t_status, 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)) { - current_status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); - last_print_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; - - /* if the signal is gone for 0.1 seconds, consider it lost */ - 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(); - - /* kill motors after timeout */ - if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { - current_status.failsave_lowlevel = true; - state_changed = true; - } - } - } - } - - - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); - - - /* 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, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - } - - /* publish at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - publish_armed_status(¤t_status); - orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); - state_changed = false; - } - - /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.voltage_battery; - - fflush(stdout); - counter++; - usleep(COMMANDER_MONITORING_INTERVAL); - } - - /* wait for threads to complete */ - // pthread_join(command_handling_thread, NULL); - pthread_join(subsystem_info_thread, NULL); - - /* close fds */ - led_deinit(); - buzzer_deinit(); - close(sp_man_sub); - close(sp_offboard_sub); - close(global_position_sub); - close(sensor_sub); - close(cmd_sub); - - warnx("exiting..\n"); - fflush(stdout); - - thread_running = false; - - return 0; -} diff --git a/apps/commander/commander.h b/apps/commander/commander.h deleted file mode 100644 index 04b4e72ab..000000000 --- a/apps/commander/commander.h +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file commander.h - * Main system state machine definition. - * - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * - */ - -#ifndef COMMANDER_H_ -#define COMMANDER_H_ - -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f - -void tune_confirm(void); -void tune_error(void); - -#endif /* COMMANDER_H_ */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c deleted file mode 100644 index bea388a10..000000000 --- a/apps/commander/state_machine_helper.c +++ /dev/null @@ -1,752 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file state_machine_helper.c - * State machine helper functions implementations - */ - -#include <stdio.h> -#include <unistd.h> - -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/actuator_controls.h> -#include <systemlib/systemlib.h> -#include <drivers/drv_hrt.h> -#include <mavlink/mavlink_log.h> - -#include "state_machine_helper.h" - -static const char *system_state_txt[] = { - "SYSTEM_STATE_PREFLIGHT", - "SYSTEM_STATE_STANDBY", - "SYSTEM_STATE_GROUND_READY", - "SYSTEM_STATE_MANUAL", - "SYSTEM_STATE_STABILIZED", - "SYSTEM_STATE_AUTO", - "SYSTEM_STATE_MISSION_ABORT", - "SYSTEM_STATE_EMCY_LANDING", - "SYSTEM_STATE_EMCY_CUTOFF", - "SYSTEM_STATE_GROUND_ERROR", - "SYSTEM_STATE_REBOOT", - -}; - -/** - * Transition from one state to another - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) -{ - int invalid_state = false; - int ret = ERROR; - - commander_state_machine_t old_state = current_status->state_machine; - - switch (new_state) { - case SYSTEM_STATE_MISSION_ABORT: { - /* Indoor or outdoor */ - // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { - ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); - - // } else { - // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); - // } - } - break; - - case SYSTEM_STATE_EMCY_LANDING: - /* Tell the controller to land */ - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - warnx("EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - /* Tell the controller to cutoff the motors (thrust = 0) */ - - /* set system flags according to state */ - current_status->flag_system_armed = false; - - warnx("EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); - break; - - case SYSTEM_STATE_GROUND_ERROR: - - /* set system flags according to state */ - - /* prevent actuators from arming */ - current_status->flag_system_armed = false; - - warnx("GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); - break; - - case SYSTEM_STATE_PREFLIGHT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); - - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); - } - - break; - - case SYSTEM_STATE_REBOOT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - invalid_state = false; - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); - } - - break; - - case SYSTEM_STATE_STANDBY: - /* set system flags according to state */ - - /* standby enforces disarmed */ - current_status->flag_system_armed = false; - - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - break; - - case SYSTEM_STATE_GROUND_READY: - - /* set system flags according to state */ - - /* ground ready has motors / actuators armed */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); - break; - - case SYSTEM_STATE_AUTO: - - /* set system flags according to state */ - - /* auto is airborne and in auto mode, motors armed */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); - break; - - case SYSTEM_STATE_STABILIZED: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); - break; - - case SYSTEM_STATE_MANUAL: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); - break; - - default: - invalid_state = true; - break; - } - - if (invalid_state == false || old_state != new_state) { - current_status->state_machine = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - ret = OK; - } - - if (invalid_state) { - mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); - ret = ERROR; - } - - return ret; -} - -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* publish the new state */ - current_status->counter++; - current_status->timestamp = hrt_absolute_time(); - - /* assemble state vector based on flag values */ - if (current_status->flag_control_rates_enabled) { - current_status->onboard_control_sensors_present |= 0x400; - - } else { - current_status->onboard_control_sensors_present &= ~0x400; - } - - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); -} - -void publish_armed_status(const struct vehicle_status_s *current_status) -{ - struct actuator_armed_s armed; - armed.armed = current_status->flag_system_armed; - /* lock down actuators if required, only in HIL */ - armed.lockdown = (current_status->flag_hil_enabled) ? true : false; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -} - - -/* - * Private functions, update the state machine - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - warnx("EMERGENCY HANDLER\n"); - /* Depending on the current state go to one of the error states */ - - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); - - } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { - - // DO NOT abort mission - //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); - - } else { - warnx("Unknown system state: #%d\n", current_status->state_machine); - } -} - -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors -{ - if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself - state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); - - } else { - //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); - } - -} - - - -// /* -// * Wrapper functions (to be used in the commander), all functions assume lock on current_status -// */ - -// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR -// * -// * START SUBSYSTEM/EMERGENCY FUNCTIONS -// * */ - -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was removed something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was disabled something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - - -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //TODO state machine change (recovering) -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_GPS: -// //TODO state machine change -// break; - -// default: -// break; -// } - - -// } - - -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); -// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_GPS: -// // //TODO: remove this block -// // break; -// // /////////////////// -// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); - -// // printf("previosly_healthy = %u\n", previosly_healthy); -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency(status_pub, current_status); - -// break; - -// default: -// break; -// } - -// } - - -/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ - - -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { - state_machine_emergency(status_pub, current_status, mavlink_fd); - } -} - -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[cmd] arming\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - } -} - -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[cmd] going standby\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - - } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] MISSION ABORT!\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - - current_status->flag_control_manual_enabled = true; - - /* set behaviour based on airframe */ - 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, set 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, set 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; - } - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] manual mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - } -} - -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - int old_mode = current_status->flight_mode; - int old_manual_control_mode = current_status->manual_control_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - current_status->flag_control_manual_enabled = true; - - if (old_mode != current_status->flight_mode || - old_manual_control_mode != current_status->manual_control_mode) { - printf("[cmd] att stabilized mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - state_machine_publish(status_pub, current_status, mavlink_fd); - } - - } -} - -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); - tune_error(); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] position guided mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - } -} - -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[cmd] auto mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - } -} - - -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -{ - uint8_t ret = 1; - - /* Switch on HIL if in standby and not already in HIL mode */ - if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && - current_status->flag_system_armed) { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") - - } else { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") - } - } - - /* switch manual / auto */ - if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { - update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { - update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { - update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { - update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); - } - - /* vehicle is disarmed, mode requests arming */ - if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only arm in standby state */ - // XXX REMOVE - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - ret = OK; - printf("[cmd] arming due to command request\n"); - } - } - - /* vehicle is armed, mode requests disarming */ - if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only disarm in ground ready */ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - ret = OK; - printf("[cmd] disarming due to command request\n"); - } - } - - /* NEVER actually switch off HIL without reboot */ - if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); - mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); - ret = ERROR; - } - - return ret; -} - -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations -{ - commander_state_machine_t current_system_state = current_status->state_machine; - - uint8_t ret = 1; - - switch (custom_mode) { - case SYSTEM_STATE_GROUND_READY: - break; - - case SYSTEM_STATE_STANDBY: - break; - - case SYSTEM_STATE_REBOOT: - printf("try to reboot\n"); - - if (current_system_state == SYSTEM_STATE_STANDBY - || current_system_state == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - printf("system will reboot\n"); - mavlink_log_critical(mavlink_fd, "Rebooting.."); - usleep(200000); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); - ret = 0; - } - - break; - - case SYSTEM_STATE_AUTO: - printf("try to switch to auto/takeoff\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - printf("state: auto\n"); - ret = 0; - } - - break; - - case SYSTEM_STATE_MANUAL: - printf("try to switch to manual\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - printf("state: manual\n"); - ret = 0; - } - - break; - - default: - break; - } - - return ret; -} - diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h deleted file mode 100644 index 2f2ccc729..000000000 --- a/apps/commander/state_machine_helper.h +++ /dev/null @@ -1,209 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file state_machine_helper.h - * State machine helper functions definitions - */ - -#ifndef STATE_MACHINE_HELPER_H_ -#define STATE_MACHINE_HELPER_H_ - -#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor) -#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock - -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_status.h> - -/** - * Switch to new state with no checking. - * - * do_state_update: this is the functions that all other functions have to call in order to update the state. - * the function does not question the state change, this must be done before - * The function performs actions that are connected with the new state (buzzer, reboot, ...) - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - * - * @return 0 (macro OK) or 1 on error (macro ERROR) - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state); - -/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - - -/** - * Handle state machine if got position fix - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if position fix lost - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to arm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to disarm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is manual - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is stabilized - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is guided - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is auto - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish current state information - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - - -/* - * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app). - * If the request is obeyed the functions return 0 - * - */ - -/** - * Handles *incoming request* to switch to a specific state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode); - -/** - * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode); - -/** - * Always switches to critical mode under any circumstances. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Switches to emergency if required. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish the armed state depending on the current system state - * - * @param current_status the current system status - */ -void publish_armed_status(const struct vehicle_status_s *current_status); - - - -#endif /* STATE_MACHINE_HELPER_H_ */ |