fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 17:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

added parameter which defines threshold for airspeed given to the filter

Browse Source 
remove unnecessary replay fields
This commit is contained in:
CarlOlsson
2016-06-08 17:11:10 +02:00
committed by tumbili
parent 847cf684db
commit 9c170f7fae
6 changed files with 58 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2_replay/ekf2_replay_main.cpp
+19 -3
View File
@@ -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)