zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ekf2: Improved publishing rules 

If the ekf has not completed alignment or encounters a serious error that produces NaN's on the attitude states, then the control, attitude and position topics are not published
The control topic is published first to reduce latency
sbg
Paul Riseborough 9 years ago committed by Lorenz Meier
parent
8ea581189a
commit
75ebdda179
1 changed files with 143 additions and 135 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 278
      src/modules/ekf2/ekf2_main.cpp

278
src/modules/ekf2/ekf2_main.cpp
Unescape View File

@ -377,20 +377,22 @@ void Ekf2::task_main() @@ -377,20 +377,22 @@ void Ekf2::task_main()
orb_check(_optical_flow_sub, &optical_flow_updated);
if(optical_flow_updated) {
if (optical_flow_updated) {
orb_copy(ORB_ID(optical_flow), _optical_flow_sub, &optical_flow);
}
orb_check(_range_finder_sub, &range_finder_updated);
if(range_finder_updated) {
if (range_finder_updated) {
orb_copy(ORB_ID(distance_sensor), _range_finder_sub, &range_finder);
}
// 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();
}
@ -434,7 +436,7 @@ void Ekf2::task_main() @@ -434,7 +436,7 @@ void Ekf2::task_main()
_ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s);
}
if(optical_flow_updated) {
if (optical_flow_updated) {
flow_message flow;
flow.flowdata(0) = optical_flow.pixel_flow_x_integral;
flow.flowdata(1) = optical_flow.pixel_flow_y_integral;
@ -442,12 +444,13 @@ void Ekf2::task_main() @@ -442,12 +444,13 @@ void Ekf2::task_main()
flow.gyrodata(0) = optical_flow.gyro_x_rate_integral;
flow.gyrodata(1) = optical_flow.gyro_y_rate_integral;
flow.dt = optical_flow.integration_timespan;
if(!isnan(optical_flow.pixel_flow_y_integral) && !isnan(optical_flow.pixel_flow_x_integral)) {
if (!isnan(optical_flow.pixel_flow_y_integral) && !isnan(optical_flow.pixel_flow_x_integral)) {
_ekf->setOpticalFlowData(optical_flow.timestamp, &flow);
}
}
if(range_finder_updated) {
if (range_finder_updated) {
_ekf->setRangeData(range_finder.timestamp, &range_finder.current_distance);
}
@ -461,156 +464,155 @@ void Ekf2::task_main() @@ -461,156 +464,155 @@ void Ekf2::task_main()
_ekf->set_arm_status(status.arming_state & vehicle_status_s::ARMING_STATE_ARMED);
}
// run the EKF update
_ekf->update();
// generate vehicle attitude data
struct vehicle_attitude_s att = {};
att.timestamp = hrt_absolute_time();
_ekf->copy_quaternion(att.q);
matrix::Quaternion<float> q(att.q[0], att.q[1], att.q[2], att.q[3]);
matrix::Euler<float> euler(q);
att.roll = euler(0);
att.pitch = euler(1);
att.yaw = euler(2);
// generate vehicle local position data
struct vehicle_local_position_s lpos = {};
float pos[3] = {};
float vel[3] = {};
lpos.timestamp = hrt_absolute_time();
// Position in local NED frame
_ekf->copy_position(pos);
lpos.x = pos[0];
lpos.y = pos[1];
lpos.z = pos[2];
// Velocity in NED frame (m/s)
_ekf->copy_velocity(vel);
lpos.vx = vel[0];
lpos.vy = vel[1];
lpos.vz = vel[2];
// TODO: better status reporting
lpos.xy_valid = _ekf->local_position_is_valid();
lpos.z_valid = true;
lpos.v_xy_valid = _ekf->local_position_is_valid();
lpos.v_z_valid = true;
// 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); // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
lpos.xy_global = _ekf->global_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
// The rotation of the tangent plane vs. geographical north
lpos.yaw = att.yaw;
float terrain_vpos;
lpos.dist_bottom_valid = _ekf->get_terrain_vert_pos(&terrain_vpos);
lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters
lpos.dist_bottom_rate = -vel[2]; // Distance to bottom surface (ground) change rate
lpos.surface_bottom_timestamp = hrt_absolute_time(); // Time when new bottom surface found
// TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
// TODO: Should use sqrt of filter position variances
// get pos vel state variance
Vector3f pos_var, vel_var;
_ekf->get_pos_var(pos_var);
_ekf->get_vel_var(vel_var);
lpos.eph = sqrt(pos_var(0)+pos_var(1));
lpos.epv = sqrt(pos_var(2));
// publish vehicle local position data
if (_lpos_pub == nullptr) {
_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);
// run the EKF update and output
if (_ekf->update()) {
} else {
orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
}
// generate vehicle attitude quaternion data
struct vehicle_attitude_s att = {};
_ekf->copy_quaternion(att.q);
matrix::Quaternion<float> q(att.q[0], att.q[1], att.q[2], att.q[3]);
// generate control state data
control_state_s ctrl_state = {};
ctrl_state.timestamp = hrt_absolute_time();
ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]);
ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]);
ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]);
// generate control state data
control_state_s ctrl_state = {};
ctrl_state.timestamp = hrt_absolute_time();
ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]);
ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]);
ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]);
ctrl_state.q[0] = q(0);
ctrl_state.q[1] = q(1);
ctrl_state.q[2] = q(2);
ctrl_state.q[3] = q(3);
ctrl_state.q[0] = q(0);
ctrl_state.q[1] = q(1);
ctrl_state.q[2] = q(2);
ctrl_state.q[3] = q(3);
// publish control state data
if (_control_state_pub == nullptr) {
_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
// publish control state data
if (_control_state_pub == nullptr) {
_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
} else {
orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
}
} else {
orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
}
// generate vehicle attitude data
att.q[0] = q(0);
att.q[1] = q(1);
att.q[2] = q(2);
att.q[3] = q(3);
att.q_valid = true;
att.rollspeed = sensors.gyro_rad_s[0];
att.pitchspeed = sensors.gyro_rad_s[1];
att.yawspeed = sensors.gyro_rad_s[2];
// generate remaining vehicle attitude data
att.timestamp = hrt_absolute_time();
matrix::Euler<float> euler(q);
att.roll = euler(0);
att.pitch = euler(1);
att.yaw = euler(2);
// publish vehicle attitude data
if (_att_pub == nullptr) {
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
att.q[0] = q(0);
att.q[1] = q(1);
att.q[2] = q(2);
att.q[3] = q(3);
att.q_valid = true;
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
att.rollspeed = sensors.gyro_rad_s[0];
att.pitchspeed = sensors.gyro_rad_s[1];
att.yawspeed = sensors.gyro_rad_s[2];
// publish vehicle attitude data
if (_att_pub == nullptr) {
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
// generate and publish global position data
struct vehicle_global_position_s global_pos = {};
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
// generate vehicle local position data
struct vehicle_local_position_s lpos = {};
float pos[3] = {};
float vel[3] = {};
lpos.timestamp = hrt_absolute_time();
// Position in local NED frame
_ekf->copy_position(pos);
lpos.x = pos[0];
lpos.y = pos[1];
lpos.z = pos[2];
// Velocity in NED frame (m/s)
_ekf->copy_velocity(vel);
lpos.vx = vel[0];
lpos.vy = vel[1];
lpos.vz = vel[2];
// TODO: better status reporting
lpos.xy_valid = _ekf->local_position_is_valid();
lpos.z_valid = true;
lpos.v_xy_valid = _ekf->local_position_is_valid();
lpos.v_z_valid = true;
// Position of local NED origin in GPS / WGS84 frame
struct map_projection_reference_s ekf_origin = {};
// 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);
lpos.xy_global = _ekf->global_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
// The rotation of the tangent plane vs. geographical north
lpos.yaw = att.yaw;
float terrain_vpos;
lpos.dist_bottom_valid = _ekf->get_terrain_vert_pos(&terrain_vpos);
lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters
lpos.dist_bottom_rate = -vel[2]; // Distance to bottom surface (ground) change rate
lpos.surface_bottom_timestamp = hrt_absolute_time(); // Time when new bottom surface found
// TODO: uORB definition does not define what these variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
Vector3f pos_var, vel_var;
_ekf->get_pos_var(pos_var);
_ekf->get_vel_var(vel_var);
lpos.eph = sqrt(pos_var(0) + pos_var(1));
lpos.epv = sqrt(pos_var(2));
// publish vehicle local position data
if (_lpos_pub == nullptr) {
_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);
if (_ekf->global_position_is_valid()) {
// TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator
} else {
orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
}
global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start
global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds
// generate and publish global position data
struct vehicle_global_position_s global_pos = {};
double est_lat, est_lon;
map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon);
global_pos.lat = est_lat; // Latitude in degrees
global_pos.lon = est_lon; // Longitude in degrees
if (_ekf->global_position_is_valid()) {
global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start
global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds
global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters
double est_lat, est_lon;
map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon);
global_pos.lat = est_lat; // Latitude in degrees
global_pos.lon = est_lon; // Longitude in degrees
global_pos.vel_n = vel[0]; // Ground north velocity, m/s
global_pos.vel_e = vel[1]; // Ground east velocity, m/s
global_pos.vel_d = vel[2]; // Ground downside velocity, m/s
global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters
global_pos.yaw = euler(2); // Yaw in radians -PI..+PI.
global_pos.vel_n = vel[0]; // Ground north velocity, m/s
global_pos.vel_e = vel[1]; // Ground east velocity, m/s
global_pos.vel_d = vel[2]; // Ground downside velocity, m/s
global_pos.eph = sqrt(pos_var(0)+pos_var(1));; // Standard deviation of position estimate horizontally
global_pos.epv = sqrt(pos_var(2)); // Standard deviation of position vertically
global_pos.yaw = euler(2); // Yaw in radians -PI..+PI.
// TODO: implement terrain estimator
global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
// TODO use innovatun consistency check timouts to set this
global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning
global_pos.eph = sqrt(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally
global_pos.epv = sqrt(pos_var(2)); // Standard deviation of position vertically
global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m)
// TODO: implement terrain estimator
global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
// TODO use innovatun consistency check timouts to set this
global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning
if (_vehicle_global_position_pub == nullptr) {
_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m)
} else {
orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
if (_vehicle_global_position_pub == nullptr) {
_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
} else {
orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
}
}
}
@ -642,6 +644,7 @@ void Ekf2::task_main() @@ -642,6 +644,7 @@ void Ekf2::task_main()
_ekf->get_heading_innov_var(&innovations.heading_innov_var);
_ekf->get_flow_innov_var(&innovations.flow_innov_var[0]);
_ekf->get_hagl_innov_var(&innovations.hagl_innov_var);
if (_estimator_innovations_pub == nullptr) {
_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);
@ -658,6 +661,7 @@ void Ekf2::task_main() @@ -658,6 +661,7 @@ void Ekf2::task_main()
// 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;
@ -666,7 +670,8 @@ void Ekf2::task_main() @@ -666,7 +670,8 @@ void Ekf2::task_main()
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.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];
@ -687,6 +692,7 @@ void Ekf2::task_main() @@ -687,6 +692,7 @@ void Ekf2::task_main()
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;
} else {
// this will tell the logging app not to bother logging any gps replay data
replay.time_usec = 0;
@ -700,6 +706,7 @@ void Ekf2::task_main() @@ -700,6 +706,7 @@ void Ekf2::task_main()
replay.flow_gyro_integral[1] = optical_flow.gyro_y_rate_integral;
replay.flow_time_integral = optical_flow.integration_timespan;
replay.flow_quality = optical_flow.quality;
} else {
replay.flow_timestamp = 0;
}
@ -707,6 +714,7 @@ void Ekf2::task_main() @@ -707,6 +714,7 @@ void Ekf2::task_main()
if (range_finder_updated) {
replay.rng_timestamp = range_finder.timestamp;
replay.range_to_ground = range_finder.current_distance;
} else {
replay.rng_timestamp = 0;
}

Write Preview
Loading…
Cancel
Save