From bba26c34303269c0cb1e25f593e968e1aedf8a85 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 15 Apr 2015 18:53:59 -0700 Subject: [PATCH] Linux: enabled commander module The commander module now compiles for Linux. state_machine_helper_linux.cpp iterates over the virtual devices vs all devices under /dev as per NuttX when disabling publishing. Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 1 + src/drivers/device/vdev.cpp | 8 + src/drivers/device/vdev.h | 1 + src/drivers/device/vdev_posix.cpp | 5 + src/drivers/drv_led.h | 6 +- src/drivers/drv_rgbled.h | 2 +- src/drivers/drv_tone_alarm.h | 2 +- .../commander/airspeed_calibration.cpp | 1 + .../commander/calibration_routines.cpp | 3 +- src/modules/commander/commander.cpp | 2 +- src/modules/commander/commander_helper.cpp | 5 +- src/modules/commander/gyro_calibration.cpp | 1 + .../commander/gyro_calibration_linux.cpp | 274 +++++++ .../commander/mag_calibration_linux.cpp | 470 ++++++++++++ src/modules/commander/module.mk | 16 +- .../commander/state_machine_helper_linux.cpp | 675 ++++++++++++++++++ src/modules/systemlib/cpuload.c | 5 + src/modules/systemlib/cpuload.h | 4 +- src/platforms/px4_config.h | 3 + src/platforms/px4_posix.h | 1 + 20 files changed, 1471 insertions(+), 14 deletions(-) create mode 100644 src/modules/commander/gyro_calibration_linux.cpp create mode 100644 src/modules/commander/mag_calibration_linux.cpp create mode 100644 src/modules/commander/state_machine_helper_linux.cpp diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index fd3c691b09..e40a58d46a 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -46,6 +46,7 @@ MODULES += modules/uORB MODULES += modules/dataman MODULES += modules/sdlog2 MODULES += modules/simulator +MODULES += modules/commander # # Libraries diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 89275a919c..e5004b6841 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -467,5 +467,13 @@ void VDev::showDevices() } } +const char *VDev::devList(unsigned int *next) +{ + for (;*nextname; + return NULL; +} + } // namespace device diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 62ae479c62..1ce48285f7 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -334,6 +334,7 @@ public: static VDev *getDev(const char *path); static void showDevices(void); + static const char *devList(unsigned int *next); protected: diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index df19fd8392..50df5535d2 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -274,5 +274,10 @@ void px4_show_devices() VDev::showDevices(); } +const char * px4_get_device_names(unsigned int *handle) +{ + return VDev::devList(handle); +} + } diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index f3e8164714..3f5897bd00 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -52,9 +52,9 @@ #define LED_BLUE 0 #define LED_SAFETY 2 -#define LED_ON _IOC(_LED_BASE, 0) -#define LED_OFF _IOC(_LED_BASE, 1) -#define LED_TOGGLE _IOC(_LED_BASE, 2) +#define LED_ON _PX4_IOC(_LED_BASE, 0) +#define LED_OFF _PX4_IOC(_LED_BASE, 1) +#define LED_TOGGLE _PX4_IOC(_LED_BASE, 2) __BEGIN_DECLS diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index 0633768684..4e3de36abd 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -50,7 +50,7 @@ */ #define _RGBLEDIOCBASE (0x2900) -#define _RGBLEDIOC(_n) (_IOC(_RGBLEDIOCBASE, _n)) +#define _RGBLEDIOC(_n) (_PX4_IOC(_RGBLEDIOCBASE, _n)) /** play the named script in *(char *)arg, repeating forever */ #define RGBLED_PLAY_SCRIPT_NAMED _RGBLEDIOC(1) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 31aee266e1..b2e02cdb8f 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -65,7 +65,7 @@ #define TONEALARM0_DEVICE_PATH "/dev/tone_alarm0" #define _TONE_ALARM_BASE 0x7400 -#define TONE_SET_ALARM _IOC(_TONE_ALARM_BASE, 1) +#define TONE_SET_ALARM _PX4_IOC(_TONE_ALARM_BASE, 1) /* structure describing one note in a tone pattern */ struct tone_note { diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 747d915ff1..773c62b3e1 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -41,6 +41,7 @@ #include "commander_helper.h" #include +#include #include #include #include diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 192b8c7270..98a354e0cd 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -39,6 +39,7 @@ */ #include +#include #include #include #include @@ -53,7 +54,7 @@ #include "commander_helper.h" // FIXME: Fix return codes -static const int ERROR = -1; +//static const int ERROR = -1; 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, diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 48b5070440..31baedb2fc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -53,7 +53,7 @@ #include #include #include -#include +//#include #include #include #include diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index a5e4d19725..c06523ce44 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -41,6 +41,7 @@ * */ +#include #include #include #include @@ -110,7 +111,7 @@ int battery_init() bat_capacity_h = param_find("BAT_CAPACITY"); bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP"); - return OK; + return PX4_OK; } int buzzer_init() @@ -130,7 +131,7 @@ int buzzer_init() return ERROR; } - return OK; + return PX4_OK; } void buzzer_deinit() diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 291ccfe804..a2b0900ef6 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -42,6 +42,7 @@ #include "commander_helper.h" #include +#include #include #include #include diff --git a/src/modules/commander/gyro_calibration_linux.cpp b/src/modules/commander/gyro_calibration_linux.cpp new file mode 100644 index 0000000000..f490487f3b --- /dev/null +++ b/src/modules/commander/gyro_calibration_linux.cpp @@ -0,0 +1,274 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 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. + * + ****************************************************************************/ + +/** + * @file gyro_calibration.cpp + * + * Gyroscope calibration routine + */ + +#include "gyro_calibration.h" +#include "calibration_messages.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +static const char *sensor_name = "gyro"; + +int do_gyro_calibration(int mavlink_fd) +{ + const unsigned max_gyros = 3; + + int32_t device_id[3]; + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "HOLD STILL"); + + /* wait for the user to respond */ + sleep(2); + + struct gyro_scale gyro_scale_zero = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + struct gyro_scale gyro_scale[max_gyros] = {}; + + int res = OK; + + /* store board ID */ + uint32_t mcu_id[3]; + mcu_unique_id(&mcu_id[0]); + + /* store last 32bit number - not unique, but unique in a given set */ + (void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]); + + char str[30]; + + for (unsigned s = 0; s < max_gyros; s++) { + + /* ensure all scale fields are initialized tha same as the first struct */ + (void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0])); + + sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); + /* reset all offsets to zero and all scales to one */ + int fd = px4_open(str, 0); + + if (fd < 0) { + continue; + } + + device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); + px4_close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); + } + } + + unsigned calibration_counter[max_gyros] = { 0 }; + const unsigned calibration_count = 5000; + + struct gyro_report gyro_report_0 = {}; + + if (res == OK) { + /* determine gyro mean values */ + unsigned poll_errcount = 0; + + /* subscribe to gyro sensor topic */ + int sub_sensor_gyro[max_gyros]; + px4_pollfd_struct_t fds[max_gyros]; + + for (unsigned s = 0; s < max_gyros; s++) { + sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); + fds[s].fd = sub_sensor_gyro[s]; + fds[s].events = POLLIN; + } + + struct gyro_report gyro_report; + + /* use first gyro to pace, but count correctly per-gyro for statistics */ + while (calibration_counter[0] < calibration_count) { + /* wait blocking for new data */ + + int poll_ret = px4_poll(&fds[0], max_gyros, 1000); + + if (poll_ret > 0) { + + for (unsigned s = 0; s < max_gyros; s++) { + bool changed; + orb_check(sub_sensor_gyro[s], &changed); + + if (changed) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report); + + if (s == 0) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0); + } + + gyro_scale[s].x_offset += gyro_report.x; + gyro_scale[s].y_offset += gyro_report.y; + gyro_scale[s].z_offset += gyro_report.z; + calibration_counter[s]++; + } + + if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter[0] * 100) / calibration_count); + } + } + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } + } + + for (unsigned s = 0; s < max_gyros; s++) { + px4_close(sub_sensor_gyro[s]); + + gyro_scale[s].x_offset /= calibration_counter[s]; + gyro_scale[s].y_offset /= calibration_counter[s]; + gyro_scale[s].z_offset /= calibration_counter[s]; + } + } + + if (res == OK) { + /* check offsets */ + float xdiff = gyro_report_0.x - gyro_scale[0].x_offset; + float ydiff = gyro_report_0.y - gyro_scale[0].y_offset; + float zdiff = gyro_report_0.z - gyro_scale[0].z_offset; + + /* maximum allowable calibration error in radians */ + const float maxoff = 0.002f; + + if (!isfinite(gyro_scale[0].x_offset) || + !isfinite(gyro_scale[0].y_offset) || + !isfinite(gyro_scale[0].z_offset) || + fabsf(xdiff) > maxoff || + fabsf(ydiff) > maxoff || + fabsf(zdiff) > maxoff) { + mavlink_log_critical(mavlink_fd, "ERROR: Motion during calibration"); + res = ERROR; + } + } + + if (res == OK) { + /* set offset parameters to new values */ + bool failed = false; + + for (unsigned s = 0; s < max_gyros; s++) { + + /* if any reasonable amount of data is missing, skip */ + if (calibration_counter[s] < calibration_count / 2) { + continue; + } + + (void)sprintf(str, "CAL_GYRO%u_XOFF", s); + failed |= (OK != param_set(param_find(str), &(gyro_scale[s].x_offset))); + (void)sprintf(str, "CAL_GYRO%u_YOFF", s); + failed |= (OK != param_set(param_find(str), &(gyro_scale[s].y_offset))); + (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); + failed |= (OK != param_set(param_find(str), &(gyro_scale[s].z_offset))); + (void)sprintf(str, "CAL_GYRO%u_ID", s); + failed |= (OK != param_set(param_find(str), &(device_id[s]))); + + /* apply new scaling and offsets */ + (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); + int fd = px4_open(str, 0); + + if (fd < 0) { + failed = true; + continue; + } + + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]); + px4_close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + } + } + + if (failed) { + mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params"); + res = ERROR; + } + } + + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + } + } + + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + + } else { + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + } + + return res; +} diff --git a/src/modules/commander/mag_calibration_linux.cpp b/src/modules/commander/mag_calibration_linux.cpp new file mode 100644 index 0000000000..49ccbf3ef1 --- /dev/null +++ b/src/modules/commander/mag_calibration_linux.cpp @@ -0,0 +1,470 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 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. + * + ****************************************************************************/ + +/** + * @file mag_calibration.cpp + * + * Magnetometer calibration routine + */ + +#include "mag_calibration.h" +#include "commander_helper.h" +#include "calibration_routines.h" +#include "calibration_messages.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +static const char *sensor_name = "mag"; +static const unsigned max_mags = 3; + +int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); +int mag_calibration_worker(detect_orientation_return orientation, void* worker_data); + +/// Data passed to calibration worker routine +typedef struct { + int mavlink_fd; + unsigned done_count; + int sub_mag[max_mags]; + unsigned int calibration_points_perside; + unsigned int calibration_interval_perside_seconds; + uint64_t calibration_interval_perside_useconds; + unsigned int calibration_counter_total; + bool side_data_collected[detect_orientation_side_count]; + float* x[max_mags]; + float* y[max_mags]; + float* z[max_mags]; +} mag_worker_data_t; + + +int do_mag_calibration(int mavlink_fd) +{ + mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + int result = OK; + + // Determine which mags are available and reset each + + int32_t device_ids[max_mags]; + char str[30]; + + for (size_t i=0; imavlink_fd, "Rotate vehicle around the detected orientation"); + mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds); + sleep(2); + + uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; + unsigned poll_errcount = 0; + + calibration_counter_side = 0; + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter_side < worker_data->calibration_points_perside) { + + // Wait clocking for new data on all mags + px4_pollfd_struct_t fds[max_mags]; + size_t fd_count = 0; + for (size_t cur_mag=0; cur_magsub_mag[cur_mag] >= 0) { + fds[fd_count].fd = worker_data->sub_mag[cur_mag]; + fds[fd_count].events = POLLIN; + fd_count++; + } + } + int poll_ret = px4_poll(fds, fd_count, 1000); + + if (poll_ret > 0) { + for (size_t cur_mag=0; cur_magsub_mag[cur_mag] >= 0) { + struct mag_report mag; + + orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag); + + worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x; + worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y; + worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z; + + } + } + + worker_data->calibration_counter_total++; + calibration_counter_side++; + + // Progress indicator for side + mavlink_and_console_log_info(worker_data->mavlink_fd, + "%s %s side calibration: progress <%u>", + sensor_name, + detect_orientation_str(orientation), + (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); + } else { + poll_errcount++; + } + + if (poll_errcount > worker_data->calibration_points_perside * 3) { + result = ERROR; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG); + break; + } + } + + // Mark the opposite side as collected as well. No need to collect opposite side since it + // would generate similar points. + detect_orientation_return alternateOrientation = orientation; + switch (orientation) { + case DETECT_ORIENTATION_TAIL_DOWN: + alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN; + break; + case DETECT_ORIENTATION_NOSE_DOWN: + alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN; + break; + case DETECT_ORIENTATION_LEFT: + alternateOrientation = DETECT_ORIENTATION_RIGHT; + break; + case DETECT_ORIENTATION_RIGHT: + alternateOrientation = DETECT_ORIENTATION_LEFT; + break; + case DETECT_ORIENTATION_UPSIDE_DOWN: + alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP; + break; + case DETECT_ORIENTATION_RIGHTSIDE_UP: + alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN; + break; + case DETECT_ORIENTATION_ERROR: + warnx("Invalid orientation in mag_calibration_worker"); + break; + } + worker_data->side_data_collected[alternateOrientation] = true; + mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation)); + + worker_data->done_count++; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count); + + return result; +} + +int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) +{ + int result = OK; + + mag_worker_data_t worker_data; + + worker_data.mavlink_fd = mavlink_fd; + worker_data.done_count = 0; + worker_data.calibration_counter_total = 0; + worker_data.calibration_points_perside = 80; + worker_data.calibration_interval_perside_seconds = 20; + worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; + + // Initialize to collect all sides + for (size_t cur_side=0; cur_side<6; cur_side++) { + worker_data.side_data_collected[cur_side] = false; + } + + for (size_t cur_mag=0; cur_mag(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.y[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.z[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); + if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) { + mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); + result = ERROR; + } + } + + + // Setup subscriptions to mag sensors + if (result == OK) { + for (unsigned cur_mag=0; cur_mag= 0) { + px4_close(worker_data.sub_mag[cur_mag]); + } + } + + // Calculate calibration values for each mag + + + float sphere_x[max_mags]; + float sphere_y[max_mags]; + float sphere_z[max_mags]; + float sphere_radius[max_mags]; + + // Sphere fit the data to get calibration values + if (result == OK) { + for (unsigned cur_mag=0; cur_mag= 0) { + px4_close(fd_mag); + } + + if (result == OK) { + bool failed = false; + + /* set parameters */ + (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag]))); + (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); + (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset))); + (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); + (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); + (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); + (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); + + if (failed) { + mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, cur_mag); + result = ERROR; + } else { + mavlink_and_console_log_info(mavlink_fd, "mag #%u off: x:%.2f y:%.2f z:%.2f Ga", + cur_mag, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_and_console_log_info(mavlink_fd, "mag #%u scale: x:%.2f y:%.2f z:%.2f", + cur_mag, + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); + } + } + } + } + } + + return result; +} diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 4437041e26..c9de1ffa2f 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -38,17 +38,25 @@ MODULE_COMMAND = commander SRCS = commander.cpp \ commander_params.c \ - state_machine_helper.cpp \ commander_helper.cpp \ calibration_routines.cpp \ - accelerometer_calibration.cpp \ - gyro_calibration.cpp \ - mag_calibration.cpp \ baro_calibration.cpp \ + accelerometer_calibration.cpp \ rc_calibration.cpp \ airspeed_calibration.cpp \ PreflightCheck.cpp +ifdef ($(PX4_TARGET_OS),nuttx) +SRCS += + state_machine_helper.cpp \ + gyro_calibration.cpp \ + mag_calibration.cpp +else +SRCS += state_machine_helper_linux.cpp \ + gyro_calibration_linux.cpp \ + mag_calibration_linux.cpp +endif + MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os diff --git a/src/modules/commander/state_machine_helper_linux.cpp b/src/modules/commander/state_machine_helper_linux.cpp new file mode 100644 index 0000000000..97c2f11043 --- /dev/null +++ b/src/modules/commander/state_machine_helper_linux.cpp @@ -0,0 +1,675 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 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. + * + ****************************************************************************/ + +/** + * @file state_machine_helper.cpp + * State machine helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "state_machine_helper.h" +#include "commander_helper.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +// This array defines the arming state transitions. The rows are the new state, and the columns +// are the current state. Using new state and current state you can index into the array which +// will be true for a valid transition or false for a invalid transition. In some cases even +// though the transition is marked as true additional checks must be made. See arming_state_transition +// code for those checks. +static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI +}; + +// You can index into the array with an arming_state_t in order to get it's textual representation +static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { + "ARMING_STATE_INIT", + "ARMING_STATE_STANDBY", + "ARMING_STATE_ARMED", + "ARMING_STATE_ARMED_ERROR", + "ARMING_STATE_STANDBY_ERROR", + "ARMING_STATE_REBOOT", + "ARMING_STATE_IN_AIR_RESTORE", +}; + +transition_result_t +arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status + const struct safety_s *safety, ///< current safety settings + arming_state_t new_arming_state, ///< arming state requested + struct actuator_armed_s *armed, ///< current armed status + bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing + const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none +{ + // Double check that our static arrays are still valid + ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); + ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1); + + transition_result_t ret = TRANSITION_DENIED; + arming_state_t current_arming_state = status->arming_state; + bool feedback_provided = false; + + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state == current_arming_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + /* + * Get sensing state if necessary + */ + int prearm_ret = OK; + + /* only perform the check if we have to */ + if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + prearm_ret = prearm_check(status, mavlink_fd); + } + + /* + * Perform an atomic state update + */ + //irqstate_t flags = irqsave(); + + /* enforce lockdown in HIL */ + if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { + armed->lockdown = true; + + } else { + armed->lockdown = false; + } + + // Check that we have a valid state transition + bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; + + if (valid_transition) { + // We have a good transition. Now perform any secondary validation. + if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + + // Do not perform pre-arm checks if coming from in air restore + // Allow if vehicle_status_s::HIL_STATE_ON + if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && + status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + + // Fail transition if pre-arm check fails + if (prearm_ret) { + /* the prearm check already prints the reject reason */ + feedback_provided = true; + valid_transition = false; + + // Fail transition if we need safety switch press + } else if (safety->safety_switch_available && !safety->safety_off) { + + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); + feedback_provided = true; + valid_transition = false; + } + + // Perform power checks only if circuit breaker is not + // engaged for these checks + if (!status->circuit_breaker_engaged_power_check) { + // Fail transition if power is not good + if (!status->condition_power_input_valid) { + + mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); + feedback_provided = true; + valid_transition = false; + } + + // Fail transition if power levels on the avionics rail + // are measured but are insufficient + if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { + // Check avionics rail voltages + if (status->avionics_power_rail_voltage < 4.75f) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + valid_transition = false; + } else if (status->avionics_power_rail_voltage < 4.9f) { + mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + } else if (status->avionics_power_rail_voltage > 5.4f) { + mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + } + } + } + + } + + } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { + new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; + } + } + + // HIL can always go to standby + if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { + valid_transition = true; + } + + /* Sensors need to be initialized for STANDBY state */ + if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); + feedback_provided = true; + valid_transition = false; + } + + // Finish up the state transition + if (valid_transition) { + armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; + ret = TRANSITION_CHANGED; + status->arming_state = new_arming_state; + } + + /* end of atomic state update */ + //irqrestore(flags); + } + + if (ret == TRANSITION_DENIED) { + const char * str = "INVAL: %s - %s"; + /* only print to console here by default as this is too technical to be useful during operation */ + warnx(str, state_names[status->arming_state], state_names[new_arming_state]); + + /* print to MAVLink if we didn't provide any feedback yet */ + if (!feedback_provided) { + mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]); + } + } + + return ret; +} + +bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed) +{ + // System is safe if: + // 1) Not armed + // 2) Armed, but in software lockdown (HIL) + // 3) Safety switch is present AND engaged -> actuators locked + if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { + return true; + + } else { + return false; + } +} + +transition_result_t +main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* transition may be denied even if the same state is requested because conditions may have changed */ + switch (new_main_state) { + case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_ACRO: + ret = TRANSITION_CHANGED; + break; + + case vehicle_status_s::MAIN_STATE_ALTCTL: + /* need at minimum altitude estimate */ + /* TODO: add this for fixedwing as well */ + if (!status->is_rotary_wing || + (status->condition_local_altitude_valid || + status->condition_global_position_valid)) { + ret = TRANSITION_CHANGED; + } + break; + + case vehicle_status_s::MAIN_STATE_POSCTL: + /* need at minimum local position estimate */ + if (status->condition_local_position_valid || + status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + break; + + case vehicle_status_s::MAIN_STATE_AUTO_LOITER: + /* need global position estimate */ + if (status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + break; + + case vehicle_status_s::MAIN_STATE_AUTO_MISSION: + case vehicle_status_s::MAIN_STATE_AUTO_RTL: + /* need global position and home position */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + ret = TRANSITION_CHANGED; + } + break; + + case vehicle_status_s::MAIN_STATE_OFFBOARD: + + /* need offboard signal */ + if (!status->offboard_control_signal_lost) { + ret = TRANSITION_CHANGED; + } + + break; + + case vehicle_status_s::MAIN_STATE_MAX: + default: + break; + } + if (ret == TRANSITION_CHANGED) { + if (status->main_state != new_main_state) { + status->main_state = new_main_state; + } else { + ret = TRANSITION_NOT_CHANGED; + } + } + + return ret; +} + +/** + * Transition from one hil state to another + */ +transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; + + if (current_status->hil_state == new_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + switch (new_state) { + case vehicle_status_s::HIL_STATE_OFF: + /* we're in HIL and unexpected things can happen if we disable HIL now */ + mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); + ret = TRANSITION_DENIED; + break; + + case vehicle_status_s::HIL_STATE_ON: + if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { + + const char *devname; + unsigned int handle = 0; + for(;;) { + devname = px4_get_device_names(&handle); + if (devname == NULL) + break; + + /* skip mavlink */ + if (!strcmp("/dev/mavlink", devname)) { + continue; + } + + + int sensfd = px4_open(devname, 0); + + if (sensfd < 0) { + warn("failed opening device %s", devname); + continue; + } + + int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); + px4_close(sensfd); + + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); + } + ret = TRANSITION_CHANGED; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + } else { + mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); + ret = TRANSITION_DENIED; + } + break; + + default: + warnx("Unknown HIL state"); + break; + } + } + + if (ret == TRANSITION_CHANGED) { + current_status->hil_state = new_state; + current_status->timestamp = hrt_absolute_time(); + // XXX also set lockdown here + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + } + return ret; +} + +/** + * Check failsafe and main status and set navigation status for navigator accordingly + */ +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, + const bool stay_in_failsafe) +{ + navigation_state_t nav_state_old = status->nav_state; + + bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); + status->failsafe = false; + + /* evaluate main state to decide in normal (non-failsafe) mode */ + switch (status->main_state) { + case vehicle_status_s::MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_ALTCTL: + case vehicle_status_s::MAIN_STATE_POSCTL: + /* require RC for all manual modes */ + if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; + } else if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + + } else { + switch (status->main_state) { + case vehicle_status_s::MAIN_STATE_ACRO: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; + break; + + case vehicle_status_s::MAIN_STATE_MANUAL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + break; + + case vehicle_status_s::MAIN_STATE_ALTCTL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + break; + + case vehicle_status_s::MAIN_STATE_POSCTL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + break; + + default: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + break; + } + } + break; + + case vehicle_status_s::MAIN_STATE_AUTO_MISSION: + + /* go into failsafe + * - if commanded to do so + * - if we have an engine failure + * - depending on datalink, RC and if the mission is finished */ + + /* first look at the commands */ + if (status->engine_failure_cmd) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (status->data_link_lost_cmd) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; + } else if (status->gps_failure_cmd) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } else if (status->rc_signal_lost_cmd) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; + + /* finished handling commands which have priority, now handle failures */ + } else if (status->gps_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } else if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + + /* datalink loss enabled: + * check for datalink lost: this should always trigger RTGS */ + } else if (data_link_loss_enabled && status->data_link_lost) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; + } else if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + + /* datalink loss disabled: + * check if both, RC and datalink are lost during the mission + * or RC is lost after the mission is finished: this should always trigger RCRECOVER */ + } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) || + (status->rc_signal_lost && mission_finished))) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; + } else if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + + /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ + } else if (!stay_in_failsafe){ + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; + } + break; + + case vehicle_status_s::MAIN_STATE_AUTO_LOITER: + /* go into failsafe on a engine failure */ + if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; + } else if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + + /* go into failsafe if RC is lost and datalink loss is not set up */ + } else if (status->rc_signal_lost && !data_link_loss_enabled) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; + } else if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + + /* don't bother if RC is lost if datalink is connected */ + } else if (status->rc_signal_lost) { + + /* this mode is ok, we don't need RC for loitering */ + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + } else { + /* everything is perfect */ + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + } + break; + + case vehicle_status_s::MAIN_STATE_AUTO_RTL: + /* require global position and home, also go into failsafe on an engine failure */ + + if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((!status->condition_global_position_valid || + !status->condition_home_position_valid)) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + } + break; + + case vehicle_status_s::MAIN_STATE_OFFBOARD: + /* require offboard control, otherwise stay where you are */ + if (status->offboard_control_signal_lost && !status->rc_signal_lost) { + status->failsafe = true; + + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; + } + default: + break; + } + + return status->nav_state != nav_state_old; +} + +int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) +{ + int ret; + bool failed = false; + + int fd = px4_open(ACCEL0_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING"); + failed = true; + goto system_eval; + } + + ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION"); + failed = true; + goto system_eval; + } + + /* check measurement result range */ + struct accel_report acc; + ret = px4_read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + + if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still"); + /* this is frickin' fatal */ + failed = true; + goto system_eval; + } + } else { + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ"); + /* this is frickin' fatal */ + failed = true; + goto system_eval; + } + + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { + /* accel done, close it */ + px4_close(fd); + fd = orb_subscribe(ORB_ID(airspeed)); + + struct airspeed_s airspeed; + + if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || + (hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) { + mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); + failed = true; + goto system_eval; + } + + if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { + mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + // XXX do not make this fatal yet + } + } + +system_eval: + px4_close(fd); + return (failed); +} diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index 4f58ac71a7..fe08da71fd 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -61,6 +61,8 @@ #ifdef CONFIG_SCHED_INSTRUMENTATION +#ifdef __PX4_NUTTX + __EXPORT void sched_note_start(FAR struct tcb_s *tcb); __EXPORT void sched_note_stop(FAR struct tcb_s *tcb); __EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb); @@ -167,4 +169,7 @@ void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb) } } +#else +__EXPORT struct system_load_s system_load; +#endif #endif /* CONFIG_SCHED_INSTRUMENTATION */ diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h index 16d132fdb8..dc173baa79 100644 --- a/src/modules/systemlib/cpuload.h +++ b/src/modules/systemlib/cpuload.h @@ -37,13 +37,15 @@ __BEGIN_DECLS -#include +#include struct system_load_taskinfo_s { uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load uint64_t curr_start_time; ///< Start time of the current scheduling slot uint64_t start_time; ///< FIRST start time of task +#ifdef __PX4_NUTTX FAR struct tcb_s *tcb; ///< +#endif bool valid; ///< Task is currently active / valid }; diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 82a79312a0..5388f08f1d 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -52,6 +52,9 @@ /** time in ms between checks for work in work queues **/ #define CONFIG_SCHED_WORKPERIOD 10 +#define CONFIG_SCHED_INSTRUMENTATION 1 +#define CONFIG_MAX_TASKS 32 + #define px4_errx(x, ...) errx(x, __VA_ARGS__) #endif diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index f88deab466..2eff37f88e 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -84,5 +84,6 @@ __EXPORT ssize_t px4_write(int fd, const void *buffer, size_t buflen); __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); __EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout); __EXPORT void px4_show_devices(void); +__EXPORT const char * px4_get_device_names(unsigned int *handle); __END_DECLS