fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-06 16:53:02 +02:00 committed by tumbili
parent 847cf684db
commit 9c170f7fae
6 changed files with 58 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/ekf2_replay.msg
View File

@ -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)

33
src/modules/ekf2/ekf2_main.cpp
View File

@ -80,6 +80,7 @@
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_status.h>
#include <ecl/EKF/ekf.h>
@ -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;

11
src/modules/ekf2/ekf2_params.c
View File

@ -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

22
src/modules/ekf2_replay/ekf2_replay_main.cpp
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)

6
src/modules/sdlog2/sdlog2.c
View File

@ -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);
}

8
src/modules/sdlog2/sdlog2_messages.h
View File

@ -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 */