From 9c170f7fae5b482db7ee5a27bb52894e772329cb Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Mon, 6 Jun 2016 16:53:02 +0200 Subject: [PATCH] added parameter which defines threshold for airspeed given to the filter remove unnecessary replay fields --- msg/ekf2_replay.msg | 3 -- src/modules/ekf2/ekf2_main.cpp | 33 ++++++++++++++------ src/modules/ekf2/ekf2_params.c | 11 +++++++ src/modules/ekf2_replay/ekf2_replay_main.cpp | 22 +++++++++++-- src/modules/sdlog2/sdlog2.c | 6 ++-- src/modules/sdlog2/sdlog2_messages.h | 8 ++--- 6 files changed, 58 insertions(+), 25 deletions(-) diff --git a/msg/ekf2_replay.msg b/msg/ekf2_replay.msg index e19daae59b..834230da1a 100644 --- a/msg/ekf2_replay.msg +++ b/msg/ekf2_replay.msg @@ -43,9 +43,6 @@ uint8 flow_quality # Quality of accumulated optical flow data (0 - 255) # airspeed float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if unknown float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if unknown -float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown -float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown -float32 confidence # confidence value from 0 to 1 for this sensor # external vision measurements float32[3] pos_ev # position in earth (NED) frame (metres) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 26c59cf93d..113f8d840e 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -80,6 +80,7 @@ #include #include #include +#include #include @@ -143,6 +144,7 @@ private: int _ev_pos_sub = -1; int _actuator_armed_sub = -1; int _vehicle_land_detected_sub = -1; + int _status_sub = -1; bool _prev_landed = true; // landed status from the previous frame @@ -255,7 +257,10 @@ private: control::BlockParamFloat _ev_pos_x; // X position of VI sensor focal point in body frame (m) 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 + // 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) @@ -355,6 +360,7 @@ Ekf2::Ekf2(): _ev_pos_x(this, "EKF2_EV_POS_X", false, &_params->ev_pos_body(0)), _ev_pos_y(this, "EKF2_EV_POS_Y", false, &_params->ev_pos_body(1)), _ev_pos_z(this, "EKF2_EV_POS_Z", false, &_params->ev_pos_body(2)), + _arspFusionThreshold(this, "EKF2_ARSP_THR", false), _tau_vel(this, "EKF2_TAU_VEL", false, &_params->vel_Tau), _tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau), _gyr_bias_init(this, "EKF2_GBIAS_INIT", false, &_params->switch_on_gyro_bias), @@ -386,6 +392,7 @@ void Ekf2::task_main() _range_finder_sub = orb_subscribe(ORB_ID(distance_sensor)); _ev_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate)); _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + _status_sub = orb_subscribe(ORB_ID(vehicle_status)); px4_pollfd_struct_t fds[2] = {}; fds[0].fd = _sensors_sub; @@ -406,6 +413,7 @@ void Ekf2::task_main() distance_sensor_s range_finder = {}; vehicle_land_detected_s vehicle_land_detected = {}; vision_position_estimate_s ev = {}; + vehicle_status_s _vehicle_status = {}; while (!_task_should_exit) { int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); @@ -440,9 +448,17 @@ void Ekf2::task_main() bool range_finder_updated = false; bool vehicle_land_detected_updated = false; bool vision_position_updated = false; + bool vehicle_status_updated = false; 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); + + if (vehicle_status_updated) { + orb_copy(ORB_ID(vehicle_status), _status_sub, &_vehicle_status); + } + orb_check(_gps_sub, &gps_updated); if (gps_updated) { @@ -517,12 +533,12 @@ void Ekf2::task_main() _ekf.setGpsData(gps.timestamp_position, &gps_msg); } - // read airspeed data if available - float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s; - - if (airspeed_updated && airspeed.true_airspeed_m_s > 7.0f) { + // 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; _ekf.setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s, &eas2tas); - } + } if (optical_flow_updated) { flow_message flow; @@ -907,13 +923,10 @@ void Ekf2::task_main() replay.rng_timestamp = 0; } - if (airspeed_updated) { + if (fuse_airspeed) { replay.asp_timestamp = airspeed.timestamp; replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s; replay.true_airspeed_m_s = airspeed.true_airspeed_m_s; - replay.true_airspeed_unfiltered_m_s = airspeed.true_airspeed_unfiltered_m_s; - replay.air_temperature_celsius = airspeed.air_temperature_celsius; - replay.confidence = airspeed.confidence; } else { replay.asp_timestamp = 0; diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index f316495926..6a9eab4770 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -782,6 +782,17 @@ PARAM_DEFINE_FLOAT(EKF2_EV_POS_Y, 0.0f); */ PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f); + /** + * Airspeed fusion threshold. A value of zero will deactivate airspeed fusion. Any other positive + * value will determine the minimum airspeed which will still be fused. + * + * @group EKF2 + * @min 0.0 + * @unit m/s + * @decimal 1 + */ + PARAM_DEFINE_FLOAT(EKF2_ARSP_THR, 0.0f); + /** * Time constant of the velocity output prediction and smoothing filter diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index c6ff14e86a..df74663ee7 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -137,6 +137,7 @@ private: orb_advert_t _range_pub; orb_advert_t _airspeed_pub; orb_advert_t _ev_pub; + orb_advert_t _vehicle_status_pub; int _att_sub; int _estimator_status_sub; @@ -154,6 +155,7 @@ private: struct distance_sensor_s _range; struct airspeed_s _airspeed; struct vision_position_estimate_s _ev; + struct vehicle_status_s _vehicle_status; unsigned _message_counter; // counter which will increase with every message read from the log unsigned _part1_counter_ref; // this is the value of _message_counter when the part1 of the replay message is read (imu data) @@ -211,6 +213,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) : _range_pub(nullptr), _airspeed_pub(nullptr), _ev_pub(nullptr), + _vehicle_status_pub(nullptr), _att_sub(-1), _estimator_status_sub(-1), _innov_sub(-1), @@ -222,6 +225,8 @@ Ekf2Replay::Ekf2Replay(char *logfile) : _land_detected{}, _flow{}, _range{}, + _airspeed{}, + _vehicle_status{}, _message_counter(0), _part1_counter_ref(0), _read_part2(false), @@ -364,6 +369,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) struct log_RPL6_s replay_part6 = {}; struct log_RPL5_s replay_part5 = {}; struct log_LAND_s vehicle_landed = {}; + struct log_STAT_s vehicle_status = {}; if (type == LOG_RPL1_MSG) { @@ -431,9 +437,6 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) _airspeed.timestamp = replay_part6.time_airs_usec; _airspeed.indicated_airspeed_m_s = replay_part6.indicated_airspeed_m_s; _airspeed.true_airspeed_m_s = replay_part6.true_airspeed_m_s; - _airspeed.true_airspeed_unfiltered_m_s = replay_part6.true_airspeed_unfiltered_m_s; - _airspeed.air_temperature_celsius = replay_part6.air_temperature_celsius; - _airspeed.confidence = replay_part6.confidence; _read_part6 = true; } else if (type == LOG_RPL5_MSG) { @@ -464,6 +467,19 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) orb_publish(ORB_ID(vehicle_land_detected), _landed_pub, &_land_detected); } } + + 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); + } + } } void Ekf2Replay::writeMessage(int &fd, void *data, size_t size) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3d63da3e2d..ceab974656 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1517,6 +1517,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.nav_state = buf_status.nav_state; log_msg.body.log_STAT.arming_state = buf_status.arming_state; log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe; + log_msg.body.log_STAT.is_rot_wing = (uint8_t)buf_status.is_rotary_wing; LOGBUFFER_WRITE_AND_COUNT(STAT); } @@ -1600,10 +1601,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_RPL6_MSG; log_msg.body.log_RPL6.time_airs_usec = buf.replay.asp_timestamp; log_msg.body.log_RPL6.indicated_airspeed_m_s = buf.replay.indicated_airspeed_m_s; - log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s; - log_msg.body.log_RPL6.true_airspeed_unfiltered_m_s = buf.replay.true_airspeed_unfiltered_m_s; - log_msg.body.log_RPL6.air_temperature_celsius = buf.replay.air_temperature_celsius; - log_msg.body.log_RPL6.confidence = buf.replay.confidence; + log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s;; LOGBUFFER_WRITE_AND_COUNT(RPL6); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 48d22b1c94..a6a253f921 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -182,6 +182,7 @@ struct log_STAT_s { uint8_t nav_state; uint8_t arming_state; uint8_t failsafe; + uint8_t is_rot_wing; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -600,9 +601,6 @@ struct log_RPL6_s { uint64_t time_airs_usec; float indicated_airspeed_m_s; float true_airspeed_m_s; - float true_airspeed_unfiltered_m_s; - float air_temperature_celsius; - float confidence; }; /* --- EKF2 REPLAY Part 5 --- */ @@ -668,7 +666,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(DGPS, GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBB", "MainState,NavState,ArmS,Failsafe"), + LOG_FORMAT(STAT, "BBBBB", "MainState,NavState,ArmS,Failsafe,IsRotWing"), LOG_FORMAT(VTOL, "fBBB", "Arsp,RwMode,TransMode,Failsafe"), LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), LOG_FORMAT(RC, "ffffffffffffBBBL", "C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"), @@ -712,7 +710,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"), LOG_FORMAT(RPL4, "Qf", "Trng,rng"), LOG_FORMAT(RPL5, "Qfffffffff", "Tev,x,y,z,q0,q1,q2,q3,posErr,angErr"), - LOG_FORMAT(RPL6, "Qfffff", "Tasp,inAsp,trAsp,ufAsp,tpAsp,confAsp"), + LOG_FORMAT(RPL6, "Qff", "Tasp,inAsp,trAsp"), LOG_FORMAT(LAND, "B", "Landed"), LOG_FORMAT(LOAD, "f", "CPU"), /* system-level messages, ID >= 0x80 */