Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 16:56:25 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

ekf2 app:

Browse Source 
- support use of replay
This commit is contained in:
tumbili
2016-02-17 14:56:31 +01:00
committed by Lorenz Meier
parent 35d573e12a
commit 652fb5e99e
2 changed files with 80 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+70 -13
View File
@@ -76,6 +76,7 @@
#include <uORB/topics/ekf2_innovations.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/ekf2_replay.h>
#include <ecl/EKF/ekf.h>
@@ -111,6 +112,8 @@ public:
*/
int start();
void set_replay_mode(bool replay) {_replay_mode = true;};
static void task_main_trampoline(int argc, char *argv[]);
void task_main();
@@ -121,8 +124,10 @@ public:
private:
static constexpr float _dt_max = 0.02;
bool _task_should_exit = false; /**< if true, task should exit */
int _control_task = -1; /**< task handle for task */
bool _task_should_exit = false;
int _control_task = -1; // task handle for task
bool _replay_mode; // should we use replay data from a log
int _publish_replay_mode; // defines if we should publish replay messages
int _sensors_sub = -1;
int _gps_sub = -1;
@@ -139,6 +144,7 @@ private:
orb_advert_t _vehicle_global_position_pub;
orb_advert_t _estimator_status_pub;
orb_advert_t _estimator_innovations_pub;
orb_advert_t _replay_pub;
/* Low pass filter for attitude rates */
math::LowPassFilter2p _lp_roll_rate;
@@ -189,6 +195,7 @@ private:
control::BlockParamFloat *_requiredGDoP; // maximum acceptable geometric dilution of precision
control::BlockParamFloat *_requiredHdrift; // maximum acceptable horizontal drift speed (m/s)
control::BlockParamFloat *_requiredVdrift; // maximum acceptable vertical drift speed (m/s)
control::BlockParamInt *_param_record_replay_msg; // indicates if we want to record ekf2 replay messages
int update_subscriptions();
@@ -196,12 +203,15 @@ private:
Ekf2::Ekf2():
SuperBlock(NULL, "EKF"),
_replay_mode(false),
_publish_replay_mode(0),
_att_pub(nullptr),
_lpos_pub(nullptr),
_control_state_pub(nullptr),
_vehicle_global_position_pub(nullptr),
_estimator_status_pub(nullptr),
_estimator_innovations_pub(nullptr),
_replay_pub(nullptr),
_lp_roll_rate(250.0f, 30.0f),
_lp_pitch_rate(250.0f, 30.0f),
_lp_yaw_rate(250.0f, 20.0f),
@@ -239,7 +249,8 @@ Ekf2::Ekf2():
_requiredNsats(new control::BlockParamInt(this, "EKF2_REQ_NSATS", false, &_params->req_nsats)),
_requiredGDoP(new control::BlockParamFloat(this, "EKF2_REQ_GDOP", false, &_params->req_gdop)),
_requiredHdrift(new control::BlockParamFloat(this, "EKF2_REQ_HDRIFT", false, &_params->req_hdrift)),
_requiredVdrift(new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift))
_requiredVdrift(new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift)),
_param_record_replay_msg(new control::BlockParamInt(this, "EKF2_REC_RPL", false, &_publish_replay_mode))
{
}
@@ -273,7 +284,13 @@ void Ekf2::task_main()
// initialise parameter cache
updateParams();
// initialize data structures outside of loop
// because they will else not always be
// properly populated
sensor_combined_s sensors = {};
vehicle_gps_position_s gps = {};
airspeed_s airspeed = {};
vehicle_control_mode_s vehicle_control_mode = {};
while (!_task_should_exit) {
int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
@@ -307,12 +324,7 @@ void Ekf2::task_main()
bool control_mode_updated = false;
bool vehicle_status_updated = false;
sensor_combined_s sensors = {};
airspeed_s airspeed = {};
vehicle_control_mode_s vehicle_control_mode = {};
orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
// update all other topics if they have new data
orb_check(_gps_sub, &gps_updated);
@@ -328,6 +340,7 @@ void Ekf2::task_main()
// Use the control model data to determine if the motors are armed as a surrogate for an on-ground vs in-air status
// TODO implement a global vehicle on-ground/in-air check
// XXX: Look at the land_detection module!
orb_check(_control_mode_sub, &control_mode_updated);
if (control_mode_updated) {
@@ -335,7 +348,14 @@ void Ekf2::task_main()
_ekf->set_arm_status(vehicle_control_mode.flag_armed);
}
hrt_abstime now = hrt_absolute_time();
// in replay mode we are getting the actual timestamp from the sensor topic
hrt_abstime now = 0;
if (_replay_mode) {
now = sensors.timestamp;
} else {
now = hrt_absolute_time();
}
// push imu data into estimator
_ekf->setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0],
&sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]);
@@ -425,9 +445,8 @@ void Ekf2::task_main()
// Position of local NED origin in GPS / WGS84 frame
struct map_projection_reference_s ekf_origin = {};
_ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt);
lpos.xy_global =
_ekf->position_is_valid(); // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
_ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt); // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
lpos.xy_global = _ekf->position_is_valid();
lpos.z_global = true; // true if z is valid and has valid global reference (ref_alt)
lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
@@ -572,8 +591,40 @@ void Ekf2::task_main()
_mag_declination_deg->set(decl_deg);
}
_prev_motors_armed = vehicle_control_mode.flag_armed;
// publish replay message if in replay mode
bool publish_replay_message = (bool)_param_record_replay_msg->get();
if (publish_replay_message) {
struct ekf2_replay_s replay = {};
replay.time_ref = now;
replay.gyro_integral_dt = sensors.gyro_integral_dt[0];
replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt[0];
replay.magnetometer_timestamp = sensors.magnetometer_timestamp[0];
replay.baro_timestamp = sensors.baro_timestamp[0];
memcpy(&replay.gyro_integral_rad[0], &sensors.gyro_integral_rad[0], sizeof(replay.gyro_integral_rad));
memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0], sizeof(replay.accelerometer_integral_m_s));
memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga));
replay.baro_alt_meter = sensors.baro_alt_meter[0];
replay.time_usec = gps.timestamp_position;
replay.time_usec_vel = gps.timestamp_velocity;
replay.lat = gps.lat;
replay.lon = gps.lon;
replay.alt = gps.alt;
replay.fix_type = gps.fix_type;
replay.eph = gps.eph;
replay.epv = gps.epv;
replay.vel_m_s = gps.vel_m_s;
replay.vel_n_m_s = gps.vel_n_m_s;
replay.vel_e_m_s = gps.vel_e_m_s;
replay.vel_d_m_s = gps.vel_d_m_s;
replay.vel_ned_valid = gps.vel_ned_valid;
if (_replay_pub == nullptr) {
_replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay);
} else {
orb_publish(ORB_ID(ekf2_replay), _replay_pub, &replay);
}
}
}
delete ekf2::instance;
@@ -621,6 +672,12 @@ int ekf2_main(int argc, char *argv[])
ekf2::instance = new Ekf2();
if (argc >= 3) {
if (!strcmp(argv[2], "--replay")) {
ekf2::instance->set_replay_mode(true);
}
}
if (ekf2::instance == nullptr) {
PX4_WARN("alloc failed");
return 1;
src/modules/ekf2/ekf2_params.c
+10
View File
@@ -378,3 +378,13 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 3.0f);
* @unit SD
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 3.0f);
/**
* A value of 1 indicates that the ekf2 module will publish
* replay messages for logging.
*
* @group EKF2
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(EKF2_REC_RPL, 0);