diff options
author | Julian Oes <julian@oes.ch> | 2013-05-17 11:24:02 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-05-17 11:24:02 +0200 |
commit | f5c157e74df12a0cb36b7d27cdad9828d96cc534 (patch) | |
tree | 3f758990921a7b52df8afe5131a8298b1141b6f4 /src/modules/commander | |
parent | 80e8eeab2931e79e31adb17c93f5794e666c5763 (diff) | |
parent | fa816d0fd65da461fd5bf8803cf00caebaf23c5c (diff) | |
download | px4-firmware-f5c157e74df12a0cb36b7d27cdad9828d96cc534.tar.gz px4-firmware-f5c157e74df12a0cb36b7d27cdad9828d96cc534.tar.bz2 px4-firmware-f5c157e74df12a0cb36b7d27cdad9828d96cc534.zip |
Merge remote-tracking branch 'upstream/master' into new_state_machine
Conflicts:
src/drivers/px4io/px4io.cpp
src/modules/commander/commander.c
src/modules/commander/state_machine_helper.c
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/accelerometer_calibration.c | 404 | ||||
-rw-r--r-- | src/modules/commander/accelerometer_calibration.h | 16 | ||||
-rw-r--r-- | src/modules/commander/calibration_routines.c | 219 | ||||
-rw-r--r-- | src/modules/commander/calibration_routines.h | 61 | ||||
-rw-r--r-- | src/modules/commander/commander.c | 2269 | ||||
-rw-r--r-- | src/modules/commander/commander.h | 58 | ||||
-rw-r--r-- | src/modules/commander/module.mk | 43 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.c | 758 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.h | 61 |
9 files changed, 3889 insertions, 0 deletions
diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c new file mode 100644 index 000000000..231739962 --- /dev/null +++ b/src/modules/commander/accelerometer_calibration.c @@ -0,0 +1,404 @@ +/* + * accelerometer_calibration.c + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@gmail.com> + * + * Transform acceleration vector to true orientation and scale + * + * * * * Model * * * + * accel_corr = accel_T * (accel_raw - accel_offs) + * + * accel_corr[3] - fully corrected acceleration vector in body frame + * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform + * accel_raw[3] - raw acceleration vector + * accel_offs[3] - acceleration offset vector + * + * * * * Calibration * * * + * + * Reference vectors + * accel_corr_ref[6][3] = [ g 0 0 ] // nose up + * | -g 0 0 | // nose down + * | 0 g 0 | // left side down + * | 0 -g 0 | // right side down + * | 0 0 g | // on back + * [ 0 0 -g ] // level + * accel_raw_ref[6][3] + * + * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 + * + * 6 reference vectors * 3 axes = 18 equations + * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants + * + * Find accel_offs + * + * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 + * + * + * Find accel_T + * + * 9 unknown constants + * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations + * + * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 + * + * Solve separate system for each row of accel_T: + * + * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 + * + * A * x = b + * + * x = [ accel_T[0][i] ] + * | accel_T[1][i] | + * [ accel_T[2][i] ] + * + * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * | accel_corr_ref[2][i] | + * [ accel_corr_ref[4][i] ] + * + * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 + * + * Matrix A is common for all three systems: + * A = [ a[0][0] a[0][1] a[0][2] ] + * | a[2][0] a[2][1] a[2][2] | + * [ a[4][0] a[4][1] a[4][2] ] + * + * x = A^-1 * b + * + * accel_T = A^-1 * g + * g = 9.80665 + */ + +#include "accelerometer_calibration.h" + +#include <poll.h> +#include <drivers/drv_hrt.h> +#include <uORB/topics/sensor_combined.h> +#include <drivers/drv_accel.h> +#include <systemlib/conversions.h> +#include <mavlink/mavlink_log.h> + +void do_accel_calibration(int mavlink_fd); +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +int detect_orientation(int mavlink_fd, int sub_sensor_combined); +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); +int mat_invert3(float src[3][3], float dst[3][3]); +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); + +void do_accel_calibration(int mavlink_fd) { + /* announce change */ + mavlink_log_info(mavlink_fd, "accel calibration started"); + + /* measure and calculate offsets & scales */ + float accel_offs[3]; + float accel_scale[3]; + int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); + + if (res == OK) { + /* measurements complete successfully, set parameters */ + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); + } + + int fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale = { + accel_offs[0], + accel_scale[0], + accel_offs[1], + accel_scale[1], + accel_offs[2], + accel_scale[2], + }; + + 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"); + } + + mavlink_log_info(mavlink_fd, "accel calibration done"); + tune_positive(); + } else { + /* measurements error */ + mavlink_log_info(mavlink_fd, "accel calibration aborted"); + tune_negative(); + } + + /* exit accel calibration mode */ +} + +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { + const int samples_num = 2500; + float accel_ref[6][3]; + bool data_collected[6] = { false, false, false, false, false, false }; + const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + + /* reset existing calibration */ + int fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); + close(fd); + + if (OK != ioctl_res) { + warn("ERROR: failed to set scale / offsets for accel"); + return ERROR; + } + + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + while (true) { + bool done = true; + char str[80]; + int str_ptr; + str_ptr = sprintf(str, "keep vehicle still:"); + for (int i = 0; i < 6; i++) { + if (!data_collected[i]) { + str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); + done = false; + } + } + if (done) + break; + mavlink_log_info(mavlink_fd, str); + + int orient = detect_orientation(mavlink_fd, sensor_combined_sub); + if (orient < 0) + return ERROR; + + sprintf(str, "meas started: %s", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, str); + read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); + str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); + mavlink_log_info(mavlink_fd, str); + data_collected[orient] = true; + tune_neutral(); + } + close(sensor_combined_sub); + + /* calculate offsets and rotation+scale matrix */ + float accel_T[3][3]; + int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + if (res != 0) { + mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); + return ERROR; + } + + /* convert accel transform matrix to scales, + * rotation part of transform matrix is not used by now + */ + for (int i = 0; i < 3; i++) { + accel_scale[i] = accel_T[i][i]; + } + + return OK; +} + +/* + * Wait for vehicle become still and detect it's orientation. + * + * @return 0..5 according to orientation when vehicle is still and ready for measurements, + * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 + */ +int detect_orientation(int mavlink_fd, int sub_sensor_combined) { + struct sensor_combined_s sensor; + /* exponential moving average of accel */ + float accel_ema[3] = { 0.0f, 0.0f, 0.0f }; + /* max-hold dispersion of accel */ + float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; + /* EMA time constant in seconds*/ + float ema_len = 0.2f; + /* set "still" threshold to 0.1 m/s^2 */ + float still_thr2 = pow(0.1f, 2); + /* set accel error threshold to 5m/s^2 */ + float accel_err_thr = 5.0f; + /* still time required in us */ + int64_t still_time = 2000000; + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + hrt_abstime t_start = hrt_absolute_time(); + /* set timeout to 30s */ + hrt_abstime timeout = 30000000; + hrt_abstime t_timeout = t_start + timeout; + hrt_abstime t = t_start; + hrt_abstime t_prev = t_start; + hrt_abstime t_still = 0; + while (true) { + /* wait blocking for new data */ + int poll_ret = poll(fds, 1, 1000); + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor); + t = hrt_absolute_time(); + float dt = (t - t_prev) / 1000000.0f; + t_prev = t; + float w = dt / ema_len; + for (int i = 0; i < 3; i++) { + accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; + float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; + d = d * d; + accel_disp[i] = accel_disp[i] * (1.0f - w); + if (d > accel_disp[i]) + accel_disp[i] = d; + } + /* still detector with hysteresis */ + if ( accel_disp[0] < still_thr2 && + accel_disp[1] < still_thr2 && + accel_disp[2] < still_thr2 ) { + /* is still now */ + if (t_still == 0) { + /* first time */ + mavlink_log_info(mavlink_fd, "still..."); + t_still = t; + t_timeout = t + timeout; + } else { + /* still since t_still */ + if ((int64_t) t - (int64_t) t_still > still_time) { + /* vehicle is still, exit from the loop to detection of its orientation */ + break; + } + } + } else if ( accel_disp[0] > still_thr2 * 2.0f || + accel_disp[1] > still_thr2 * 2.0f || + accel_disp[2] > still_thr2 * 2.0f) { + /* not still, reset still start time */ + if (t_still != 0) { + mavlink_log_info(mavlink_fd, "moving..."); + t_still = 0; + } + } + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "ERROR: poll failure"); + return -3; + } + if (t > t_timeout) { + mavlink_log_info(mavlink_fd, "ERROR: timeout"); + return -1; + } + } + + if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabs(accel_ema[1]) < accel_err_thr && + fabs(accel_ema[2]) < accel_err_thr ) + return 0; // [ g, 0, 0 ] + if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabs(accel_ema[1]) < accel_err_thr && + fabs(accel_ema[2]) < accel_err_thr ) + return 1; // [ -g, 0, 0 ] + if ( fabs(accel_ema[0]) < accel_err_thr && + fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabs(accel_ema[2]) < accel_err_thr ) + return 2; // [ 0, g, 0 ] + if ( fabs(accel_ema[0]) < accel_err_thr && + fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabs(accel_ema[2]) < accel_err_thr ) + return 3; // [ 0, -g, 0 ] + if ( fabs(accel_ema[0]) < accel_err_thr && + fabs(accel_ema[1]) < accel_err_thr && + fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) + return 4; // [ 0, 0, g ] + if ( fabs(accel_ema[0]) < accel_err_thr && + fabs(accel_ema[1]) < accel_err_thr && + fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) + return 5; // [ 0, 0, -g ] + + mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); + + return -2; // Can't detect orientation +} + +/* + * Read specified number of accelerometer samples, calculate average and dispersion. + */ +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { + struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; + int count = 0; + float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; + + while (count < samples_num) { + int poll_ret = poll(fds, 1, 1000); + if (poll_ret == 1) { + struct sensor_combined_s sensor; + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + for (int i = 0; i < 3; i++) + accel_sum[i] += sensor.accelerometer_m_s2[i]; + count++; + } else { + return ERROR; + } + } + + for (int i = 0; i < 3; i++) { + accel_avg[i] = accel_sum[i] / count; + } + + return OK; +} + +int mat_invert3(float src[3][3], float dst[3][3]) { + float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - + src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + if (det == 0.0) + return ERROR; // Singular matrix + + dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; + dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; + dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; + dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det; + dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det; + dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det; + dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; + dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; + dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; + + return OK; +} + +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) { + /* calculate offsets */ + for (int i = 0; i < 3; i++) { + accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; + } + + /* fill matrix A for linear equations system*/ + float mat_A[3][3]; + memset(mat_A, 0, sizeof(mat_A)); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + float a = accel_ref[i * 2][j] - accel_offs[j]; + mat_A[i][j] = a; + } + } + + /* calculate inverse matrix for A */ + float mat_A_inv[3][3]; + if (mat_invert3(mat_A, mat_A_inv) != OK) + return ERROR; + + /* copy results to accel_T */ + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + /* simplify matrices mult because b has only one non-zero element == g at index i */ + accel_T[j][i] = mat_A_inv[j][i] * g; + } + } + + return OK; +} diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h new file mode 100644 index 000000000..6a920c4ef --- /dev/null +++ b/src/modules/commander/accelerometer_calibration.h @@ -0,0 +1,16 @@ +/* + * accelerometer_calibration.h + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@gmail.com> + */ + +#ifndef ACCELEROMETER_CALIBRATION_H_ +#define ACCELEROMETER_CALIBRATION_H_ + +#include <stdint.h> +#include <uORB/topics/vehicle_status.h> + +void do_accel_calibration(int mavlink_fd); + +#endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c new file mode 100644 index 000000000..a26938637 --- /dev/null +++ b/src/modules/commander/calibration_routines.c @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * 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/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h new file mode 100644 index 000000000..e3e7fbafd --- /dev/null +++ b/src/modules/commander/calibration_routines.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * 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/src/modules/commander/commander.c b/src/modules/commander/commander.c new file mode 100644 index 000000000..a34e526a8 --- /dev/null +++ b/src/modules/commander/commander.c @@ -0,0 +1,2269 @@ +/**************************************************************************** + * + * 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 <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 <drivers/drv_led.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_tone_alarm.h> + +#include <mavlink/mavlink_log.h> +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> +#include <systemlib/err.h> +#include <systemlib/cpuload.h> + +#include "state_machine_helper.h" + +#include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_mag.h> +#include <drivers/drv_baro.h> + +#include "calibration_routines.h" +#include "accelerometer_calibration.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); + + +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) + +#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ + +/* File descriptors */ +static int leds; +static int buzzer; +static int mavlink_fd; + +/* flags */ +static bool commander_initialized = false; +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +/* Main state machine */ +static struct vehicle_status_s current_status; +static orb_advert_t stat_pub; + +/* timout until lowlevel failsafe */ +static unsigned int failsafe_lowlevel_timeout_ms; + +/* tasks waiting for low prio thread */ +enum { + LOW_PRIO_TASK_NONE = 0, + LOW_PRIO_TASK_PARAM_SAVE, + LOW_PRIO_TASK_PARAM_LOAD, + LOW_PRIO_TASK_GYRO_CALIBRATION, + LOW_PRIO_TASK_MAG_CALIBRATION, + LOW_PRIO_TASK_ALTITUDE_CALIBRATION, + LOW_PRIO_TASK_RC_CALIBRATION, + LOW_PRIO_TASK_ACCEL_CALIBRATION, + LOW_PRIO_TASK_AIRSPEED_CALIBRATION +} low_prio_task; + + +/* pthread loops */ +void *orb_receive_loop(void *arg); +void *commander_low_prio_loop(void *arg); + +__EXPORT int commander_main(int argc, char *argv[]); + +/** + * Mainloop of commander. + */ +int commander_thread_main(int argc, char *argv[]); + +int buzzer_init(void); +void buzzer_deinit(void); +int led_init(void); +void led_deinit(void); +int led_toggle(int led); +int led_on(int led); +int led_off(int led); +void tune_error(void); +void tune_positive(void); +void tune_neutral(void); +void tune_negative(void); +void do_reboot(void); +void do_gyro_calibration(void); +void do_mag_calibration(void); +void do_rc_calibration(void); +void do_airspeed_calibration(void); + + +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); + +int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); + + + +/** + * Print the correct usage. + */ +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); + +int buzzer_init() +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return 0; +} + +void buzzer_deinit() +{ + close(buzzer); +} + + +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; +} + +void led_deinit() +{ + close(leds); +} + +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); +} + +int led_on(int led) +{ + return ioctl(leds, LED_ON, led); +} + +int led_off(int led) +{ + return ioctl(leds, LED_OFF, led); +} + + +void tune_error() +{ + ioctl(buzzer, TONE_SET_ALARM, 2); +} + +void tune_positive() +{ + ioctl(buzzer, TONE_SET_ALARM, 3); +} + +void tune_neutral() +{ + ioctl(buzzer, TONE_SET_ALARM, 4); +} + +void tune_negative() +{ + ioctl(buzzer, TONE_SET_ALARM, 5); +} + +void do_reboot() +{ + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ +} + + +void do_rc_calibration() +{ + mavlink_log_info(mavlink_fd, "trim calibration starting"); + + 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 */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + } + + tune_positive(); + + mavlink_log_info(mavlink_fd, "trim calibration done"); +} + +void do_mag_calibration() +{ + mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); + + 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); + + 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; + } + + 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_neutral(); + + 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_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + } + + close(sub_mag); +} + +void do_gyro_calibration() +{ + mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); + + 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; + + 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_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + } + + close(sub_sensor_combined); +} + + +void do_airspeed_calibration() +{ + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + + const int calibration_count = 2500; + + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + + int calibration_counter = 0; + float diff_pres_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + diff_pres_offset += diff_pres.differential_pressure_pa; + 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; + } + } + + diff_pres_offset = diff_pres_offset / calibration_count; + + if (isfinite(diff_pres_offset)) { + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_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_positive(); + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + close(diff_pres_sub); +} + +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; + + /* request to set different system mode */ + switch (cmd->command) { + case VEHICLE_CMD_DO_SET_MODE: + + /* request to activate HIL */ + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { + + if (OK == hil_state_transition(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { + 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 == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + /* request to disarm */ + } else if ((int)cmd->param1 == 0) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + break; + + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + + /* request for an autopilot reboot */ + if ((int)cmd->param1 == 1) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) { + /* reboot is executed later, after positive tune is sent */ + result = VEHICLE_CMD_RESULT_ACCEPTED; + tune_positive(); + /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + do_reboot(); + + } else { + /* system may return here */ + result = VEHICLE_CMD_RESULT_DENIED; + tune_negative(); + } + } + } + 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: + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + + // /* zero-altitude pressure calibration */ + // if ((int)(cmd->param3) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } + + + + // /* trim calibration */ + // if ((int)(cmd->param4) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } + + + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + + case VEHICLE_CMD_PREFLIGHT_STORAGE: + + if (((int)(cmd->param1)) == 0) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + + } else if (((int)(cmd->param1)) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + + default: + mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + break; + } + + /* supported command handling stop */ + if (result == VEHICLE_CMD_RESULT_FAILED || + result == VEHICLE_CMD_RESULT_DENIED || + result == VEHICLE_CMD_RESULT_UNSUPPORTED || + result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + + tune_negative(); + + } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + + tune_positive(); + } + + /* send any requested ACKs */ + if (cmd->confirmation > 0) { + /* send acknowledge command */ + // XXX TODO + } + +} + +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; +} + +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("[commander] starting"); + + /* pthreads for command and subsystem info handling */ + // pthread_t command_handling_thread; + pthread_t subsystem_info_thread; + pthread_t commander_low_prio_thread; + + /* initialize */ + if (led_init() != 0) { + warnx("ERROR: Failed to initialize leds"); + } + + if (buzzer_init() != 0) { + warnx("ERROR: Failed to initialize buzzer"); + } + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); + } + + /* make sure we are in preflight state */ + memset(¤t_status, 0, sizeof(current_status)); + + + current_status.navigation_state = NAVIGATION_STATE_INIT; + current_status.arming_state = ARMING_STATE_INIT; + current_status.hil_state = HIL_STATE_OFF; + current_status.flag_hil_enabled = false; + current_status.flag_fmu_armed = false; + current_status.flag_io_armed = false; // XXX read this from somewhere + + /* 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; + + // XXX for now just set sensors as initialized + current_status.condition_system_sensors_initialized = true; + + /* 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); + } + + // XXX needed? + 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); + + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + + struct sched_param param; + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + + /* Start monitoring loop */ + uint16_t counter = 0; + + /* 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; + float bat_remain = 1.0f; + + uint16_t stick_off_counter = 0; + uint16_t stick_on_counter = 0; + + /* XXX what exactly is this for? */ + uint64_t last_print_time = 0; + + /* Subscribe to manual control data */ + int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp_man; + 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)); + + /* Subscribe to global position */ + int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_s global_position; + memset(&global_position, 0, sizeof(global_position)); + uint64_t last_global_position_time = 0; + + /* Subscribe to local position data */ + int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + struct vehicle_local_position_s local_position; + memset(&local_position, 0, sizeof(local_position)); + 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. + */ + + /* Subscribe to GPS topic */ + int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps_position; + memset(&gps_position, 0, sizeof(gps_position)); + + /* Subscribe to sensor topic */ + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s sensors; + memset(&sensors, 0, sizeof(sensors)); + + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + memset(&diff_pres, 0, sizeof(diff_pres)); + uint64_t last_diff_pres_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(); + + /* XXX use this! */ + //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(diff_pres_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + last_diff_pres_time = diff_pres.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_fmu_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; + } + + /* set the condition to valid if there has recently been a local position estimate */ + if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { + current_status.condition_local_position_valid = true; + } else { + current_status.condition_local_position_valid = false; + } + + /* update battery status */ + orb_check(battery_sub, &new_data); + if (new_data) { + 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) { + + /* XXX if armed */ + if ((false /*current_status.state_machine == SYSTEM_STATE_GROUND_READY || + current_status.state_machine == SYSTEM_STATE_AUTO || + current_status.state_machine == SYSTEM_STATE_MANUAL*/)) { + /* armed, solid */ + led_on(LED_AMBER); + + } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not armed */ + led_toggle(LED_AMBER); + } + + if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { + + /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ + if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { + /* GPS lock */ + led_on(LED_BLUE); + + } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* no GPS lock, but GPS module is aquiring lock */ + led_toggle(LED_BLUE); + } + + } else { + /* no GPS info, don't light the blue led */ + led_off(LED_BLUE); + } + + /* toggle GPS led at 5 Hz in HIL mode */ + if (current_status.flag_hil_enabled) { + /* hil enabled */ + led_toggle(LED_BLUE); + + } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + /* toggle arming (red) at 5 Hz on low battery or error */ + led_toggle(LED_AMBER); + } + + } + + 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_positive(); + + } 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, 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; + // XXX implement this + // 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 */ + + /* If in INIT state, try to proceed to STANDBY state */ + if (current_status.arming_state == ARMING_STATE_INIT) { + // XXX check for sensors + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + } else { + // XXX: Add emergency stuff if sensors are lost + } + + + /* + * 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.condition_global_position_valid; + bool local_pos_valid = current_status.condition_local_position_valid; + bool airspeed_valid = current_status.condition_airspeed_valid; + + + /* check for global or local position updates, set a timeout of 2s */ + if (hrt_absolute_time() - last_global_position_time < 2000000) { + current_status.condition_global_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.condition_global_position_valid = false; + } + + if (hrt_absolute_time() - last_local_position_time < 2000000) { + current_status.condition_local_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.condition_local_position_valid = false; + } + + /* Check for valid airspeed/differential pressure measurements */ + if (hrt_absolute_time() - last_diff_pres_time < 2000000) { + current_status.condition_airspeed_valid = true; + + } else { + current_status.condition_airspeed_valid = false; + } + + /* + * Consolidate global position and local position valid flags + * for vector flight mode. + */ + // if (current_status.condition_local_position_valid || + // current_status.condition_global_position_valid) { + // current_status.flag_vector_flight_mode_ok = true; + + // } else { + // current_status.flag_vector_flight_mode_ok = false; + // } + + /* consolidate state change, flag as changed if required */ + if (global_pos_valid != current_status.condition_global_position_valid || + local_pos_valid != current_status.condition_local_position_valid || + airspeed_valid != current_status.condition_airspeed_valid) { + state_changed = true; + } + + /* + * 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(gps_sub, &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) // XXX note that vdop is 0 for mtk + && !home_position_set + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && !current_status.flag_fmu_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_positive(); + } + } + + /* 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.mode_switch)) { + + warnx("mode sw not finite"); + /* no valid stick position, go to default */ + + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, go to manual mode */ + current_status.mode_switch = MODE_SWITCH_MANUAL; + printf("mode switch: manual\n"); + + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position, set auto/mission for all vehicle types */ + current_status.mode_switch = MODE_SWITCH_AUTO; + printf("mode switch: auto\n"); + + } else { + + /* center stick position, set seatbelt/simple control */ + current_status.mode_switch = MODE_SWITCH_SEATBELT; + printf("mode switch: seatbelt\n"); + } + + // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); + + /* + * Check if land/RTL is requested + */ + if (!isfinite(sp_man.return_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, set altitude hold */ + current_status.return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.return_switch = RETURN_SWITCH_RETURN; + + } else { + /* center stick position, set default */ + current_status.return_switch = RETURN_SWITCH_NONE; + } + + /* check mission switch */ + if (!isfinite(sp_man.mission_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.mission_switch = MISSION_SWITCH_NONE; + + } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { + + /* top switch position */ + current_status.mission_switch = MISSION_SWITCH_MISSION; + + } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom switch position */ + current_status.mission_switch = MISSION_SWITCH_NONE; + + } else { + + /* center switch position, set default */ + current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? + } + + /* Now it's time to handle the stick inputs */ + + switch (current_status.arming_state) { + + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: + + /* just manual, XXX this might depend on the return switch */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + + /* Try seatbelt or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + + /* Try auto or fallback to seatbelt or even manual */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + // first fallback to SEATBELT_STANDY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + // or fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + } + } + + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + + /* SEATBELT_STANDBY (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_NONE) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* SEATBELT_DESCENT (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_RETURN) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_NONE) { + + /* we might come from the disarmed state AUTO_STANDBY */ + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + /* or from some other armed state like SEATBELT or MANUAL */ + } else if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_MISSION) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_RETURN + && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } + break; + + // XXX we might be missing something that triggers a transition from RTL to LAND + + case ARMING_STATE_ARMED_ERROR: + + // XXX work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // XXX work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } + /* handle the case where RC signal was regained */ + if (!current_status.rc_signal_found_once) { + current_status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + + } else { + if (current_status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } + } + + /* + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) && + (sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + stick_off_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) { + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); + stick_on_counter = 0; + + } else { + stick_on_counter++; + stick_off_counter = 0; + } + } + + current_status.rc_signal_cutting_off = false; + current_status.rc_signal_lost = false; + current_status.rc_signal_lost_interval = 0; + + } else { + + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* only complain if the offboard control is NOT active */ + current_status.rc_signal_cutting_off = true; + mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); + + if (!current_status.rc_signal_cutting_off) { + printf("Reason: not rc_signal_cutting_off\n"); + } else { + printf("last print time: %llu\n", last_print_time); + } + + last_print_time = hrt_absolute_time(); + } + + /* 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_positive(); + + 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_positive(); + } + } + + current_status.offboard_control_signal_weak = false; + current_status.offboard_control_signal_lost = false; + current_status.offboard_control_signal_lost_interval = 0; + + // XXX check if this is correct + /* arm / disarm on request */ + if (sp_offboard.armed && !current_status.flag_fmu_armed) { + + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); + + } else if (!sp_offboard.armed && current_status.flag_fmu_armed) { + + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + } + + } else { + + /* 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_positive(); + + /* 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(); + + + // XXX this is missing + /* If full run came back clean, transition to standby */ + // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && + // current_status.flag_preflight_gyro_calibration == false && + // current_status.flag_preflight_mag_calibration == false && + // current_status.flag_preflight_accel_calibration == false) { + // /* All ok, no calibration going on, go to standby */ + // do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + // } + + /* publish at least with 1 Hz */ + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { + + 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; + /* XXX use this voltage_previous */ + fflush(stdout); + counter++; + usleep(COMMANDER_MONITORING_INTERVAL); + } + + /* wait for threads to complete */ + // pthread_join(command_handling_thread, NULL); + pthread_join(subsystem_info_thread, NULL); + pthread_join(commander_low_prio_thread, NULL); + + /* close fds */ + led_deinit(); + buzzer_deinit(); + close(sp_man_sub); + close(sp_offboard_sub); + close(global_position_sub); + close(sensor_sub); + close(cmd_sub); + + warnx("exiting"); + fflush(stdout); + + thread_running = false; + + return 0; +} + + +void *commander_low_prio_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander low prio", getpid()); + + while (!thread_should_exit) { + + switch (low_prio_task) { + + case LOW_PRIO_TASK_PARAM_LOAD: + + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "Param load success"); + } else { + mavlink_log_critical(mavlink_fd, "Param load ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_PARAM_SAVE: + + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "Param save success"); + } else { + mavlink_log_critical(mavlink_fd, "Param save ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_GYRO_CALIBRATION: + + do_gyro_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_MAG_CALIBRATION: + + do_mag_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + + // do_baro_calibration(); + + case LOW_PRIO_TASK_RC_CALIBRATION: + + // do_rc_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ACCEL_CALIBRATION: + + do_accel_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + + do_airspeed_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_NONE: + default: + /* slow down to 10Hz */ + usleep(100000); + break; + } + + } + + return 0; +} diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander.h new file mode 100644 index 000000000..04b4e72ab --- /dev/null +++ b/src/modules/commander/commander.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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/src/modules/commander/module.mk b/src/modules/commander/module.mk new file mode 100644 index 000000000..fe44e955a --- /dev/null +++ b/src/modules/commander/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# Main system state machine +# + +MODULE_COMMAND = commander +SRCS = commander.c \ + state_machine_helper.c \ + calibration_routines.c \ + accelerometer_calibration.c + diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c new file mode 100644 index 000000000..daed81553 --- /dev/null +++ b/src/modules/commander/state_machine_helper.c @@ -0,0 +1,758 @@ +/**************************************************************************** + * + * 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 <stdint.h> +#include <stdbool.h> + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_controls.h> +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> +#include <mavlink/mavlink_log.h> + +#include "state_machine_helper.h" + +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { + + int ret = ERROR; + + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state == current_state->arming_state) { + ret = OK; + } else { + + switch (new_arming_state) { + case ARMING_STATE_INIT: + + /* allow going back from INIT for calibration */ + if (current_state->arming_state == ARMING_STATE_STANDBY) { + ret = OK; + current_state->flag_fmu_armed = false; + } + break; + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED) { + + /* sensors need to be initialized for STANDBY state */ + if (current_state->condition_system_sensors_initialized) { + ret = OK; + current_state->flag_fmu_armed = false; + } else { + mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); + } + } + break; + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + + /* XXX conditions for arming? */ + ret = OK; + current_state->flag_fmu_armed = true; + } + break; + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_ARMED) { + + /* XXX conditions for an error state? */ + ret = OK; + current_state->flag_fmu_armed = true; + } + break; + case ARMING_STATE_STANDBY_ERROR: + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { + ret = OK; + current_state->flag_fmu_armed = false; + } + break; + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { + + ret = OK; + current_state->flag_fmu_armed = false; + + } + break; + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; + default: + break; + } + + if (ret == OK) { + current_state->arming_state = new_arming_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } + } + + return ret; +} + + + +/* + * This functions does not evaluate any input flags but only checks if the transitions + * are valid. + */ +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd) { + + int ret = ERROR; + + /* only check transition if the new state is actually different from the current one */ + if (new_navigation_state == current_state->navigation_state) { + ret = OK; + } else { + + switch (new_navigation_state) { + case NAVIGATION_STATE_INIT: + + /* transitions back to INIT are possible for calibration */ + if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { + + ret = OK; + current_state->flag_control_rates_enabled = false; + current_state->flag_control_attitude_enabled = false; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; + } + break; + + case NAVIGATION_STATE_MANUAL_STANDBY: + + /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to be disarmed first */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; + } + } + break; + + case NAVIGATION_STATE_MANUAL: + + /* need to be armed first */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; + } + break; + + case NAVIGATION_STATE_SEATBELT_STANDBY: + + /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + + /* need to be disarmed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_SEATBELT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_SEATBELT_DESCENT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_STANDBY: + + /* transitions from INIT or from other STANDBY modes or from AUTO READY */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + /* need to be disarmed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); + } else if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_READY: + + /* transitions from AUTO_STANDBY or AUTO_LAND */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + + // XXX flag test needed? + + /* need to be armed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* only transitions from AUTO_READY */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a mission ready */ + if (!current_state-> condition_auto_mission_available) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_LAND: + /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + default: + break; + } + + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } + } + + + + return ret; +} + + +/** +* Transition from one hil state to another +*/ +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; + + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); + + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; + + } else { + + switch (new_state) { + + case HIL_STATE_OFF: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; + + case HIL_STATE_ON: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; + + default: + warnx("Unknown hil state"); + break; + } + } + + if (valid_transition) { + current_status->hil_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + } + + 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); +} + +// void publish_armed_status(const struct vehicle_status_s *current_status) +// { +// struct actuator_armed_s armed; +// armed.armed = current_status->flag_system_armed; +// +// /* XXX allow arming by external components on multicopters only if not yet armed by RC */ +// /* XXX allow arming only if core sensors are ok */ +// armed.ready_to_arm = true; +// +// /* 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); +// } + + +// /* +// * 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; +// } + +// } + + + +///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +// +//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +//{ +// int 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_fmu_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_fmu_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_fmu_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; +//} diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h new file mode 100644 index 000000000..5b57cffb7 --- /dev/null +++ b/src/modules/commander/state_machine_helper.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * 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> + +void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); + +void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd); +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd); + +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); + +#endif /* STATE_MACHINE_HELPER_H_ */
\ No newline at end of file |