dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge remote-tracking branch 'px4/mtecs' into navigator_rewrite

This commit is contained in:
Julian Oes 2014-06-06 23:16:39 +02:00
parent 289094e059 e03411fc89
commit 078eed41af
14 changed files with 300 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
makefiles/config_px4fmu-v1_default.mk
View File

@ -52,7 +52,6 @@ MODULES += systemcmds/pwm
MODULES += systemcmds/esc_calib
MODULES += systemcmds/reboot
MODULES += systemcmds/top
MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
MODULES += systemcmds/dumpfile
@ -67,12 +66,11 @@ MODULES += modules/mavlink
MODULES += modules/gpio_led
#
# Estimation modules (EKF/ SO3 / other filters)
# Estimation modules (EKF / other filters)
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav
#MODULES += examples/flow_position_estimator
#
# Vehicle Control
@ -81,8 +79,6 @@ MODULES += modules/fw_pos_control_l1
MODULES += modules/fw_att_control
MODULES += modules/mc_att_control
MODULES += modules/mc_pos_control
#MODULES += examples/flow_position_control
#MODULES += examples/flow_speed_control
#
# Logging

2
src/lib/launchdetection/module.mk
View File

@ -38,3 +38,5 @@
SRCS = LaunchDetector.cpp \
CatapultLaunchMethod.cpp \
launchdetection_params.c
MAXOPTIMIZATION = -Os

33
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -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;
@ -316,6 +319,7 @@ FixedwingEstimator::FixedwingEstimator() :
_global_pos_pub(-1),
_local_pos_pub(-1),
_estimator_status_pub(-1),
_wind_pub(-1),
_att({}),
_gyro({}),
@ -327,6 +331,7 @@ FixedwingEstimator::FixedwingEstimator() :
_global_pos({}),
_local_pos({}),
_gps({}),
_wind({}),
_gyro_offsets({}),
_accel_offsets({}),
@ -1351,13 +1356,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);
}
}
}
}
@ -1419,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",

4
src/modules/ekf_att_pos_estimator/estimator.cpp
View File

@ -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;
}

25
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -380,7 +380,7 @@ private:
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
fwPosctrl::mTecs::tecs_mode mode = fwPosctrl::mTecs::TECS_MODE_NORMAL);
tecs_mode mode = TECS_MODE_NORMAL);
};
@ -835,7 +835,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
if (!_mTecs.getEnabled()) {
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
}
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
/* no throttle limit as default */
@ -851,6 +854,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
}
}
@ -1008,7 +1012,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
0.0f, throttle_max, throttle_land,
false, flare_pitch_angle_rad,
_pos_sp_triplet.current.alt + relative_alt, ground_speed,
land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND);
land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
if (!land_noreturn_vertical) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
@ -1101,7 +1105,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::radians(10.0f)),
_global_pos.alt,
ground_speed,
fwPosctrl::mTecs::TECS_MODE_TAKEOFF);
TECS_MODE_TAKEOFF);
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
@ -1439,16 +1443,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
fwPosctrl::mTecs::tecs_mode mode)
tecs_mode mode)
{
if (_mTecs.getEnabled()) {
/* Using mtecs library: prepare arguments for mtecs call */
float flightPathAngle = 0.0f;
float ground_speed_length = ground_speed.length();
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode);
fwPosctrl::mTecs::LimitOverride limitOverride;
if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
} else {
limitOverride.disablePitchMinOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
} else {
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
climbout_mode, climbout_pitch_min_rad,

62
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
View File

@ -79,7 +79,8 @@ mTecs::~mTecs()
{
}
int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode)
int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
{
/* check if all input arguments are numbers and abort if not so */
if (!isfinite(flightPathAngle) || !isfinite(altitude) ||
@ -105,10 +106,12 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
/* use flightpath angle setpoint for total energy control */
return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode);
return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed,
airspeedSp, mode, limitOverride);
}
int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode)
int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
{
/* check if all input arguments are numbers and abort if not so */
if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
@ -135,10 +138,12 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
/* use longitudinal acceleration setpoint for total energy control */
return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode);
return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed,
accelerationLongitudinalSp, mode, limitOverride);
}
int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode)
int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed,
float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride)
{
/* check if all input arguments are numbers and abort if not so */
if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
@ -180,8 +185,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2);
}
/* Check airspeed: if below safe value switch to underspeed mode */
if (airspeed < _airspeedMin.get()) {
/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeed < _airspeedMin.get()) {
mode = TECS_MODE_UNDERSPEED;
}
@ -202,6 +207,16 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
}
/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
* parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
* is running) */
bool limitApplied = limitOverride.applyOverride(outputLimiterThrottle == NULL ?
_controlTotalEnergy.getOutputLimiter() :
*outputLimiterThrottle,
outputLimiterPitch == NULL ?
_controlEnergyDistribution.getOutputLimiter() :
*outputLimiterPitch);
/* Write part of the status message */
_status.airspeedDerivativeSp = airspeedDerivativeSp;
_status.airspeedDerivative = airspeedDerivative;
@ -209,6 +224,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
_status.totalEnergyRate = totalEnergyRate;
_status.energyDistributionRateSp = energyDistributionRateSp;
_status.energyDistributionRate = energyDistributionRate;
_status.mode = mode;
/** update control blocks **/
/* update total energy rate control block */
@ -245,6 +261,12 @@ void mTecs::resetIntegrators()
_firstIterationAfterReset = true;
}
void mTecs::resetDerivatives(float airspeed)
{
_airspeedDerivative.setU(airspeed);
}
void mTecs::updateTimeMeasurement()
{
if (!_dtCalculated) {
@ -280,5 +302,29 @@ void mTecs::debug(const char *fmt, ...) {
debug_print(fmt, args);
}
} /* namespace fwPosctrl */
bool mTecs::LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
BlockOutputLimiter &outputLimiterPitch)
{
bool ret = false;
if (overrideThrottleMinEnabled) {
outputLimiterThrottle.setMin(overrideThrottleMin);
ret = true;
}
if (overrideThrottleMaxEnabled) {
outputLimiterThrottle.setMax(overrideThrottleMax);
ret = true;
}
if (overridePitchMinEnabled) {
outputLimiterPitch.setMin(overridePitchMin);
ret = true;
}
if (overridePitchMaxEnabled) {
outputLimiterPitch.setMax(overridePitchMax);
ret = true;
}
return ret;
}
} /* namespace fwPosctrl */

