From c7bf00562d685b513e1720f558fee5840aca151b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 08:22:54 +0200 Subject: commander: Enforce (in presence of power sensing) that a) system is not purely servo rail powered and b) power rail voltage is higher than 4.5V on the main avionics rail --- src/modules/commander/state_machine_helper.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) (limited to 'src/modules/commander/state_machine_helper.cpp') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87309b238..818974648 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -144,6 +144,28 @@ arming_state_transition(struct vehicle_status_s *status, /// current valid_transition = false; } + // Fail transition if power is not good + if (!status->condition_power_input_valid) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + } + + valid_transition = false; + } + + // Fail transition if power levels on the avionics rail + // are insufficient + if ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.5f)) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + } + + 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; } -- cgit v1.2.3 From 064329dd099819b0f0b8a9a7bc0233440fdf6df1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:34:32 +0200 Subject: commander: put circuit breaker into effect --- src/modules/commander/commander.cpp | 7 ++++++ src/modules/commander/state_machine_helper.cpp | 32 +++++++++++++++----------- 2 files changed, 25 insertions(+), 14 deletions(-) (limited to 'src/modules/commander/state_machine_helper.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 55c74fdff..71067ac4f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -721,6 +722,9 @@ int commander_thread_main(int argc, char *argv[]) status.condition_power_input_valid = true; status.avionics_power_rail_voltage = -1.0f; + // CIRCUIT BREAKERS + status.circuit_breaker_engaged_power_check = false; + /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -907,6 +911,9 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); + + status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status_changed = true; /* re-check RC calibration */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 818974648..84f4d03af 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -144,26 +144,30 @@ arming_state_transition(struct vehicle_status_s *status, /// current valid_transition = false; } - // Fail transition if power is not good - if (!status->condition_power_input_valid) { + // Perform power checks only if circuit breaker is not + // engaged for these checks + if (!status->circuit_breaker_engaged_power_check) { + // Fail transition if power is not good + if (!status->condition_power_input_valid) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + } - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + valid_transition = false; } - valid_transition = false; - } + // Fail transition if power levels on the avionics rail + // are insufficient + if ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.5f)) { - // Fail transition if power levels on the avionics rail - // are insufficient - if ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.5f)) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + } - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + valid_transition = false; } - - valid_transition = false; } } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { -- cgit v1.2.3 From 571e139c24464902470901501c5bbfb618c1258c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Jun 2014 21:33:03 +0200 Subject: commander: Formatting and logic flow cleanup, no functional change --- src/modules/commander/state_machine_helper.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) (limited to 'src/modules/commander/state_machine_helper.cpp') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87309b238..1a5a52a02 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * 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 @@ -36,6 +34,9 @@ /** * @file state_machine_helper.cpp * State machine helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes */ #include @@ -133,15 +134,20 @@ arming_state_transition(struct vehicle_status_s *status, /// current 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, "#audio: NOT ARMING: Press safety switch first."); - } + if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && + status->hil_state == HIL_STATE_OFF) { + + // Fail transition if we need safety switch press + if (safety->safety_switch_available && !safety->safety_off) { - valid_transition = false; + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); + } + + valid_transition = false; + } } } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { -- cgit v1.2.3 From a1132580c55512a56ec7646f757099409ebad781 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:11:39 +0200 Subject: commander: Add pre-arm check for main sensors. --- src/modules/commander/state_machine_helper.cpp | 135 +++++++++++++++++++++---- 1 file changed, 118 insertions(+), 17 deletions(-) (limited to 'src/modules/commander/state_machine_helper.cpp') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1a5a52a02..6816d46ab 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -46,14 +46,18 @@ #include #include #include +#include #include #include #include +#include #include #include #include #include +#include +#include #include #include @@ -66,6 +70,8 @@ #endif static const int ERROR = -1; +static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); + static bool arming_state_changed = true; static bool main_state_changed = true; static bool failsafe_state_changed = true; @@ -108,18 +114,31 @@ arming_state_transition(struct vehicle_status_s *status, /// current ASSERT(ARMING_STATE_INIT == 0); ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); - /* - * Perform an atomic state update - */ - irqstate_t flags = irqsave(); - transition_result_t ret = TRANSITION_DENIED; + arming_state_t current_arming_state = status->arming_state; + /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == status->arming_state) { + if (new_arming_state == current_arming_state) { ret = TRANSITION_NOT_CHANGED; } else { + + /* + * Get sensing state if necessary + */ + int prearm_ret = OK; + + /* only perform the check if we have to */ + if (new_arming_state == ARMING_STATE_ARMED) { + prearm_ret = prearm_check(status, mavlink_fd); + } + + /* + * Perform an atomic state update + */ + irqstate_t flags = irqsave(); + /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; @@ -139,12 +158,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF) { + // Fail transition if pre-arm check fails + if (prearm_ret) { + valid_transition = false; + // Fail transition if we need safety switch press - if (safety->safety_switch_available && !safety->safety_off) { + } else if (safety->safety_switch_available && !safety->safety_off) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); - } + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!"); valid_transition = false; } @@ -173,17 +194,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current status->arming_state = new_arming_state; arming_state_changed = true; } - } - /* end of atomic state update */ - irqrestore(flags); + /* end of atomic state update */ + irqrestore(flags); + } if (ret == TRANSITION_DENIED) { - static const char *errMsg = "Invalid arming transition from %s to %s"; + static const char *errMsg = "INVAL: %s - %s"; - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); - } + 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]); } @@ -496,6 +515,88 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f return ret; } +int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) +{ + warnx("PREARM"); + + int ret; + + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); + ret = fd; + goto system_eval; + } + + ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + goto system_eval; + } + + /* check measurement result range */ + struct accel_report acc; + ret = read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + + if (accel_scale < 9.78f || accel_scale > 9.83f) { + mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); + } + + if (accel_scale > 30.0f /* m/s^2 */) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); + /* this is frickin' fatal */ + ret = ERROR; + goto system_eval; + } else { + ret = OK; + } + } else { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); + /* this is frickin' fatal */ + ret = ERROR; + goto system_eval; + } + + if (!status->is_rotary_wing) { + int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); + ret = fd; + goto system_eval; + } + + struct differential_pressure_s diff_pres; + + ret = read(fd, &diff_pres, sizeof(diff_pres)); + + if (ret == sizeof(diff_pres)) { + if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { + mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + // XXX do not make this fatal yet + ret = OK; + } + } else { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); + /* this is frickin' fatal */ + ret = ERROR; + goto system_eval; + } + } + + warnx("PRE RES: %d", ret); + +system_eval: + close(fd); + return ret; +} + // /* -- cgit v1.2.3 From b20d0c0f015fb62955f7a399b50626a0d57c5f64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:36:43 +0200 Subject: commander: Remove prearm test print statements --- src/modules/commander/state_machine_helper.cpp | 4 ---- 1 file changed, 4 deletions(-) (limited to 'src/modules/commander/state_machine_helper.cpp') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6816d46ab..8ce724b4c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -517,8 +517,6 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) { - warnx("PREARM"); - int ret; int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -590,8 +588,6 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } } - warnx("PRE RES: %d", ret); - system_eval: close(fd); return ret; -- cgit v1.2.3 From 799f568ab48ca3446b24f41290589f7f3c39ef0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 14:33:56 +0200 Subject: commander: Minor checks / improvements to power enforce patch --- src/modules/commander/commander.cpp | 4 +++- src/modules/commander/state_machine_helper.cpp | 16 +++++----------- 2 files changed, 8 insertions(+), 12 deletions(-) (limited to 'src/modules/commander/state_machine_helper.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 507f18e24..a15712a91 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -974,8 +974,10 @@ int commander_thread_main(int argc, char *argv[]) status.condition_power_input_valid = false; } else { status.condition_power_input_valid = true; - status.avionics_power_rail_voltage = system_power.voltage5V_v; } + + /* copy avionics voltage */ + status.avionics_power_rail_voltage = system_power.voltage5V_v; } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1997729d4..7a61d481b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -141,22 +141,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if power is not good if (!status->condition_power_input_valid) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); - } - + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); valid_transition = false; } // Fail transition if power levels on the avionics rail - // are insufficient - if ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.5f)) { - - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); - } + // are measured but are insufficient + if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.9f)) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); valid_transition = false; } } -- cgit v1.2.3 From fb98251e4d08d351c7ba404e3816958435c57515 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:52:36 +0200 Subject: Warning fixes in commander --- src/modules/commander/state_machine_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/commander/state_machine_helper.cpp') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 244513375..423ce2f23 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -182,7 +182,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && (status->avionics_power_rail_voltage < 4.9f)) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); valid_transition = false; } } @@ -638,7 +638,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { - int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); -- cgit v1.2.3