att / pos estimator: Publishing wind estimate

This commit is contained in:
Lorenz Meier 2014-06-01 15:35:01 +02:00
parent ff4aec6588
commit 50342f9661
2 changed files with 31 additions and 6 deletions

View File

@ -74,6 +74,7 @@
#include <uORB/topics/estimator_status.h> #include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h> #include <uORB/topics/home_position.h>
#include <uORB/topics/wind_estimate.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <geo/geo.h> #include <geo/geo.h>
@ -158,6 +159,7 @@ private:
orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _global_pos_pub; /**< global position */
orb_advert_t _local_pos_pub; /**< position in local frame */ orb_advert_t _local_pos_pub; /**< position in local frame */
orb_advert_t _estimator_status_pub; /**< status of the estimator */ 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 vehicle_attitude_s _att; /**< vehicle attitude */
struct gyro_report _gyro; struct gyro_report _gyro;
@ -169,6 +171,7 @@ private:
struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_local_position_s _local_pos; /**< local vehicle position */
struct vehicle_gps_position_s _gps; /**< GPS position */ struct vehicle_gps_position_s _gps; /**< GPS position */
struct wind_estimate_s _wind; /**< Wind estimate */
struct gyro_scale _gyro_offsets; struct gyro_scale _gyro_offsets;
struct accel_scale _accel_offsets; struct accel_scale _accel_offsets;
@ -312,6 +315,7 @@ FixedwingEstimator::FixedwingEstimator() :
_global_pos_pub(-1), _global_pos_pub(-1),
_local_pos_pub(-1), _local_pos_pub(-1),
_estimator_status_pub(-1), _estimator_status_pub(-1),
_wind_pub(-1),
_att({}), _att({}),
_gyro({}), _gyro({}),
@ -323,6 +327,7 @@ FixedwingEstimator::FixedwingEstimator() :
_global_pos({}), _global_pos({}),
_local_pos({}), _local_pos({}),
_gps({}), _gps({}),
_wind({}),
_gyro_offsets({}), _gyro_offsets({}),
_accel_offsets({}), _accel_offsets({}),
@ -1328,13 +1333,31 @@ FixedwingEstimator::task_main()
/* lazily publish the global position only once available */ /* lazily publish the global position only once available */
if (_global_pos_pub > 0) { 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); orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
} else { } else {
/* advertise and publish */ /* advertise and publish */
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); _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 +1419,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 (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 (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 (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 (accel offs) [14]: %8.4f\n", (double)_ekf->states[13]);
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 (wind) [15-16]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]);
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 (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", printf("states: %s %s %s %s %s %s %s %s %s %s\n",
(_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
(_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",

View File

@ -2042,10 +2042,10 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max)
float ret; float ret;
if (val > max) { if (val > max) {
ret = max; ret = max;
ekf_debug("> max: %8.4f, val: %8.4f", max, val); ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val);
} else if (val < min) { } else if (val < min) {
ret = min; ret = min;
ekf_debug("< min: %8.4f, val: %8.4f", min, val); ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val);
} else { } else {
ret = val; ret = val;
} }