75
src/modules/fw_pos_control_l1/mtecs/mTecs.h
View File

@ -60,34 +60,89 @@ public:
mTecs();
virtual ~mTecs();
typedef enum {
TECS_MODE_NORMAL,
TECS_MODE_UNDERSPEED,
TECS_MODE_TAKEOFF,
TECS_MODE_LAND,
TECS_MODE_LAND_THROTTLELIM
} tecs_mode;
/* A small class which provides helper fucntions to override control output limits which are usually set by
* parameters in special cases
*/
class LimitOverride
{
public:
LimitOverride() :
overrideThrottleMinEnabled(false),
overrideThrottleMaxEnabled(false),
overridePitchMinEnabled(false),
overridePitchMaxEnabled(false)
{};
~LimitOverride() {};
/*
* Override the limits of the outputlimiter instances given by the arguments with the limits saved in
* this class (if enabled)
* @return true if the limit was applied
*/
bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
BlockOutputLimiter &outputLimiterPitch);
/* Functions to enable or disable the override */
void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
&overrideThrottleMin, value); }
void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
&overrideThrottleMax, value); }
void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
&overridePitchMin, value); }
void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
&overridePitchMax, value); }
void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
protected:
bool overrideThrottleMinEnabled;
float overrideThrottleMin;
bool overrideThrottleMaxEnabled;
float overrideThrottleMax;
bool overridePitchMinEnabled;
float overridePitchMin; //in degrees (replaces param values)
bool overridePitchMaxEnabled;
float overridePitchMax; //in degrees (replaces param values)
/* Enable a specific limit override */
void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value;
};
/* Disable a specific limit override */
void disable(bool *flag) { *flag = false; };
};
/*
* Control in altitude setpoint and speed mode
*/
int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode);
int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
/*
* Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode
*/
int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode);
int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
/*
* Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
*/
int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode);
int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed,
float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride);
/*
* Reset all integrators
*/
void resetIntegrators();
/*
* Reset all derivative calculations
*/
void resetDerivatives(float airspeed);
/* Accessors */
bool getEnabled() {return _mTecsEnabled.get() > 0;}

3
src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
View File

