aboutsummaryrefslogtreecommitdiff
path: root/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp')
-rw-r--r--src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp70
1 files changed, 59 insertions, 11 deletions
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 0a6d3fa8f..abe272e56 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -74,6 +74,7 @@
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
+#include <uORB/topics/wind_estimate.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <geo/geo.h>
@@ -158,6 +159,7 @@ private:
orb_advert_t _global_pos_pub; /**< global position */
orb_advert_t _local_pos_pub; /**< position in local frame */
orb_advert_t _estimator_status_pub; /**< status of the estimator */
+ orb_advert_t _wind_pub; /**< wind estimate */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct gyro_report _gyro;
@@ -169,6 +171,7 @@ private:
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct vehicle_local_position_s _local_pos; /**< local vehicle position */
struct vehicle_gps_position_s _gps; /**< GPS position */
+ struct wind_estimate_s _wind; /**< Wind estimate */
struct gyro_scale _gyro_offsets;
struct accel_scale _accel_offsets;
@@ -246,6 +249,10 @@ private:
AttPosEKF *_ekf;
+ float _velocity_xy_filtered;
+ float _velocity_z_filtered;
+ float _airspeed_filtered;
+
/**
* Update our local parameter cache.
*/
@@ -312,6 +319,7 @@ FixedwingEstimator::FixedwingEstimator() :
_global_pos_pub(-1),
_local_pos_pub(-1),
_estimator_status_pub(-1),
+ _wind_pub(-1),
_att({}),
_gyro({}),
@@ -323,6 +331,7 @@ FixedwingEstimator::FixedwingEstimator() :
_global_pos({}),
_local_pos({}),
_gps({}),
+ _wind({}),
_gyro_offsets({}),
_accel_offsets({}),
@@ -352,7 +361,10 @@ FixedwingEstimator::FixedwingEstimator() :
_accel_valid(false),
_mag_valid(false),
_mavlink_fd(-1),
- _ekf(nullptr)
+ _ekf(nullptr),
+ _velocity_xy_filtered(0.0f),
+ _velocity_z_filtered(0.0f),
+ _airspeed_filtered(0.0f)
{
last_run = hrt_absolute_time();
@@ -1028,7 +1040,7 @@ FixedwingEstimator::task_main()
float initVelNED[3];
- if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
+ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
initVelNED[0] = _gps.vel_n_m_s;
initVelNED[1] = _gps.vel_e_m_s;
@@ -1068,7 +1080,7 @@ FixedwingEstimator::task_main()
warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset);
- warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination));
+ warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination));
_gps_initialized = true;
@@ -1282,6 +1294,22 @@ FixedwingEstimator::task_main()
_local_pos.z_global = false;
_local_pos.yaw = _att.yaw;
+ _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
+ _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
+ _airspeed_filtered = 0.95*_airspeed_filtered + + 0.05*_airspeed.true_airspeed_m_s;
+
+
+ /* crude land detector for fixedwing only,
+ * TODO: adapt so that it works for both, maybe move to another location
+ */
+ if (_velocity_xy_filtered < 5
+ && _velocity_z_filtered < 10
+ && _airspeed_filtered < 10) {
+ _local_pos.landed = true;
+ } else {
+ _local_pos.landed = false;
+ }
+
/* lazily publish the local position only once available */
if (_local_pos_pub > 0) {
/* publish the attitude setpoint */
@@ -1300,8 +1328,8 @@ FixedwingEstimator::task_main()
_global_pos.lat = est_lat;
_global_pos.lon = est_lon;
_global_pos.time_gps_usec = _gps.time_gps_usec;
- _global_pos.eph = _gps.eph_m;
- _global_pos.epv = _gps.epv_m;
+ _global_pos.eph = _gps.eph;
+ _global_pos.epv = _gps.epv;
}
if (_local_pos.v_xy_valid) {
@@ -1321,20 +1349,38 @@ FixedwingEstimator::task_main()
_global_pos.yaw = _local_pos.yaw;
- _global_pos.eph = _gps.eph_m;
- _global_pos.epv = _gps.epv_m;
+ _global_pos.eph = _gps.eph;
+ _global_pos.epv = _gps.epv;
_global_pos.timestamp = _local_pos.timestamp;
/* lazily publish the global position only once available */
if (_global_pos_pub > 0) {
- /* publish the attitude setpoint */
+ /* publish the global position */
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
} else {
/* advertise and publish */
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
}
+
+ if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
+ _wind.timestamp = _global_pos.timestamp;
+ _wind.windspeed_north = _ekf->states[14];
+ _wind.windspeed_east = _ekf->states[15];
+ _wind.covariance_north = 0.0f; // XXX get form filter
+ _wind.covariance_east = 0.0f;
+
+ /* lazily publish the wind estimate only once available */
+ if (_wind_pub > 0) {
+ /* publish the wind estimate */
+ orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
+
+ } else {
+ /* advertise and publish */
+ _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
+ }
+ }
}
}
@@ -1396,9 +1442,11 @@ FixedwingEstimator::print_status()
printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
- printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
- printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
- printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
+ printf("states (accel offs) [14]: %8.4f\n", (double)_ekf->states[13]);
+ printf("states (wind) [15-16]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]);
+ printf("states (earth mag) [17-19]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]);
+ printf("states (body mag) [20-22]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]);
+ printf("states (terrain) [23]: %8.4f\n", (double)_ekf->states[22]);
printf("states: %s %s %s %s %s %s %s %s %s %s\n",
(_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
(_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-05 07:45:36 +0000