Skip to content

Commit

Permalink
EKF2 move all orb_subscribe/unsubsribe to the constructor/destructor
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed Apr 12, 2018
1 parent 2a58c7a commit 1ecfb22
Showing 1 changed file with 101 additions and 88 deletions.
189 changes: 101 additions & 88 deletions src/modules/ekf2/ekf2_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
{
public:
Ekf2();
~Ekf2() override = default;
~Ekf2() override;

/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
Expand Down Expand Up @@ -165,6 +165,24 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
const float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim; ///< preflight velocity innovation spike limit (m/sec)
const float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim; ///< preflight position innovation spike limit (m)

int _airdata_sub{-1};
int _airspeed_sub{-1};
int _ev_att_sub{-1};
int _ev_pos_sub{-1};
int _gps_sub{-1};
int _landing_target_pose_sub{-1};
int _magnetometer_sub{-1};
int _optical_flow_sub{-1};
int _params_sub{-1};
int _sensor_selection_sub{-1};
int _sensors_sub{-1};
int _status_sub{-1};
int _vehicle_land_detected_sub{-1};

// because we can have several distance sensor instances with different orientations
int _range_finder_subs[ORB_MULTI_MAX_INSTANCES];
int _range_finder_sub_index = -1; // index for downward-facing range finder subscription

orb_advert_t _att_pub{nullptr};
orb_advert_t _wind_pub{nullptr};
orb_advert_t _estimator_status_pub{nullptr};
Expand Down Expand Up @@ -478,6 +496,48 @@ Ekf2::Ekf2():
_bcoef_x(_params->bcoef_x),
_bcoef_y(_params->bcoef_y)
{
_airdata_sub = orb_subscribe(ORB_ID(vehicle_air_data));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
_ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_landing_target_pose_sub = orb_subscribe(ORB_ID(landing_target_pose));
_magnetometer_sub = orb_subscribe(ORB_ID(vehicle_magnetometer));
_optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_sensor_selection_sub = orb_subscribe(ORB_ID(sensor_selection));
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));

for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
_range_finder_subs[i] = orb_subscribe_multi(ORB_ID(distance_sensor), i);
}

// initialise parameter cache
updateParams();
}

Ekf2::~Ekf2()
{
orb_unsubscribe(_airdata_sub);
orb_unsubscribe(_airspeed_sub);
orb_unsubscribe(_ev_att_sub);
orb_unsubscribe(_ev_pos_sub);
orb_unsubscribe(_gps_sub);
orb_unsubscribe(_landing_target_pose_sub);
orb_unsubscribe(_magnetometer_sub);
orb_unsubscribe(_optical_flow_sub);
orb_unsubscribe(_params_sub);
orb_unsubscribe(_sensor_selection_sub);
orb_unsubscribe(_sensors_sub);
orb_unsubscribe(_status_sub);
orb_unsubscribe(_vehicle_land_detected_sub);

for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
orb_unsubscribe(_range_finder_subs[i]);
_range_finder_subs[i] = -1;
}
}

int Ekf2::print_status()
Expand Down Expand Up @@ -509,46 +569,19 @@ void Ekf2::update_mag_bias(Param &mag_bias_param, int axis_index)

void Ekf2::run()
{
// subscribe to relevant topics
int sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int airspeed_sub = orb_subscribe(ORB_ID(airspeed));
int params_sub = orb_subscribe(ORB_ID(parameter_update));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position));
int ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
int status_sub = orb_subscribe(ORB_ID(vehicle_status));
int sensor_selection_sub = orb_subscribe(ORB_ID(sensor_selection));
int airdata_sub = orb_subscribe(ORB_ID(vehicle_air_data));
int landing_target_pose_sub = orb_subscribe(ORB_ID(landing_target_pose));
int magnetometer_sub = orb_subscribe(ORB_ID(vehicle_magnetometer));

bool imu_bias_reset_request = false;

// because we can have several distance sensor instances with different orientations
int range_finder_subs[ORB_MULTI_MAX_INSTANCES];
int range_finder_sub_index = -1; // index for downward-facing range finder subscription

for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
range_finder_subs[i] = orb_subscribe_multi(ORB_ID(distance_sensor), i);
}

px4_pollfd_struct_t fds[1] = {};
fds[0].fd = sensors_sub;
fds[0].fd = _sensors_sub;
fds[0].events = POLLIN;

// initialise parameter cache
updateParams();

// initialize data structures outside of loop
// because they will else not always be
// properly populated
sensor_combined_s sensors = {};
vehicle_land_detected_s vehicle_land_detected = {};
vehicle_status_s vehicle_status = {};
sensor_selection_s sensor_selection = {};
landing_target_pose_s landing_target_pose = {};

while (!should_exit()) {
int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
Expand All @@ -569,18 +602,16 @@ void Ekf2::run()
}

bool params_updated = false;
orb_check(params_sub, &params_updated);
orb_check(_params_sub, &params_updated);

if (params_updated) {
// read from param to clear updated flag
parameter_update_s update;
orb_copy(ORB_ID(parameter_update), params_sub, &update);
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
updateParams();
}

bool landing_target_pose_updated = false;

orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors);
orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);

// ekf2_timestamps (using 0.1 ms relative timestamps)
ekf2_timestamps_s ekf2_timestamps = {};
Expand All @@ -599,10 +630,10 @@ void Ekf2::run()

bool vehicle_status_updated = false;

orb_check(status_sub, &vehicle_status_updated);
orb_check(_status_sub, &vehicle_status_updated);

if (vehicle_status_updated) {
if (orb_copy(ORB_ID(vehicle_status), status_sub, &vehicle_status) == PX4_OK) {
if (orb_copy(ORB_ID(vehicle_status), _status_sub, &vehicle_status) == PX4_OK) {
// only fuse synthetic sideslip measurements if conditions are met
_ekf.set_fuse_beta_flag(!vehicle_status.is_rotary_wing && (_fuseBeta.get() == 1));

Expand All @@ -613,13 +644,13 @@ void Ekf2::run()

bool sensor_selection_updated = false;

orb_check(sensor_selection_sub, &sensor_selection_updated);
orb_check(_sensor_selection_sub, &sensor_selection_updated);

// Always update sensor selction first time through if time stamp is non zero
if (sensor_selection_updated || (sensor_selection.timestamp == 0)) {
sensor_selection_s sensor_selection_prev = sensor_selection;

if (orb_copy(ORB_ID(sensor_selection), sensor_selection_sub, &sensor_selection) == PX4_OK) {
if (orb_copy(ORB_ID(sensor_selection), _sensor_selection_sub, &sensor_selection) == PX4_OK) {
if ((sensor_selection_prev.timestamp > 0) && (sensor_selection.timestamp > sensor_selection_prev.timestamp)) {
if (sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) {
PX4_WARN("accel id changed, resetting IMU bias");
Expand Down Expand Up @@ -666,12 +697,12 @@ void Ekf2::run()

// read mag data
bool magnetometer_updated = false;
orb_check(magnetometer_sub, &magnetometer_updated);
orb_check(_magnetometer_sub, &magnetometer_updated);

if (magnetometer_updated) {
vehicle_magnetometer_s magnetometer;

if (orb_copy(ORB_ID(vehicle_magnetometer), magnetometer_sub, &magnetometer) == PX4_OK) {
if (orb_copy(ORB_ID(vehicle_magnetometer), _magnetometer_sub, &magnetometer) == PX4_OK) {
// Reset learned bias parameters if there has been a persistant change in magnetometer ID
// Do not reset parmameters when armed to prevent potential time slips casued by parameter set
// and notification events
Expand Down Expand Up @@ -738,12 +769,12 @@ void Ekf2::run()

// read baro data
bool airdata_updated = false;
orb_check(airdata_sub, &airdata_updated);
orb_check(_airdata_sub, &airdata_updated);

if (airdata_updated) {
vehicle_air_data_s airdata;

if (orb_copy(ORB_ID(vehicle_air_data), airdata_sub, &airdata) == PX4_OK) {
if (orb_copy(ORB_ID(vehicle_air_data), _airdata_sub, &airdata) == PX4_OK) {
// If the time last used by the EKF is less than specified, then accumulate the
// data and push the average when the specified interval is reached.
_balt_time_sum_ms += airdata.timestamp / 1000;
Expand Down Expand Up @@ -797,12 +828,12 @@ void Ekf2::run()

// read gps data if available
bool gps_updated = false;
orb_check(gps_sub, &gps_updated);
orb_check(_gps_sub, &gps_updated);

if (gps_updated) {
vehicle_gps_position_s gps;

if (orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps) == PX4_OK) {
if (orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps) == PX4_OK) {
struct gps_message gps_msg;
gps_msg.time_usec = gps.timestamp;
gps_msg.lat = gps.lat;
Expand All @@ -828,12 +859,12 @@ void Ekf2::run()
}

bool airspeed_updated = false;
orb_check(airspeed_sub, &airspeed_updated);
orb_check(_airspeed_sub, &airspeed_updated);

if (airspeed_updated) {
airspeed_s airspeed;

if (orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed)) {
if (orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed)) {
// only set airspeed data if condition for airspeed fusion are met
if ((_arspFusionThreshold.get() > FLT_EPSILON) && (airspeed.true_airspeed_m_s > _arspFusionThreshold.get())) {

Expand All @@ -848,12 +879,12 @@ void Ekf2::run()

bool optical_flow_updated = false;

orb_check(optical_flow_sub, &optical_flow_updated);
orb_check(_optical_flow_sub, &optical_flow_updated);

if (optical_flow_updated) {
optical_flow_s optical_flow;

if (orb_copy(ORB_ID(optical_flow), optical_flow_sub, &optical_flow) == PX4_OK) {
if (orb_copy(ORB_ID(optical_flow), _optical_flow_sub, &optical_flow) == PX4_OK) {
flow_message flow;
flow.flowdata(0) = optical_flow.pixel_flow_x_integral;
flow.flowdata(1) = optical_flow.pixel_flow_y_integral;
Expand All @@ -874,15 +905,15 @@ void Ekf2::run()
}
}

if (range_finder_sub_index >= 0) {
if (_range_finder_sub_index >= 0) {
bool range_finder_updated = false;

orb_check(range_finder_subs[range_finder_sub_index], &range_finder_updated);
orb_check(_range_finder_subs[_range_finder_sub_index], &range_finder_updated);

if (range_finder_updated) {
distance_sensor_s range_finder;

if (orb_copy(ORB_ID(distance_sensor), range_finder_subs[range_finder_sub_index], &range_finder) == PX4_OK) {
if (orb_copy(ORB_ID(distance_sensor), _range_finder_subs[_range_finder_sub_index], &range_finder) == PX4_OK) {
// check if distance sensor is within working boundaries
if (range_finder.min_distance >= range_finder.current_distance ||
range_finder.max_distance <= range_finder.current_distance) {
Expand All @@ -903,23 +934,23 @@ void Ekf2::run()
}

} else {
range_finder_sub_index = getRangeSubIndex(range_finder_subs);
_range_finder_sub_index = getRangeSubIndex(_range_finder_subs);
}

// get external vision data
// if error estimates are unavailable, use parameter defined defaults
bool vision_position_updated = false;
bool vision_attitude_updated = false;
orb_check(ev_pos_sub, &vision_position_updated);
orb_check(ev_att_sub, &vision_attitude_updated);
orb_check(_ev_pos_sub, &vision_position_updated);
orb_check(_ev_att_sub, &vision_attitude_updated);

if (vision_position_updated || vision_attitude_updated) {
// copy both attitude & position if either updated, we need both to fill a single ext_vision_message
vehicle_attitude_s ev_att = {};
orb_copy(ORB_ID(vehicle_vision_attitude), ev_att_sub, &ev_att);
orb_copy(ORB_ID(vehicle_vision_attitude), _ev_att_sub, &ev_att);

vehicle_local_position_s ev_pos = {};
orb_copy(ORB_ID(vehicle_vision_position), ev_pos_sub, &ev_pos);
orb_copy(ORB_ID(vehicle_vision_position), _ev_pos_sub, &ev_pos);

ext_vision_message ev_data;
ev_data.posNED(0) = ev_pos.x;
Expand All @@ -945,30 +976,29 @@ void Ekf2::run()
}

bool vehicle_land_detected_updated = false;
orb_check(vehicle_land_detected_sub, &vehicle_land_detected_updated);
orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);

if (vehicle_land_detected_updated) {
if (orb_copy(ORB_ID(vehicle_land_detected), vehicle_land_detected_sub, &vehicle_land_detected) == PX4_OK) {
if (orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected) == PX4_OK) {
_ekf.set_in_air_status(!vehicle_land_detected.landed);
}
}

// use the landing target pose estimate as another source of velocity data
orb_check(landing_target_pose_sub, &landing_target_pose_updated);
bool landing_target_pose_updated = false;
orb_check(_landing_target_pose_sub, &landing_target_pose_updated);

if (landing_target_pose_updated) {
orb_copy(ORB_ID(landing_target_pose), landing_target_pose_sub, &landing_target_pose);

// we can only use the landing target if it has a fixed position and a valid velocity estimate
if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) {
// velocity of vehicle relative to target has opposite sign to target relative to vehicle
float velocity[2];
velocity[0] = -landing_target_pose.vx_rel;
velocity[1] = -landing_target_pose.vy_rel;
float variance[2];
variance[0] = landing_target_pose.cov_vx_rel;
variance[1] = landing_target_pose.cov_vy_rel;
_ekf.setAuxVelData(landing_target_pose.timestamp, velocity, variance);
landing_target_pose_s landing_target_pose;

if (orb_copy(ORB_ID(landing_target_pose), _landing_target_pose_sub, &landing_target_pose) == PX4_OK) {
// we can only use the landing target if it has a fixed position and a valid velocity estimate
if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) {
// velocity of vehicle relative to target has opposite sign to target relative to vehicle
float velocity[2] = {-landing_target_pose.vx_rel, -landing_target_pose.vy_rel};
float variance[2] = {landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel};
_ekf.setAuxVelData(landing_target_pose.timestamp, velocity, variance);
}
}
}

Expand Down Expand Up @@ -1396,23 +1426,6 @@ void Ekf2::run()
orb_publish(ORB_ID(ekf2_timestamps), _ekf2_timestamps_pub, &ekf2_timestamps);
}
}

orb_unsubscribe(sensors_sub);
orb_unsubscribe(gps_sub);
orb_unsubscribe(airspeed_sub);
orb_unsubscribe(params_sub);
orb_unsubscribe(optical_flow_sub);
orb_unsubscribe(ev_pos_sub);
orb_unsubscribe(ev_att_sub);
orb_unsubscribe(vehicle_land_detected_sub);
orb_unsubscribe(status_sub);
orb_unsubscribe(sensor_selection_sub);
orb_unsubscribe(landing_target_pose_sub);

for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
orb_unsubscribe(range_finder_subs[i]);
range_finder_subs[i] = -1;
}
}

int Ekf2::getRangeSubIndex(const int *subs)
Expand Down

0 comments on commit 1ecfb22

Please sign in to comment.