void Ekf2::task_main()
{
// subscribe to relevant topics
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
px4_pollfd_struct_t fds[2] = {};
fds[0].fd = _sensors_sub;
fds[0].events = POLLIN;
fds[1].fd = _params_sub;
fds[1].events = POLLIN;
// initialise parameter cache
updateParams();
vehicle_gps_position_s gps = {};
while (!_task_should_exit) {
int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
if (ret < 0) {
// Poll error, sleep and try again
usleep(10000);
continue;
} else if (ret == 0) {
// Poll timeout or no new data, do nothing
continue;
}
if (fds[1].revents & POLLIN) {
// read from param to clear updated flag
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
updateParams();
// fetch sensor data in next loop
continue;
} else if (!(fds[0].revents & POLLIN)) {
// no new data
continue;
}
bool gps_updated = false;
bool airspeed_updated = false;
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);
if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
}
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
}
// 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
orb_check(_control_mode_sub, &control_mode_updated);
if (control_mode_updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &vehicle_control_mode);
_ekf->set_arm_status(vehicle_control_mode.flag_armed);
}
hrt_abstime 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]);
// read mag data
_ekf->setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]);
// read baro data
_ekf->setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]);
// read gps data if available
if (gps_updated) {
struct gps_message gps_msg = {};
gps_msg.time_usec = gps.timestamp_position;
gps_msg.lat = gps.lat;
gps_msg.lon = gps.lon;
gps_msg.alt = gps.alt;
gps_msg.fix_type = gps.fix_type;
gps_msg.eph = gps.eph;
gps_msg.epv = gps.epv;
gps_msg.sacc = gps.s_variance_m_s;
gps_msg.time_usec_vel = gps.timestamp_velocity;
gps_msg.vel_m_s = gps.vel_m_s;
gps_msg.vel_ned[0] = gps.vel_n_m_s;
gps_msg.vel_ned[1] = gps.vel_e_m_s;
gps_msg.vel_ned[2] = gps.vel_d_m_s;
gps_msg.vel_ned_valid = gps.vel_ned_valid;
gps_msg.nsats = gps.satellites_used;
//TODO add gdop to gps topic
gps_msg.gdop = 0.0f;
_ekf->setGpsData(gps.timestamp_position, &gps_msg);
}
// read airspeed data if available
if (airspeed_updated) {
_ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s);
}
// read vehicle status if available for 'landed' information
orb_check(_vehicle_status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
struct vehicle_status_s status = {};
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &status);
_ekf->set_in_air_status(!status.condition_landed);
}
// 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->position_is_valid();
lpos.z_valid = true;
lpos.v_xy_valid = _ekf->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);
lpos.xy_global =
_ekf->position_is_valid(); // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
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 = 0.0f;
lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground) in meters
lpos.dist_bottom_rate = 0.0f; // Distance to bottom surface (ground) change rate
lpos.surface_bottom_timestamp = 0; // Time when new bottom surface found
lpos.dist_bottom_valid = false; // true if distance to bottom surface is valid
// 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
lpos.eph = gps.eph;
lpos.epv = gps.epv;
// 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->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 = gps.eph; // Standard deviation of position estimate horizontally
global_pos.epv = gps.epv; // 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);
}
}
// publish estimator status
struct estimator_status_s status = {};
status.timestamp = hrt_absolute_time();
_ekf->get_state_delayed(status.states);
_ekf->get_covariances(status.covariances);
//status.gps_check_fail_flags = _ekf->_gps_check_fail_status.value;
if (_estimator_status_pub == nullptr) {
_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status);
} else {
orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status);
}
// publish estimator innovation data
struct ekf2_innovations_s innovations = {};
innovations.timestamp = hrt_absolute_time();
_ekf->get_vel_pos_innov(&innovations.vel_pos_innov[0]);
_ekf->get_mag_innov(&innovations.mag_innov[0]);
_ekf->get_heading_innov(&innovations.heading_innov);
_ekf->get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
_ekf->get_mag_innov_var(&innovations.mag_innov_var[0]);
_ekf->get_heading_innov_var(&innovations.heading_innov_var);
if (_estimator_innovations_pub == nullptr) {
_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);
} else {
orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
}
// save the declination to the EKF2_MAG_DECL parameter when a dis-arm event is detected
if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !vehicle_control_mode.flag_armed) {
float decl_deg;
_ekf->copy_mag_decl_deg(&decl_deg);
_mag_declination_deg->set(decl_deg);
}
_prev_motors_armed = vehicle_control_mode.flag_armed;
}
delete ekf2::instance;
ekf2::instance = nullptr;
}
void Ekf2::print_status()
{
warnx("position OK %s", (_ekf->position_is_valid()) ? "[YES]" : "[NO]");
}
