From 28f5cb8fe6c3111398b7fe0114ca2a0f966eebc3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 31 Dec 2015 18:37:16 +1100 Subject: [PATCH] ekf2: Changes required to enter POSCTL mode Adds missing local position and global position data --- src/lib/ecl | 2 +- src/modules/ekf2/ekf2_main.cpp | 117 ++++++++++++++++++++++++++++----- 2 files changed, 102 insertions(+), 17 deletions(-) diff --git a/src/lib/ecl b/src/lib/ecl index eda69e727f..0cb210d045 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit eda69e727fab22c81c421b635cb34303b90b7968 +Subproject commit 0cb210d045d005d0775bc7b4c1631addb128c8e3 diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 13f615c4e2..913b83227d 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -70,6 +70,7 @@ #include #include #include +#include #include @@ -111,6 +112,8 @@ public: void print(); + void print_status(); + private: static constexpr float _dt_max = 0.02; bool _task_should_exit = false; /**< if true, task should exit */ @@ -123,6 +126,7 @@ private: orb_advert_t _att_pub; orb_advert_t _lpos_pub; orb_advert_t _control_state_pub; + orb_advert_t _vehicle_global_position_pub; /* Low pass filter for attitude rates */ math::LowPassFilter2p _lp_roll_rate; @@ -162,6 +166,11 @@ void Ekf2::print() _ekf->printStoredIMU(); } +void Ekf2::print_status() +{ + warnx("position OK %s", (_ekf->position_is_valid()) ? "[YES]" : "[NO]"); +} + void Ekf2::task_main() { // subscribe to relevant topics @@ -244,11 +253,12 @@ void Ekf2::task_main() _ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s); } + // run the EKF update + _ekf->update(); + + // generate vehicle attitude data struct vehicle_attitude_s att; - struct vehicle_local_position_s lpos; att.timestamp = hrt_absolute_time(); - lpos.timestamp = hrt_absolute_time(); - _ekf->update(); _ekf->copy_quaternion(att.q); matrix::Quaternion q(att.q[0], att.q[1], att.q[2], att.q[3]); @@ -257,25 +267,65 @@ void Ekf2::task_main() 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 + lpos.ref_timestamp = _ekf->_last_gps_origin_time_us; // Time when reference position was set + 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->_posRef.lat_rad * (double)180.0 * M_PI; // Reference point latitude in degrees + lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees + lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level + + // TODO: uORB definition does not define what this variable is. We have assumed it to be heading angle in radians lying on the range from +-pi + lpos.yaw = euler(2); + + // TODO: Distance to surface. Units not defined. + lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground) + 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); @@ -283,6 +333,14 @@ void Ekf2::task_main() 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); @@ -293,25 +351,51 @@ void Ekf2::task_main() att.pitchspeed = sensors.gyro_rad_s[1]; att.yawspeed = sensors.gyro_rad_s[2]; - 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); - } - + // 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); } - 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->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->_posRef, 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]; // 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); + } + } } } @@ -394,6 +478,7 @@ int ekf2_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (ekf2::instance) { PX4_WARN("running"); + ekf2::instance->print_status(); return 0; } else {