mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-17 17:47:34 +08:00
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
This commit is contained in:
Paul Riseborough
committed by
Lorenz Meier
Lorenz Meier
parent
8ea581189a
commit
75ebdda179
+158
-150
@@ -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()
|
||||
_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()
|
||||
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()
|
||||
_ekf->set_arm_status(status.arming_state & vehicle_status_s::ARMING_STATE_ARMED);
|
||||
}
|
||||
|
||||
// run the EKF update
|
||||
_ekf->update();
|
||||
// run the EKF update and output
|
||||
if (_ekf->update()) {
|
||||
|
||||
// generate vehicle attitude data
|
||||
struct vehicle_attitude_s att = {};
|
||||
att.timestamp = hrt_absolute_time();
|
||||
// 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]);
|
||||
|
||||
_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 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 vehicle local position data
|
||||
struct vehicle_local_position_s lpos = {};
|
||||
float pos[3] = {};
|
||||
float vel[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);
|
||||
|
||||
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);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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];
|
||||
|
||||
// publish vehicle attitude data
|
||||
if (_att_pub == nullptr) {
|
||||
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
|
||||
}
|
||||
|
||||
// generate and publish global position data
|
||||
struct vehicle_global_position_s global_pos = {};
|
||||
|
||||
if (_ekf->global_position_is_valid()) {
|
||||
// TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator
|
||||
|
||||
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
|
||||
|
||||
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.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters
|
||||
|
||||
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.yaw = euler(2); // Yaw in radians -PI..+PI.
|
||||
|
||||
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
|
||||
|
||||
// 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.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m)
|
||||
|
||||
if (_vehicle_global_position_pub == nullptr) {
|
||||
_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
|
||||
// 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(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
|
||||
orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
|
||||
}
|
||||
|
||||
|
||||
// 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);
|
||||
|
||||
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];
|
||||
|
||||
// publish vehicle attitude data
|
||||
if (_att_pub == nullptr) {
|
||||
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
||||
|
||||
} 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);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
|
||||
}
|
||||
|
||||
// generate and publish global position data
|
||||
struct vehicle_global_position_s global_pos = {};
|
||||
|
||||
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
|
||||
|
||||
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.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters
|
||||
|
||||
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.yaw = euler(2); // Yaw in radians -PI..+PI.
|
||||
|
||||
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
|
||||
|
||||
// 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.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m)
|
||||
|
||||
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()
|
||||
_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()
|
||||
|
||||
// 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()
|
||||
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()
|
||||
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()
|
||||
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()
|
||||
if (range_finder_updated) {
|
||||
replay.rng_timestamp = range_finder.timestamp;
|
||||
replay.range_to_ground = range_finder.current_distance;
|
||||
|
||||
} else {
|
||||
replay.rng_timestamp = 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user