@ -89,6 +89,8 @@ public:
bool isAngularLimit() {return _isAngularLimit ;}
float getMin() { return _min.get(); }
float getMax() { return _max.get(); }
void setMin(float value) { _min.set(value); }
void setMax(float value) { _max.set(value); }
protected:
//attributes
bool _isAngularLimit;
@ -96,7 +98,6 @@ protected:
control::BlockParamFloat _max;
};
typedef
/* A combination of feed forward, P and I gain using the output limiter*/
class BlockFFPILimited: public SuperBlock

21
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
View File

@ -58,7 +58,8 @@
PARAM_DEFINE_INT32(MT_ENABLED, 1);
/**
* Total Energy Rate Control FF
* Total Energy Rate Control Feedforward
* Maps the total energy rate setpoint to the throttle setpoint
*
* @min 0.0
* @max 10.0
@ -68,6 +69,7 @@ PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f);
/**
* Total Energy Rate Control P
* Maps the total energy rate error to the throttle setpoint
*
* @min 0.0
* @max 10.0
@ -77,6 +79,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f);
/**
* Total Energy Rate Control I
* Maps the integrated total energy rate to the throttle setpoint
*
* @min 0.0
* @max 10.0
@ -85,7 +88,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f);
PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f);
/**
* Total Energy Rate Control OFF (Cruise throttle)
* Total Energy Rate Control Offset (Cruise throttle sp)
*
* @min 0.0
* @max 10.0
@ -94,7 +97,8 @@ PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f);
PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f);
/**
* Energy Distribution Rate Control FF
* Energy Distribution Rate Control Feedforward
* Maps the energy distribution rate setpoint to the pitch setpoint
*
* @min 0.0
* @max 10.0
@ -104,6 +108,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.1f);
/**
* Energy Distribution Rate Control P
* Maps the energy distribution rate error to the pitch setpoint
*
* @min 0.0
* @max 10.0
@ -113,6 +118,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f);
/**
* Energy Distribution Rate Control I
* Maps the integrated energy distribution rate error to the pitch setpoint
*
* @min 0.0
* @max 10.0
@ -122,7 +128,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f);
/**
* Total Energy Distribution OFF (Cruise pitch sp)
* Total Energy Distribution Offset (Cruise pitch sp)
*
* @min 0.0
* @max 10.0
@ -170,6 +176,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
/**
* P gain for the altitude control
* Maps the altitude error to the flight path angle setpoint
*
* @min 0.0f
* @max 10.0f
@ -179,6 +186,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f);
/**
* D gain for the altitude control
* Maps the change of altitude error to the flight path angle setpoint
*
* @min 0.0f
* @max 10.0f
@ -187,7 +195,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f);
PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f);
/**
* Lowpass for FPA error derivative (see MT_FPA_D)
* Lowpass for FPA error derivative calculation (see MT_FPA_D)
*
* @group mTECS
*/
@ -217,6 +225,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
/**
* P gain for the airspeed control
* Maps the airspeed error to the acceleration setpoint
*
* @min 0.0f
* @max 10.0f
@ -241,7 +250,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f);
PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
/**
* Airspeed derivative lowpass
* Airspeed derivative calculation lowpass
*
* @group mTECS
*/

18
src/modules/sdlog2/sdlog2.c
View File

@ -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;
@ -1508,9 +1515,20 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate;
log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp;
log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate;
log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
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 */

14
src/modules/sdlog2/sdlog2_messages.h
View File

@ -362,6 +362,17 @@ struct log_TECS_s {
float totalEnergyRate;
float energyDistributionRateSp;
float energyDistributionRate;
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 **********/
@ -419,7 +430,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
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, "ffffffffffff", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR"),
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 */

2
src/modules/uORB/objects_common.cpp
View File

@ -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);

10
src/modules/uORB/topics/tecs_status.h
View File

@ -50,6 +50,14 @@
* @{
*/
typedef enum {
TECS_MODE_NORMAL,
TECS_MODE_UNDERSPEED,
TECS_MODE_TAKEOFF,
TECS_MODE_LAND,
TECS_MODE_LAND_THROTTLELIM
} tecs_mode;
/**
* Internal values of the (m)TECS fixed wing speed alnd altitude control system
*/
@ -69,6 +77,8 @@ struct tecs_status_s {
float totalEnergyRate;
float energyDistributionRateSp;
float energyDistributionRate;
tecs_mode mode;
};
/**

68
src/modules/uORB/topics/wind_estimate.h Normal file
View File

@ -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