forked from Archive/PX4-Autopilot
Merge pull request #1036 from PX4/mtecs_wind_estimate
mTECS branch wind estimation
This commit is contained in:
Thomas Gubler
commit
b40fcb0aac
|
|
@ -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;
|
||||
|
|
@ -312,6 +315,7 @@ FixedwingEstimator::FixedwingEstimator() :
|
|||
_global_pos_pub(-1),
|
||||
_local_pos_pub(-1),
|
||||
_estimator_status_pub(-1),
|
||||
_wind_pub(-1),
|
||||
|
||||
_att({}),
|
||||
_gyro({}),
|
||||
|
|
@ -323,6 +327,7 @@ FixedwingEstimator::FixedwingEstimator() :
|
|||
_global_pos({}),
|
||||
_local_pos({}),
|
||||
_gps({}),
|
||||
_wind({}),
|
||||
|
||||
_gyro_offsets({}),
|
||||
_accel_offsets({}),
|
||||
|
|
@ -1328,13 +1333,31 @@ FixedwingEstimator::task_main()
|
|||
|
||||
/* 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 +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 (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",
|
||||
|
|
|
|||
|
|
@ -2042,10 +2042,10 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max)
|
|||
float ret;
|
||||
if (val > 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) {
|
||||
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 {
|
||||
ret = val;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -87,6 +87,7 @@
|
|||
#include <uORB/topics/tecs_status.h>
|
||||
#include <uORB/topics/system_power.h>
|
||||
#include <uORB/topics/servorail_status.h>
|
||||
#include <uORB/topics/wind_estimate.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
|
@ -943,6 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
struct tecs_status_s tecs_status;
|
||||
struct system_power_s system_power;
|
||||
struct servorail_status_s servorail_status;
|
||||
struct wind_estimate_s wind_estimate;
|
||||
} buf;
|
||||
|
||||
memset(&buf, 0, sizeof(buf));
|
||||
|
|
@ -982,6 +984,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
struct log_GS1A_s log_GS1A;
|
||||
struct log_GS1B_s log_GS1B;
|
||||
struct log_TECS_s log_TECS;
|
||||
struct log_WIND_s log_WIND;
|
||||
} body;
|
||||
} log_msg = {
|
||||
LOG_PACKET_HEADER_INIT(0)
|
||||
|
|
@ -1016,6 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
int tecs_status_sub;
|
||||
int system_power_sub;
|
||||
int servorail_status_sub;
|
||||
int wind_sub;
|
||||
} subs;
|
||||
|
||||
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
|
|
@ -1044,6 +1048,9 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
|
||||
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
|
||||
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
|
||||
subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
|
||||
/* we need to rate-limit wind, as we do not need the full update rate */
|
||||
orb_set_interval(subs.wind_sub, 90);
|
||||
|
||||
thread_running = true;
|
||||
|
||||
|
|
@ -1512,6 +1519,16 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
LOGBUFFER_WRITE_AND_COUNT(TECS);
|
||||
}
|
||||
|
||||
/* --- WIND ESTIMATE --- */
|
||||
if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) {
|
||||
log_msg.msg_type = LOG_WIND_MSG;
|
||||
log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north;
|
||||
log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east;
|
||||
log_msg.body.log_WIND.cov_x = buf.wind_estimate.covariance_north;
|
||||
log_msg.body.log_WIND.cov_y = buf.wind_estimate.covariance_east;
|
||||
LOGBUFFER_WRITE_AND_COUNT(WIND);
|
||||
}
|
||||
|
||||
/* signal the other thread new data, but not yet unlock */
|
||||
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
|
||||
/* only request write if several packets can be written at once */
|
||||
|
|
|
|||
|
|
@ -366,6 +366,15 @@ struct log_TECS_s {
|
|||
uint8_t mode;
|
||||
};
|
||||
|
||||
/* --- WIND - WIND ESTIMATE --- */
|
||||
#define LOG_WIND_MSG 31
|
||||
struct log_WIND_s {
|
||||
float x;
|
||||
float y;
|
||||
float cov_x;
|
||||
float cov_y;
|
||||
};
|
||||
|
||||
/********** SYSTEM MESSAGES, ID > 0x80 **********/
|
||||
|
||||
/* --- TIME - TIME STAMP --- */
|
||||
|
|
@ -422,6 +431,7 @@ static const struct log_format_s log_formats[] = {
|
|||
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
LOG_FORMAT(TECS, "ffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"),
|
||||
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
|
||||
|
||||
/* system-level messages, ID >= 0x80 */
|
||||
/* FMT: don't write format of format message, it's useless */
|
||||
|
|
|
|||
|
|
@ -203,3 +203,5 @@ ORB_DEFINE(estimator_status, struct estimator_status_report);
|
|||
#include "topics/tecs_status.h"
|
||||
ORB_DEFINE(tecs_status, struct tecs_status_s);
|
||||
|
||||
#include "topics/wind_estimate.h"
|
||||
ORB_DEFINE(wind_estimate, struct wind_estimate_s);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,68 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file wind_estimate.h
|
||||
*
|
||||
* Wind estimate topic topic
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef TOPIC_WIND_ESTIMATE_H
|
||||
#define TOPIC_WIND_ESTIMATE_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/** Wind estimate */
|
||||
struct wind_estimate_s {
|
||||
|
||||
uint64_t timestamp; /**< Microseconds since system boot */
|
||||
float windspeed_north; /**< Wind component in north / X direction */
|
||||
float windspeed_east; /**< Wind component in east / Y direction */
|
||||
float covariance_north; /**< Uncertainty - set to zero (no uncertainty) if not estimated */
|
||||
float covariance_east; /**< Uncertainty - set to zero (no uncertainty) if not estimated */
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
ORB_DECLARE(wind_estimate);
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue