diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index bb729e1032..b519e0e7a1 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -1,5 +1,5 @@ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -125,7 +125,11 @@ ARCHWARNINGS = -Wall \ -Wlogical-op \ -Wmissing-declarations \ -Wpacked \ - -Wno-unused-parameter + -Wno-unused-parameter \ + -Werror=format-security \ + -Werror=array-bounds \ + -Wfatal-errors \ + -Wformat=1 # -Wcast-qual - generates spurious noreturn attribute warnings, try again later # -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code # -Wcast-align - would help catch bad casts in some cases, but generates too many false positives @@ -142,7 +146,8 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ # C++-specific warnings # -ARCHWARNINGSXX = $(ARCHWARNINGS) +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-missing-field-initializers # pull in *just* libm from the toolchain ... this is grody LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 524151c90d..41942aacd5 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -82,10 +82,12 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), _max_differential_pressure_pa(0), _sensor_ok(false), + _last_published_sensor_ok(true), /* initialize differently to force publication */ _measure_ticks(0), _collect_phase(false), _diff_pres_offset(0.0f), _airspeed_pub(-1), + _subsys_pub(-1), _class_instance(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), @@ -149,8 +151,7 @@ Airspeed::init() } ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; + out: return ret; } @@ -344,22 +345,6 @@ Airspeed::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_DIFFPRESSURE - }; - static orb_advert_t pub = -1; - - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } } void @@ -368,12 +353,35 @@ Airspeed::stop() work_cancel(HPWORK, &_work); } +void +Airspeed::update_status() +{ + if (_sensor_ok != _last_published_sensor_ok) { + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + _sensor_ok, + SUBSYSTEM_TYPE_DIFFPRESSURE + }; + + if (_subsys_pub > 0) { + orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); + } else { + _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); + } + + _last_published_sensor_ok = _sensor_ok; + } +} + void Airspeed::cycle_trampoline(void *arg) { Airspeed *dev = (Airspeed *)arg; dev->cycle(); + dev->update_status(); } void diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 186602eda8..0b8e949c9d 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -118,14 +118,21 @@ protected: virtual int measure() = 0; virtual int collect() = 0; + /** + * Update the subsystem status + */ + void update_status(); + work_s _work; float _max_differential_pressure_pa; bool _sensor_ok; + bool _last_published_sensor_ok; int _measure_ticks; bool _collect_phase; float _diff_pres_offset; orb_advert_t _airspeed_pub; + orb_advert_t _subsys_pub; int _class_instance; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index d873a1132e..146a06e7cf 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -207,14 +207,18 @@ ETSAirspeed::collect() void ETSAirspeed::cycle() { + int ret; + /* collection phase? */ if (_collect_phase) { /* perform collection */ - if (OK != collect()) { + ret = collect(); + if (OK != ret) { perf_count(_comms_errors); /* restart the measurement state machine */ start(); + _sensor_ok = false; return; } @@ -238,8 +242,12 @@ ETSAirspeed::cycle() } /* measurement phase */ - if (OK != measure()) - log("measure error"); + ret = measure(); + if (OK != ret) { + debug("measure error"); + } + + _sensor_ok = (ret == OK); /* next phase is collection */ _collect_phase = true; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index cb97354ecc..cdc9d31063 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -715,7 +715,7 @@ HMC5883::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); + debug("collection error"); /* restart the measurement state machine */ start(); return; @@ -742,7 +742,7 @@ HMC5883::cycle() /* measurement phase */ if (OK != measure()) - log("measure error"); + debug("measure error"); /* next phase is collection */ _collect_phase = true; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 1ad383ee0b..c0f3c28e0f 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -288,13 +288,17 @@ MEASAirspeed::collect() void MEASAirspeed::cycle() { + int ret; + /* collection phase? */ if (_collect_phase) { /* perform collection */ - if (OK != collect()) { + ret = collect(); + if (OK != ret) { /* restart the measurement state machine */ start(); + _sensor_ok = false; return; } @@ -318,10 +322,13 @@ MEASAirspeed::cycle() } /* measurement phase */ - if (OK != measure()) { - log("measure error"); + ret = measure(); + if (OK != ret) { + debug("measure error"); } + _sensor_ok = (ret == OK); + /* next phase is collection */ _collect_phase = true; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 705e98eea4..3cb9abc492 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1015,7 +1015,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): /* copy the current output value from the channel */ - *(servo_position_t *)arg = Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue; + *(servo_position_t *)arg = Motor[cmd - PWM_SERVO_GET(0)].RawPwmValue; break; diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 12f80c4a37..c555a0a692 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -10,9 +10,9 @@ * 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 documentation4 and/or other materials provided with the + * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -33,9 +33,10 @@ /** * @file CatapultLaunchMethod.cpp - * Catpult Launch detection + * Catapult Launch detection + * + * @author Thomas Gubler * - * Authors and acknowledgements in header. */ #include "CatapultLaunchMethod.h" diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index 55c46ff3f3..23757f6f37 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -10,9 +10,9 @@ * 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 documentation4 and/or other materials provided with the + * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index bf539701b8..3bf47bbb03 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -30,12 +30,11 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - /** * @file launchDetection.cpp * Auto Detection for different launch methods (e.g. catapult) * - * Authors and acknowledgements in header. + * @author Thomas Gubler */ #include "LaunchDetector.h" diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 8066ebab36..1a214b66ee 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -10,9 +10,9 @@ * 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 documentation4 and/or other materials provided with the + * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index 01fa7050e6..e04467f6a4 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -10,9 +10,9 @@ * 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 documentation4 and/or other materials provided with the + * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 45d7957f11..8df8c696c2 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -18,7 +17,7 @@ * 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 + * "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, diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk index 13648b74ca..de0174e372 100644 --- a/src/lib/launchdetection/module.mk +++ b/src/lib/launchdetection/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. +# Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 4154e3db4a..3ff3d99229 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -40,6 +40,7 @@ */ #include "attitude_estimator_ekf_params.h" +#include /* Extended Kalman Filter covariances */ @@ -113,6 +114,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->yaw_off, &(p->yaw_off)); param_get(h->mag_decl, &(p->mag_decl)); + p->mag_decl *= M_PI / 180.0f; param_get(h->acc_comp, &(p->acc_comp)); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 819dfbe83b..f4b24cff1e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -137,7 +137,7 @@ enum MAV_MODE_FLAG { }; /* Mavlink file descriptors */ -static int mavlink_fd; +static int mavlink_fd = 0; /* flags */ static bool commander_initialized = false; @@ -217,11 +217,10 @@ void print_reject_arm(const char *msg); void print_status(); -int arm(); -int disarm(); - transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy); + /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -288,12 +287,12 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - arm(); + arm_disarm(true, mavlink_fd, "command line"); exit(0); } - if (!strcmp(argv[1], "disarm")) { - disarm(); + if (!strcmp(argv[1], "2")) { + arm_disarm(false, mavlink_fd, "command line"); exit(0); } @@ -363,30 +362,20 @@ void print_status() static orb_advert_t status_pub; -int arm() +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy) { - int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); - return 0; - - } else { - return 1; - } -} - -int disarm() -{ - int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); - return 0; - - } else { - return 1; - } + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will + // output appropriate error messages if the state cannot transition. + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); + if (arming_res == TRANSITION_CHANGED && mavlink_fd) { + mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); + } else if (arming_res == TRANSITION_DENIED) { + tune_negative(true); + } + + return arming_res; } bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) @@ -429,37 +418,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (hil_ret == OK) ret = true; - // TODO remove debug code - //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); - /* set arming state */ - arming_res = TRANSITION_NOT_CHANGED; - - if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { - print_reject_arm("NOT ARMING: Press safety switch first."); - arming_res = TRANSITION_DENIED; - - } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); - } - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); - } - - } else { - if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { - arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, safety, new_arming_state, armed); - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); - } - - } else { - arming_res = TRANSITION_NOT_CHANGED; - } - } + // Transition the arming state + arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); if (arming_res == TRANSITION_CHANGED) ret = true; @@ -518,27 +478,19 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - transition_result_t arming_res = TRANSITION_NOT_CHANGED; - - if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { - if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { - print_reject_arm("NOT ARMING: Press safety switch first."); - arming_res = TRANSITION_DENIED; - - } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); - } - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; - - } else { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } + // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. + // We use an float epsilon delta to test float equality. + if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { + mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); + } else { + transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); + if (arming_res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } + } } break; diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp index 6e72cf0d9c..0abb84a82c 100644 --- a/src/modules/commander/commander_tests/commander_tests.cpp +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -48,8 +48,7 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); int commander_tests_main(int argc, char *argv[]) { - state_machine_helper_test(); - //state_machine_test(); + stateMachineHelperTest(); return 0; } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 40bedd9f31..8a946543d1 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -49,13 +49,12 @@ public: StateMachineHelperTest(); virtual ~StateMachineHelperTest(); - virtual const char* run_tests(); + virtual void runTests(void); private: - const char* arming_state_transition_test(); - const char* arming_state_transition_arm_disarm_test(); - const char* main_state_transition_test(); - const char* is_safe_test(); + bool armingStateTransitionTest(); + bool mainStateTransitionTest(); + bool isSafeTest(); }; StateMachineHelperTest::StateMachineHelperTest() { @@ -64,61 +63,242 @@ StateMachineHelperTest::StateMachineHelperTest() { StateMachineHelperTest::~StateMachineHelperTest() { } -const char* -StateMachineHelperTest::arming_state_transition_test() +bool StateMachineHelperTest::armingStateTransitionTest(void) { + // These are the critical values from vehicle_status_s and actuator_armed_s which must be primed + // to simulate machine state prior to testing an arming state transition. This structure is also + // use to represent the expected machine state after the transition has been requested. + typedef struct { + arming_state_t arming_state; // vehicle_status_s.arming_state + bool armed; // actuator_armed_s.armed + bool ready_to_arm; // actuator_armed_s.ready_to_arm + } ArmingTransitionVolatileState_t; + + // This structure represents a test case for arming_state_transition. It contains the machine + // state prior to transtion, the requested state to transition to and finally the expected + // machine state after transition. + typedef struct { + const char* assertMsg; // Text to show when test case fails + ArmingTransitionVolatileState_t current_state; // Machine state prior to transtion + hil_state_t hil_state; // Current vehicle_status_s.hil_state + bool condition_system_sensors_initialized; // Current vehicle_status_s.condition_system_sensors_initialized + bool safety_switch_available; // Current safety_s.safety_switch_available + bool safety_off; // Current safety_s.safety_off + arming_state_t requested_state; // Requested arming state to transition to + ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition + transition_result_t expected_transition_result; // Expected result from arming_state_transition + } ArmingTransitionTest_t; + + // We use these defines so that our test cases are more readable + #define ATT_ARMED true + #define ATT_DISARMED false + #define ATT_READY_TO_ARM true + #define ATT_NOT_READY_TO_ARM false + #define ATT_SENSORS_INITIALIZED true + #define ATT_SENSORS_NOT_INITIALIZED false + #define ATT_SAFETY_AVAILABLE true + #define ATT_SAFETY_NOT_AVAILABLE true + #define ATT_SAFETY_OFF true + #define ATT_SAFETY_ON false + + // These are test cases for arming_state_transition + static const ArmingTransitionTest_t rgArmingTransitionTests[] = { + // TRANSITION_NOT_CHANGED tests + + { "no transition: identical states", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_INIT, + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED }, + + // TRANSITION_CHANGED tests + + // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s + + { "transition: init to standby", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: init to standby error", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY_ERROR, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: init to reboot", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: standby to init", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_INIT, + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: standby to standby error", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY_ERROR, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: standby to reboot", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: armed to standby", + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: armed to armed error", + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED_ERROR, + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: armed error to standby error", + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY_ERROR, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: standby error to reboot", + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: in air restore to armed", + { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: in air restore to reboot", + { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + // hil on tests, standby error to standby not normally allowed + + { "transition: standby error to standby, hil on", + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + // Safety switch arming tests + + { "transition: init to standby, no safety switch", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, + ARMING_STATE_ARMED, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: init to standby, safety switch off", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, + ARMING_STATE_ARMED, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + // standby error + { "transition: armed error to standby error requested standby", + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + // TRANSITION_DENIED tests + + // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s + + { "no transition: init to armed", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: standby to armed error", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED_ERROR, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: armed to init", + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_INIT, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: armed to reboot", + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: armed error to armed", + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: armed error to reboot", + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: standby error to armed", + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: standby error to standby", + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: reboot to armed", + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: in air restore to standby", + { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + // Sensor tests + + { "no transition: init to standby - sensors not initialized", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + // Safety switch arming tests + + { "no transition: init to standby, safety switch on", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + }; + struct vehicle_status_s status; - struct safety_s safety; - arming_state_t new_arming_state; + struct safety_s safety; struct actuator_armed_s armed; + + size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); + for (size_t i=0; icurrent_state.arming_state; + status.condition_system_sensors_initialized = test->condition_system_sensors_initialized; + status.hil_state = test->hil_state; + safety.safety_switch_available = test->safety_switch_available; + safety.safety_off = test->safety_off; + armed.armed = test->current_state.armed; + armed.ready_to_arm = test->current_state.ready_to_arm; + + // Attempt transition + transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed); + + // Validate result of transition + ut_assert(test->assertMsg, test->expected_transition_result == result); + ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state); + ut_assert(test->assertMsg, armed.armed == test->expected_state.armed); + ut_assert(test->assertMsg, armed.ready_to_arm == test->expected_state.ready_to_arm); + } - // Identical states. - status.arming_state = ARMING_STATE_INIT; - new_arming_state = ARMING_STATE_INIT; - mu_assert("no transition: identical states", - TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); - - // INIT to STANDBY. - armed.armed = false; - armed.ready_to_arm = false; - status.arming_state = ARMING_STATE_INIT; - status.condition_system_sensors_initialized = true; - new_arming_state = ARMING_STATE_STANDBY; - mu_assert("transition: init to standby", - TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); - mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state); - mu_assert("not armed", !armed.armed); - mu_assert("ready to arm", armed.ready_to_arm); - - // INIT to STANDBY, sensors not initialized. - armed.armed = false; - armed.ready_to_arm = false; - status.arming_state = ARMING_STATE_INIT; - status.condition_system_sensors_initialized = false; - new_arming_state = ARMING_STATE_STANDBY; - mu_assert("no transition: sensors not initialized", - TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed)); - mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state); - mu_assert("not armed", !armed.armed); - mu_assert("not ready to arm", !armed.ready_to_arm); - - return 0; + return true; } -const char* -StateMachineHelperTest::arming_state_transition_arm_disarm_test() -{ - struct vehicle_status_s status; - struct safety_s safety; - arming_state_t new_arming_state; - struct actuator_armed_s armed; - - // TODO(sjwilks): ARM then DISARM. - return 0; -} - -const char* -StateMachineHelperTest::main_state_transition_test() +bool StateMachineHelperTest::mainStateTransitionTest(void) { struct vehicle_status_s current_state; main_state_t new_main_state; @@ -126,70 +306,69 @@ StateMachineHelperTest::main_state_transition_test() // Identical states. current_state.main_state = MAIN_STATE_MANUAL; new_main_state = MAIN_STATE_MANUAL; - mu_assert("no transition: identical states", + ut_assert("no transition: identical states", TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); // AUTO to MANUAL. current_state.main_state = MAIN_STATE_AUTO; new_main_state = MAIN_STATE_MANUAL; - mu_assert("transition changed: auto to manual", + ut_assert("transition changed: auto to manual", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); // MANUAL to SEATBELT. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; new_main_state = MAIN_STATE_SEATBELT; - mu_assert("tranisition: manual to seatbelt", + ut_assert("tranisition: manual to seatbelt", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); // MANUAL to SEATBELT, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; new_main_state = MAIN_STATE_SEATBELT; - mu_assert("no transition: invalid local altitude", + ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); // MANUAL to EASY. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; new_main_state = MAIN_STATE_EASY; - mu_assert("transition: manual to easy", + ut_assert("transition: manual to easy", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); // MANUAL to EASY, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; new_main_state = MAIN_STATE_EASY; - mu_assert("no transition: invalid position", + ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); // MANUAL to AUTO. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_global_position_valid = true; new_main_state = MAIN_STATE_AUTO; - mu_assert("transition: manual to auto", + ut_assert("transition: manual to auto", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); + ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); // MANUAL to AUTO, invalid global position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_global_position_valid = false; new_main_state = MAIN_STATE_AUTO; - mu_assert("no transition: invalid global position", + ut_assert("no transition: invalid global position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - return 0; + return true; } -const char* -StateMachineHelperTest::is_safe_test() +bool StateMachineHelperTest::isSafeTest(void) { struct vehicle_status_s current_state; struct safety_s safety; @@ -199,49 +378,45 @@ StateMachineHelperTest::is_safe_test() armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = false; - mu_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); + ut_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); armed.armed = false; armed.lockdown = true; safety.safety_switch_available = true; safety.safety_off = true; - mu_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); + ut_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = true; - mu_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); + ut_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = false; - mu_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); + ut_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = false; safety.safety_off = false; - mu_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); + ut_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); - return 0; + return true; } -const char* -StateMachineHelperTest::run_tests() +void StateMachineHelperTest::runTests(void) { - mu_run_test(arming_state_transition_test); - mu_run_test(arming_state_transition_arm_disarm_test); - mu_run_test(main_state_transition_test); - mu_run_test(is_safe_test); - - return 0; + ut_run_test(armingStateTransitionTest); + ut_run_test(mainStateTransitionTest); + ut_run_test(isSafeTest); } -void -state_machine_helper_test() +void stateMachineHelperTest(void) { StateMachineHelperTest* test = new StateMachineHelperTest(); - test->UnitTest::print_results(test->run_tests()); + test->runTests(); + test->printResults(); } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h index 10a68e6028..bbf66255ea 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.h +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -39,6 +39,6 @@ #ifndef STATE_MACHINE_HELPER_TEST_H_ #define STATE_MACHINE_HELPER_TEST_ -void state_machine_helper_test(); +void stateMachineHelperTest(void); #endif /* STATE_MACHINE_HELPER_TEST_H_ */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e6f245cf93..f09d586c74 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -69,10 +69,44 @@ static bool arming_state_changed = true; static bool main_state_changed = true; static bool failsafe_state_changed = true; +// 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[ARMING_STATE_MAX][ARMING_STATE_MAX] = { + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* 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* state_names[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, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed) +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 + const int mavlink_fd) /// mavlink fd for error reporting, 0 for none { + // Double check that our static arrays are still valid + ASSERT(ARMING_STATE_INIT == 0); + ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); + /* * Perform an atomic state update */ @@ -83,116 +117,63 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* only check transition if the new state is actually different from the current one */ if (new_arming_state == status->arming_state) { ret = TRANSITION_NOT_CHANGED; - } else { - /* enforce lockdown in HIL */ if (status->hil_state == 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 == ARMING_STATE_ARMED) { + // Fail transition if we need safety switch press + // Allow if coming from in air restore + // Allow if HIL_STATE_ON + if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); + } + valid_transition = false; + } + } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { + new_arming_state = ARMING_STATE_STANDBY_ERROR; + } + } + + // HIL can always go to standby + if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { + valid_transition = true; + } + + /* Sensors need to be initialized for STANDBY state */ + if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + valid_transition = false; + } - switch (new_arming_state) { - case ARMING_STATE_INIT: - - /* allow going back from INIT for calibration */ - if (status->arming_state == ARMING_STATE_STANDBY) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_STANDBY: - - /* allow coming from INIT and disarming from ARMED */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED - || status->hil_state == HIL_STATE_ON) { - - /* sensors need to be initialized for STANDBY state */ - if (status->condition_system_sensors_initialized) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = true; - } - } - - break; - - case ARMING_STATE_ARMED: - - /* allow arming from STANDBY and IN-AIR-RESTORE */ - if ((status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */ - ret = TRANSITION_CHANGED; - armed->armed = true; - armed->ready_to_arm = true; - } - - break; - - case ARMING_STATE_ARMED_ERROR: - - /* an armed error happens when ARMED obviously */ - if (status->arming_state == ARMING_STATE_ARMED) { - ret = TRANSITION_CHANGED; - armed->armed = true; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_STANDBY_ERROR: - - /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ - if (status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED_ERROR) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_REBOOT: - - /* an armed error happens when ARMED obviously */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - /* XXX implement */ - break; - - default: - break; - } - - if (ret == TRANSITION_CHANGED) { - status->arming_state = new_arming_state; - arming_state_changed = true; - } - } - + // Finish up the state transition + if (valid_transition) { + armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; + ret = TRANSITION_CHANGED; + status->arming_state = new_arming_state; + arming_state_changed = true; + } + } + /* end of atomic state update */ irqrestore(flags); - if (ret == TRANSITION_DENIED) - warnx("arming transition rejected"); + if (ret == TRANSITION_DENIED) { + static const char* errMsg = "Invalid arming transition from %s to %s"; + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); + } + warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); + } return ret; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index f04879ff9e..0ddd4f05a2 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed); + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8ea6118022..46bd399f73 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -732,21 +732,21 @@ FixedwingEstimator::task_main() case 1: { const char* str = "NaN in states, resetting"; - warnx(str); + warnx("%s", str); mavlink_log_critical(_mavlink_fd, str); break; } case 2: { const char* str = "stale IMU data, resetting"; - warnx(str); + warnx("%s", str); mavlink_log_critical(_mavlink_fd, str); break; } case 3: { const char* str = "switching dynamic / static state"; - warnx(str); + warnx("%s", str); mavlink_log_critical(_mavlink_fd, str); break; } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index ad435b251b..e49288a746 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -62,6 +62,12 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); * @group MAVLink */ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); +/** + * Use/Accept HIL GPS message (even if not in HIL mode) + * If set to 1 incomming HIL GPS messages are parsed. + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); mavlink_system_t mavlink_system = { 100, diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 227e99b48a..7ecca0d65e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -208,6 +208,7 @@ Mavlink::Mavlink() : _mavlink_fd(-1), _task_running(false), _hil_enabled(false), + _use_hil_gps(false), _is_usb_uart(false), _wait_to_transmit(false), _received_messages(false), @@ -487,11 +488,13 @@ void Mavlink::mavlink_update_system(void) static param_t param_system_id; static param_t param_component_id; static param_t param_system_type; + static param_t param_use_hil_gps; if (!initialized) { param_system_id = param_find("MAV_SYS_ID"); param_component_id = param_find("MAV_COMP_ID"); param_system_type = param_find("MAV_TYPE"); + param_use_hil_gps = param_find("MAV_USEHILGPS"); initialized = true; } @@ -516,6 +519,11 @@ void Mavlink::mavlink_update_system(void) if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { mavlink_system.type = system_type; } + + int32_t use_hil_gps; + param_get(param_use_hil_gps, &use_hil_gps); + + _use_hil_gps = (bool)use_hil_gps; } int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 66d82b4718..1bf51fd31e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -157,6 +157,8 @@ public: bool get_hil_enabled() { return _hil_enabled; } + bool get_use_hil_gps() { return _use_hil_gps; } + bool get_flow_control_enabled() { return _flow_control_enabled; } bool get_forwarding_on() { return _forwarding_on; } @@ -223,6 +225,7 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ bool _is_usb_uart; /**< Port is USB */ bool _wait_to_transmit; /**< Wait to transmit until received messages. */ bool _received_messages; /**< Whether we've received valid mavlink messages. */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b4fe65fd28..fc2998646d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -162,6 +162,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) * The HIL mode is enabled by the HIL bit flag * in the system mode. Either send a set mode * COMMAND_LONG message or a SET_MODE message + * + * Accept HIL GPS messages if use_hil_gps flag is true. + * This allows to provide fake gps measurements to the system. */ if (_mavlink->get_hil_enabled()) { switch (msg->msgid) { @@ -169,10 +172,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_hil_sensor(msg); break; - case MAVLINK_MSG_ID_HIL_GPS: - handle_message_hil_gps(msg); - break; - case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: handle_message_hil_state_quaternion(msg); break; @@ -182,7 +181,20 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } } - /* If we've received a valid message, mark the flag indicating so. + + if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_GPS: + handle_message_hil_gps(msg); + break; + + default: + break; + } + + } + + /* If we've received a valid message, mark the flag indicating so. This is used in the '-w' command-line flag. */ _mavlink->set_has_received_messages(true); } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4083b8b4f5..214553d19a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -635,39 +635,39 @@ Sensors::parameters_update() /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } // if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { @@ -737,12 +737,12 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } /* scaling of ADC ticks to battery current */ if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); @@ -1254,7 +1254,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } _last_adc = t; - if (_battery_status.voltage_v > 0.0f) { + if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) { /* announce the battery status if needed, just publish else */ if (_battery_pub > 0) { orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); @@ -1571,12 +1571,10 @@ Sensors::task_main() while (!_task_should_exit) { - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + /* wait for up to 50ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) - continue; + /* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */ /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -1615,7 +1613,7 @@ Sensors::task_main() perf_end(_loop_perf); } - printf("[sensors] exiting.\n"); + warnx("[sensors] exiting."); _sensors_task = -1; _exit(0); diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 56be4d2412..256c5134cb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -69,6 +69,8 @@ typedef enum { MAIN_STATE_MAX } main_state_t; +// If you change the order, add or remove arming_state_t states make sure to update the arrays +// in state_machine_helper.cpp as well. typedef enum { ARMING_STATE_INIT = 0, ARMING_STATE_STANDBY, diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp index 64ee544a27..02d1af4818 100644 --- a/src/modules/unit_test/unit_test.cpp +++ b/src/modules/unit_test/unit_test.cpp @@ -32,17 +32,10 @@ * ****************************************************************************/ -/** - * @file unit_test.cpp - * A unit test library. - * - */ - #include "unit_test.h" #include - UnitTest::UnitTest() { } @@ -51,15 +44,15 @@ UnitTest::~UnitTest() { } -void -UnitTest::print_results(const char* result) +void UnitTest::printResults(void) { - if (result != 0) { - warnx("Failed: %s:%d", mu_last_test(), mu_line()); - warnx("%s", result); - } else { - warnx("ALL TESTS PASSED"); - warnx(" Tests run : %d", mu_tests_run()); - warnx(" Assertion : %d", mu_assertion()); - } + warnx(mu_tests_failed() ? "SOME TESTS FAILED" : "ALL TESTS PASSED"); + warnx(" Tests passed : %d", mu_tests_passed()); + warnx(" Tests failed : %d", mu_tests_failed()); + warnx(" Assertions : %d", mu_assertion()); +} + +void UnitTest::printAssert(const char* msg, const char* test, const char* file, int line) +{ + warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); } diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 3020734f4b..32eb8c308c 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -32,62 +32,55 @@ * ****************************************************************************/ -/** - * @file unit_test.h - * A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html). - * - */ - #ifndef UNIT_TEST_H_ -#define UNIT_TEST_ +#define UNIT_TEST_H_ #include - class __EXPORT UnitTest { public: -#define xstr(s) str(s) -#define str(s) #s #define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; } INLINE_GLOBAL(int, mu_tests_run) +INLINE_GLOBAL(int, mu_tests_failed) +INLINE_GLOBAL(int, mu_tests_passed) INLINE_GLOBAL(int, mu_assertion) INLINE_GLOBAL(int, mu_line) INLINE_GLOBAL(const char*, mu_last_test) -#define mu_assert(message, test) \ - do \ - { \ - if (!(test)) \ - return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \ - else \ - mu_assertion()++; \ - } while (0) - - -#define mu_run_test(test) \ -do \ -{ \ - const char *message; \ - mu_last_test() = #test; \ - mu_line() = __LINE__; \ - message = test(); \ - mu_tests_run()++; \ - if (message) \ - return message; \ -} while (0) - - -public: UnitTest(); virtual ~UnitTest(); - virtual const char* run_tests() = 0; - virtual void print_results(const char* result); + virtual void runTests(void) = 0; + void printResults(void); + + void printAssert(const char* msg, const char* test, const char* file, int line); + +#define ut_assert(message, test) \ + do { \ + if (!(test)) { \ + printAssert(message, #test, __FILE__, __LINE__); \ + return false; \ + } else { \ + mu_assertion()++; \ + } \ + } while (0) + +#define ut_run_test(test) \ + do { \ + warnx("RUNNING TEST: %s", #test); \ + mu_tests_run()++; \ + if (!test()) { \ + warnx("TEST FAILED: %s", #test); \ + mu_tests_failed()++; \ + } else { \ + warnx("TEST PASSED: %s", #test); \ + mu_tests_passed()++; \ + } \ + } while (0) + }; - - #endif /* UNIT_TEST_H_ */ diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 476015f3e3..e6d4b763be 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -121,7 +121,7 @@ do_device(int argc, char *argv[]) errx(ret,"uORB publications could not be unblocked"); } else { - errx("no valid command: %s", argv[1]); + errx(1, "no valid command: %s", argv[1]); } }