From a2da34be16531404b4c4507ab71b0518a51d60c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 20:53:01 +0200 Subject: [PATCH 1/4] Fixes #1207 --- src/modules/mavlink/mavlink_main.cpp | 31 +++++++++++++++++++++------- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 76b5459a3e..e235ff0f9c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -525,22 +525,39 @@ void Mavlink::mavlink_update_system(void) _param_component_id = param_find("MAV_COMP_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); - _param_initialized = true; } /* update system and component id */ int32_t system_id; param_get(_param_system_id, &system_id); - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - int32_t component_id; param_get(_param_component_id, &component_id); - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; + + /* only allow system ID and component ID updates + * after reboot - not during operation */ + if (!_param_initialized) { + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + _param_initialized = true; + } + + /* warn users that they need to reboot to take this + * into effect + */ + if (system_id != mavlink_system.sysid) { + send_statustext_critical("Save params and reboot to change SYSID"); + } + + if (component_id != mavlink_system.compid) { + send_statustext_critical("Save params and reboot to change COMPID"); } int32_t system_type; From 54b9698d6560290e386b8dd2a7b9f0b6f4c5f57a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Jul 2014 17:48:45 +0200 Subject: [PATCH 2/4] circuit_breakers: added param to disable airspeed check --- src/modules/commander/commander.cpp | 2 ++ src/modules/commander/state_machine_helper.cpp | 4 +++- src/modules/systemlib/circuit_breaker.c | 12 ++++++++++++ src/modules/systemlib/circuit_breaker.h | 1 + src/modules/uORB/topics/vehicle_status.h | 1 + 5 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a0c8961788..daba4e7403 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -736,6 +736,7 @@ int commander_thread_main(int argc, char *argv[]) // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; + status.circuit_breaker_engaged_airspd_check = false; /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -977,6 +978,7 @@ int commander_thread_main(int argc, char *argv[]) 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.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); status_changed = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7b26e3e8cb..3c3d2f2335 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -669,7 +669,9 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) goto system_eval; } - if (!status->is_rotary_wing) { + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { /* accel done, close it */ close(fd); fd = orb_subscribe(ORB_ID(airspeed)); diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 8f697749ed..64317a18a8 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -83,6 +83,18 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); */ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); +/** + * Circuit breaker for airspeed sensor + * + * Setting this parameter to 162128 will disable the check for an airspeed sensor. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 162128 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 1175dbce86..b605846082 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -52,6 +52,7 @@ #define CBRK_SUPPLY_CHK_KEY 894281 #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 +#define CBRK_AIRSPD_CHK_KEY 162128 #include diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index cbb148edaf..b683bf98ad 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -226,6 +226,7 @@ struct vehicle_status_s { uint16_t errors_count4; bool circuit_breaker_engaged_power_check; + bool circuit_breaker_engaged_airspd_check; }; /** From 5d36381dc5f3bbb1eefc70158680f9622ac437b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Jul 2014 23:38:14 +0200 Subject: [PATCH 3/4] EKF: Fix wind publication, fix commented out flags --- .../ekf_att_pos_estimator_main.cpp | 27 +++---------------- 1 file changed, 4 insertions(+), 23 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index ccc497323c..91d17e787c 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -630,10 +630,10 @@ FixedwingEstimator::check_filter_state() rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); - // rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); - // rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); - // rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); - // rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); + rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); + rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); + rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); + rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); @@ -1465,25 +1465,6 @@ FixedwingEstimator::task_main() _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = 0.0f; // XXX get form filter - _wind.covariance_east = 0.0f; - - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); - } - - } - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; From 67db8ee4f0908c31f17ed490c48ea21cc7ac3f86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Jul 2014 14:27:10 +0200 Subject: [PATCH 4/4] Add missing states, build fix for master --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 4 ++++ src/modules/ekf_att_pos_estimator/estimator_utilities.h | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 3d504d3955..ffdd29a5b7 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -3028,6 +3028,10 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) current_ekf_state.states[i] = states[i]; } current_ekf_state.n_states = n_states; + current_ekf_state.onGround = onGround; + current_ekf_state.staticMode = staticMode; + current_ekf_state.useCompass = useCompass; + current_ekf_state.useAirspeed = useAirspeed; memcpy(err, ¤t_ekf_state, sizeof(*err)); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 6d1f47b685..a6b670c4d2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -68,6 +68,10 @@ struct ekf_status_report { bool posTimeout; bool hgtTimeout; bool imuTimeout; + bool onGround; + bool staticMode; + bool useCompass; + bool useAirspeed; uint32_t velFailTime; uint32_t posFailTime; uint32_t hgtFailTime;