From b9e9f62bee8f9787d75eaf69df6b8bd2e040fba9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 8 Jun 2016 17:14:33 +0200 Subject: [PATCH] code style formatting --- src/modules/ekf2/ekf2_main.cpp | 29 +++++++++++++------- src/modules/ekf2_replay/ekf2_replay_main.cpp | 27 +++++++++--------- 2 files changed, 33 insertions(+), 23 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index bc5b81a758..ea333769bc 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -258,9 +258,10 @@ private: control::BlockParamFloat _ev_pos_y; // Y position of VI sensor focal point in body frame (m) control::BlockParamFloat _ev_pos_z; // Z position of VI sensor focal point in body frame (m) // control of airspeed and sideslip fusion - control::BlockParamFloat _arspFusionThreshold; // a value of zero will disabled airspeed fusion. Any another positive value will determine - // the minimum airspeed which will still be fused - + control::BlockParamFloat + _arspFusionThreshold; // a value of zero will disabled airspeed fusion. Any another positive value will determine + // the minimum airspeed which will still be fused + // output predictor filter time constants control::BlockParamFloat _tau_vel; // time constant used by the output velocity complementary filter (s) control::BlockParamFloat _tau_pos; // time constant used by the output position complementary filter (s) @@ -453,11 +454,11 @@ void Ekf2::task_main() orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors); // update all other topics if they have new data - orb_check(_status_sub, &vehicle_status_updated); + orb_check(_status_sub, &vehicle_status_updated); if (vehicle_status_updated) { - orb_copy(ORB_ID(vehicle_status), _status_sub, &_vehicle_status); - } + orb_copy(ORB_ID(vehicle_status), _status_sub, &_vehicle_status); + } orb_check(_gps_sub, &gps_updated); @@ -534,11 +535,13 @@ void Ekf2::task_main() } // only set airspeed data if condition for airspeed fusion are met - bool fuse_airspeed = airspeed_updated && !_vehicle_status.is_rotary_wing && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s; - if (fuse_airspeed) { - float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s; + bool fuse_airspeed = airspeed_updated && !_vehicle_status.is_rotary_wing + && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s; + + if (fuse_airspeed) { + float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s; _ekf.setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s, &eas2tas); - } + } if (optical_flow_updated) { flow_message flow; @@ -569,18 +572,23 @@ void Ekf2::task_main() ev_data.posNED(2) = ev.z; Quaternion q(ev.q); ev_data.quat = q; + // position measurement error if (ev.pos_err >= 0.001f) { ev_data.posErr = ev.pos_err; + } else { ev_data.posErr = _default_ev_pos_noise; } + // angle measurement error if (ev.ang_err >= 0.001f) { ev_data.angErr = ev.ang_err; + } else { ev_data.angErr = _default_ev_ang_noise; } + // use timestamp from external computer - requires clocks to be synchronised so may not be a good idea _ekf.setExtVisionData(ev.timestamp_computer, &ev_data); } @@ -861,6 +869,7 @@ void Ekf2::task_main() _ekf.copy_mag_decl_deg(&decl_deg); _mag_declination_deg.set(decl_deg); } + _prev_landed = vehicle_land_detected.landed; // publish replay message if in replay mode diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index df74663ee7..b8de2c70b1 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -226,7 +226,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) : _flow{}, _range{}, _airspeed{}, - _vehicle_status{}, + _vehicle_status{}, _message_counter(0), _part1_counter_ref(0), _read_part2(false), @@ -299,6 +299,7 @@ void Ekf2Replay::publishEstimatorInput() if (_airspeed_pub == nullptr && _read_part6) { _airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed); + } else if (_airspeed_pub != nullptr) { orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed); } @@ -431,7 +432,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) _range.current_distance = replay_part4.range_to_ground; _read_part4 = true; - } else if (type == LOG_RPL6_MSG){ + } else if (type == LOG_RPL6_MSG) { uint8_t *dest_ptr = (uint8_t *)&replay_part6.time_airs_usec; parseMessage(data, dest_ptr, type); _airspeed.timestamp = replay_part6.time_airs_usec; @@ -469,17 +470,17 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) } else if (type == LOG_STAT_MSG) { - uint8_t *dest_ptr = (uint8_t *)&vehicle_status.main_state; - parseMessage(data, dest_ptr, type); - _vehicle_status.is_rotary_wing = vehicle_status.is_rot_wing; - - if (_vehicle_status_pub == nullptr) { - _vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &_vehicle_status); - - } else if (_vehicle_status_pub != nullptr) { - orb_publish(ORB_ID(vehicle_status), _vehicle_status_pub, &_vehicle_status); - } - } + uint8_t *dest_ptr = (uint8_t *)&vehicle_status.main_state; + parseMessage(data, dest_ptr, type); + _vehicle_status.is_rotary_wing = vehicle_status.is_rot_wing; + + if (_vehicle_status_pub == nullptr) { + _vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &_vehicle_status); + + } else if (_vehicle_status_pub != nullptr) { + orb_publish(ORB_ID(vehicle_status), _vehicle_status_pub, &_vehicle_status); + } + } } void Ekf2Replay::writeMessage(int &fd, void *data, size_t size)