From 4ef87206eccd292eb5111bba7d5f39dd03f7e20c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:03:49 +0200 Subject: Code formatting and warning fixes --- src/drivers/blinkm/blinkm.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 3fabfd9a5..9fed1149d 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -486,15 +486,15 @@ BlinkM::led() /* get number of used satellites in navigation */ num_of_used_sats = 0; - //for(int satloop=0; satloop<20; satloop++) { - for(int satloop=0; satloop checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { -- cgit v1.2.3 From 1deced7629e7d140a931c42657f75da512696c7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:09:09 +0200 Subject: Added safety status feedback, disallow arming of a rotary wing with engaged safety --- src/drivers/px4io/px4io.cpp | 33 ++++++++++- src/modules/commander/commander.c | 82 ++++++++++++++++++---------- src/modules/commander/state_machine_helper.c | 35 +++++++++++- src/modules/commander/state_machine_helper.h | 4 ++ src/modules/uORB/objects_common.cpp | 4 ++ src/modules/uORB/topics/actuator_controls.h | 7 ++- src/modules/uORB/topics/safety.h | 60 ++++++++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 + 8 files changed, 189 insertions(+), 38 deletions(-) create mode 100644 src/modules/uORB/topics/safety.h (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 19163cebe..bc65c4f25 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -80,6 +80,7 @@ #include #include #include +#include #include #include @@ -161,6 +162,7 @@ private: orb_advert_t _to_actuators_effective; ///< effective actuator controls topic orb_advert_t _to_outputs; ///< mixed servo outputs topic orb_advert_t _to_battery; ///< battery status / voltage + orb_advert_t _to_safety; ///< status of safety actuator_outputs_s _outputs; ///< mixed outputs actuator_controls_effective_s _controls_effective; ///< effective controls @@ -883,6 +885,25 @@ PX4IO::io_handle_status(uint16_t status) _status = status; } + /** + * Get and handle the safety status + */ + struct safety_s safety; + safety.timestamp = hrt_absolute_time(); + + if (status & PX4IO_P_STATUS_FLAGS_ARMED) { + safety.status = SAFETY_STATUS_UNLOCKED; + } else { + safety.status = SAFETY_STATUS_SAFE; + } + + /* lazily publish the safety status */ + if (_to_safety > 0) { + orb_publish(ORB_ID(safety), _to_safety, &safety); + } else { + _to_safety = orb_advertise(ORB_ID(safety), &safety); + } + return ret; } @@ -912,7 +933,7 @@ PX4IO::io_get_status() io_handle_status(regs[0]); io_handle_alarms(regs[1]); - + /* only publish if battery has a valid minimum voltage */ if (regs[2] > 3300) { battery_status_s battery_status; @@ -946,6 +967,7 @@ PX4IO::io_get_status() _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); } } + return ret; } @@ -1273,7 +1295,7 @@ PX4IO::print_status() uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, - ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " SAFETY_UNLOCKED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), @@ -1634,6 +1656,11 @@ test(void) if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) err(1, "failed to get servo count"); + /* tell IO that its ok to disable its safety with the switch */ + ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (ret != OK) + err(1, "PWM_SERVO_SET_ARM_OK"); + if (ioctl(fd, PWM_SERVO_ARM, 0)) err(1, "failed to arm servos"); @@ -1682,7 +1709,7 @@ test(void) /* Check if user wants to quit */ char c; if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); close(console); exit(0); diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index aab8f3e04..937c515e6 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -81,6 +81,7 @@ #include #include #include +#include #include #include @@ -1281,6 +1282,8 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_vector_flight_mode_ok = false; /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + /* set safety device detection flag */ + current_status.flag_safety_present = false; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1377,6 +1380,12 @@ int commander_thread_main(int argc, char *argv[]) memset(&battery, 0, sizeof(battery)); battery.voltage_v = 0.0f; + /* subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(safety)); + struct safety_s safety; + memset(&safety, 0, sizeof(safety)); + safety.status = SAFETY_STATUS_NOT_PRESENT; + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1396,6 +1405,39 @@ int commander_thread_main(int argc, char *argv[]) /* Get current values */ bool new_data; + + /* update parameters */ + orb_check(param_changed_sub, &new_data); + + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + + /* update parameters */ + if (!current_status.flag_system_armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed setting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + current_status.flag_external_manual_override_ok = false; + + } else { + current_status.flag_external_manual_override_ok = true; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + + } + } + orb_check(sp_man_sub, &new_data); if (new_data) { @@ -1431,36 +1473,17 @@ int commander_thread_main(int argc, char *argv[]) handle_command(stat_pub, ¤t_status, &cmd); } - /* update parameters */ - orb_check(param_changed_sub, &new_data); - - if (new_data || param_init_forced) { - param_init_forced = false; - /* parameters changed */ - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - - - /* update parameters */ - if (!current_status.flag_system_armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed setting new system type"); - } - - /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; - - } else { - current_status.flag_external_manual_override_ok = true; - } + orb_check(safety_sub, &new_data); - /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); + if (new_data) { + /* got safety change */ + orb_copy(ORB_ID(safety), safety_sub, &safety); - } + /* handle it */ + current_status.flag_safety_present = (safety.status != SAFETY_STATUS_NOT_PRESENT); + + if (current_status.flag_safety_present) + current_status.flag_safety_safe = (safety.status == SAFETY_STATUS_SAFE); } /* update global position estimate */ @@ -1699,7 +1722,8 @@ int commander_thread_main(int argc, char *argv[]) state_changed = true; } - if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { + orb_check(ORB_ID(vehicle_gps_position), &new_data); + if (new_data) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ab728c7bb..ac603abfd 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -65,6 +65,18 @@ static const char *system_state_txt[] = { }; +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status); +} + /** * Transition from one state to another */ @@ -513,6 +525,25 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_STANDBY) { + + /* reject arming if safety status is wrong */ + if (current_status->flag_safety_present) { + + /* + * The operator should not touch the vehicle if + * its armed, so we don't allow arming in the + * first place + */ + if (is_rotary_wing(current_status)) { + + /* safety is in safe position, disallow arming */ + if (current_status->flag_safety_safe) { + mavlink_log_critical(mavlink_fd, "DISENGAGE SAFETY BEFORE ARMING!"); + } + + } + } + printf("[cmd] arming\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); } @@ -538,9 +569,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c current_status->flag_control_manual_enabled = true; /* set behaviour based on airframe */ - if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { + if (is_rotary_wing(current_status)) { /* assuming a rotary wing, set to SAS */ current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 2f2ccc729..95b48d326 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -47,6 +47,10 @@ #include #include +bool is_multirotor(const struct vehicle_status_s *current_status); + +bool is_rotary_wing(const struct vehicle_status_s *current_status); + /** * Switch to new state with no checking. * diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4197f6fb2..e2df31651 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -154,3 +154,7 @@ ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); + +/* status of the system safety device */ +#include "topics/safety.h" +ORB_DEFINE(safety, struct safety_s); diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index b7c4196c0..745c5bc87 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -68,9 +68,10 @@ ORB_DECLARE(actuator_controls_3); /** global 'actuator output is live' control. */ struct actuator_armed_s { - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool throttle_locked; /**< Set to true if the trottle is locked to zero */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ }; ORB_DECLARE(actuator_armed); diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h new file mode 100644 index 000000000..19e8e8d45 --- /dev/null +++ b/src/modules/uORB/topics/safety.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file safety.h + * + * Status of an attached safety device + */ + +#ifndef TOPIC_SAFETY_H +#define TOPIC_SAFETY_H + +#include +#include "../uORB.h" + +enum SAFETY_STATUS { + SAFETY_STATUS_NOT_PRESENT, + SAFETY_STATUS_SAFE, + SAFETY_STATUS_UNLOCKED +}; + +struct safety_s { + uint64_t timestamp; /**< output timestamp in us since system boot */ + enum SAFETY_STATUS status; +}; + +/* actuator output sets; this list can be expanded as more drivers emerge */ +ORB_DECLARE(safety); + +#endif /* TOPIC_SAFETY_H */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index c7c1048f6..07660614b 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -214,6 +214,8 @@ struct vehicle_status_s bool flag_valid_launch_position; /**< indicates a valid launch position */ bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ + bool flag_safety_present; /**< indicates that a safety switch is present */ + bool flag_safety_safe; /**< safety switch is in safe position */ }; /** -- cgit v1.2.3 From eb76d116cc67c6354c29fa41e49b4cf9df1a472d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Jun 2013 12:30:42 +0200 Subject: Minor state machine improvements and fixes for IO safety / in-air restart handling --- src/drivers/px4io/px4io.cpp | 70 +++++++++++++++++++++------- src/modules/commander/state_machine_helper.c | 3 ++ src/modules/mavlink/orb_listener.c | 2 +- src/modules/px4iofirmware/mixer.cpp | 11 +++-- src/modules/px4iofirmware/protocol.h | 5 +- src/modules/px4iofirmware/registers.c | 13 +++--- src/modules/px4iofirmware/safety.c | 10 ++-- 7 files changed, 78 insertions(+), 36 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bc65c4f25..f033382a6 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -336,6 +336,7 @@ PX4IO::PX4IO() : _to_actuators_effective(0), _to_outputs(0), _to_battery(0), + _to_safety(0), _primary_pwm_device(false), _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), @@ -389,6 +390,30 @@ PX4IO::init() */ _retries = 2; + /* get IO's last seen FMU state */ + int val = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); + if (val == _io_reg_get_error) { + mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! FAILED READING STATE"); + } + uint16_t arming = val; + + /* get basic software version */ + /* basic configuration */ + usleep(5000); + + unsigned proto_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + unsigned sw_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION); + + if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION) { + mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! PROTO VER MISMATCH: v%u vs v%u\n", + proto_version, + PX4IO_P_CONFIG_PROTOCOL_VERSION); + + mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); + log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION); + return 1; + } + /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); @@ -414,21 +439,23 @@ PX4IO::init() * in this case. */ - uint16_t reg; + printf("arming 0x%04x%s%s%s%s\n", + arming, + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); - /* get IO's last seen FMU state */ - ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); - if (ret != OK) - return ret; /* * in-air restart is only tried if the IO board reports it is * already armed, and has been configured for in-air restart */ - if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && - (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + if ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && + (arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + log("INAIR RESTART RECOVERY (needs commander app running)"); /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO @@ -482,7 +509,7 @@ PX4IO::init() cmd.confirmation = 1; /* send command once */ - (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd); /* spin here until IO's state has propagated into the system */ do { @@ -492,16 +519,22 @@ PX4IO::init() orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); } - /* wait 10 ms */ - usleep(10000); + /* wait 50 ms */ + usleep(50000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { + if ((hrt_absolute_time() - try_start_time)/1000 > 2000) { log("failed to recover from in-air restart (2), aborting IO driver init."); return 1; } - /* keep waiting for state change for 10 s */ + /* re-send if necessary */ + if (!status.flag_system_armed) { + orb_publish(ORB_ID(vehicle_command), pub, &cmd); + log("re-sending arm cmd"); + } + + /* keep waiting for state change for 2 s */ } while (!status.flag_system_armed); /* regular boot, no in-air restart, init IO */ @@ -540,7 +573,7 @@ PX4IO::init() return -errno; } - mavlink_log_info(_mavlink_fd, "[IO] init ok"); + mavlink_log_info(_mavlink_fd, "[IO] init ok (sw v.%u)", sw_version); return OK; } @@ -863,14 +896,14 @@ PX4IO::io_handle_status(uint16_t status) */ /* check for IO reset - force it back to armed if necessary */ - if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED) + if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the arming flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC); + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; - _status &= PX4IO_P_STATUS_FLAGS_ARMED; + _status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the sync flag */ @@ -891,7 +924,7 @@ PX4IO::io_handle_status(uint16_t status) struct safety_s safety; safety.timestamp = hrt_absolute_time(); - if (status & PX4IO_P_STATUS_FLAGS_ARMED) { + if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.status = SAFETY_STATUS_UNLOCKED; } else { safety.status = SAFETY_STATUS_SAFE; @@ -1295,7 +1328,8 @@ PX4IO::print_status() uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, - ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " SAFETY_UNLOCKED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 0a6cfd0b5..f42caad57 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -545,6 +545,9 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s /* play warning tune */ tune_error(); + + /* abort */ + return; } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 295cd5e28..9b2d984f0 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -659,7 +659,7 @@ uorb_receive_thread(void *arg) /* handle the poll result */ if (poll_ret == 0) { - mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); + /* silent */ } else if (poll_ret < 0) { mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 123eb6778..b654bdec4 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -194,7 +194,7 @@ mixer_tick(void) */ bool should_arm = ( /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && /* there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) && /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) @@ -204,11 +204,15 @@ mixer_tick(void) /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; + r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; + isr_debug(5, "> armed"); } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); + isr_debug(5, "> disarmed"); } if (mixer_servos_armed) { @@ -263,9 +267,8 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* do not allow a mixer change while outputs armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { return; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 674f9dddd..497e6af8e 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -77,7 +77,7 @@ /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 2 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ @@ -93,7 +93,7 @@ #define PX4IO_P_STATUS_CPULOAD 1 #define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ -#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ #define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ #define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ @@ -105,6 +105,7 @@ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd3..a092fc93b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -317,9 +317,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * so that an in-air reset of FMU can not lead to a * lockup of the IO arming state. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; - } + + // XXX do not reset IO's safety state by FMU for now + // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + // } r_setup_arming = value; @@ -367,9 +369,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { - /* do not allow a RC config change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* do not allow a RC config change while outputs armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 4dbecc274..95335f038 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -110,7 +110,7 @@ safety_check_button(void *arg) * state machine, keep ARM_COUNTER_THRESHOLD the same * length in all cases of the if/else struct below. */ - if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { @@ -118,18 +118,18 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* switch to armed state */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } - } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } @@ -140,7 +140,7 @@ safety_check_button(void *arg) /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; - if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_IO_FMU_ARMED; -- cgit v1.2.3 From c3a8f177b6316a9cefd814e312742f47d3049739 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Jun 2013 12:58:17 +0200 Subject: Software version check fixes --- src/drivers/px4io/px4io.cpp | 16 +++++++++++++--- src/modules/px4iofirmware/protocol.h | 7 +++++-- src/modules/px4iofirmware/registers.c | 4 ++-- 3 files changed, 20 insertions(+), 7 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f033382a6..925041e0c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -404,13 +404,23 @@ PX4IO::init() unsigned proto_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); unsigned sw_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION); - if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION) { + if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC) { mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! PROTO VER MISMATCH: v%u vs v%u\n", proto_version, - PX4IO_P_CONFIG_PROTOCOL_VERSION); + PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC); mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); - log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION); + log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC); + return 1; + } + + if (sw_version != PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC) { + mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! SOFTWARE VER MISMATCH: v%u vs v%u\n", + proto_version, + PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC); + + mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); + log("software version mismatch (v%u on IO vs v%u on FMU)", sw_version, PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC); return 1; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 497e6af8e..6c6c7b4e2 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -75,10 +75,13 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) +#define PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC 2 +#define PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC 2 + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 2 /* magic numbers TBD */ -#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers */ +#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a092fc93b..148514455 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -57,8 +57,8 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt * Static configuration parameters. */ static const uint16_t r_page_config[] = { - [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC, + [PX4IO_P_CONFIG_SOFTWARE_VERSION] = PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC, [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, -- cgit v1.2.3