aboutsummaryrefslogtreecommitdiff
path: root/src/lib
diff options
context:
space:
mode:
Diffstat (limited to 'src/lib')
-rw-r--r--src/lib/conversion/module.mk2
-rw-r--r--src/lib/conversion/rotation.h4
-rw-r--r--src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp6
-rw-r--r--src/lib/ecl/attitude_fw/ecl_roll_controller.cpp6
-rw-r--r--src/lib/ecl/module.mk2
-rw-r--r--src/lib/external_lgpl/module.mk2
-rw-r--r--src/lib/external_lgpl/tecs/tecs.cpp72
-rw-r--r--src/lib/external_lgpl/tecs/tecs.h93
-rw-r--r--src/lib/geo/geo.c218
-rw-r--r--src/lib/geo/geo.h141
-rw-r--r--src/lib/launchdetection/CatapultLaunchMethod.cpp70
-rw-r--r--src/lib/launchdetection/CatapultLaunchMethod.h15
-rw-r--r--src/lib/launchdetection/LaunchDetector.cpp47
-rw-r--r--src/lib/launchdetection/LaunchDetector.h11
-rw-r--r--src/lib/launchdetection/LaunchMethod.h16
-rw-r--r--src/lib/launchdetection/launchdetection_params.c25
-rw-r--r--src/lib/rc/module.mk40
-rw-r--r--src/lib/rc/st24.c253
-rw-r--r--src/lib/rc/st24.h163
19 files changed, 1070 insertions, 116 deletions
diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk
index f5f59a2dc..4593c4887 100644
--- a/src/lib/conversion/module.mk
+++ b/src/lib/conversion/module.mk
@@ -36,3 +36,5 @@
#
SRCS = rotation.cpp
+
+MAXOPTIMIZATION = -Os
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
index 5187b448f..917c7f830 100644
--- a/src/lib/conversion/rotation.h
+++ b/src/lib/conversion/rotation.h
@@ -74,6 +74,7 @@ enum Rotation {
ROTATION_ROLL_270_YAW_135 = 23,
ROTATION_PITCH_90 = 24,
ROTATION_PITCH_270 = 25,
+ ROTATION_ROLL_270_YAW_270 = 26,
ROTATION_MAX
};
@@ -109,7 +110,8 @@ const rot_lookup_t rot_lookup[] = {
{270, 0, 90 },
{270, 0, 135 },
{ 0, 90, 0 },
- { 0, 270, 0 }
+ { 0, 270, 0 },
+ {270, 0, 270 }
};
/**
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 46db788a6..926a8db2a 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
- float id = _rate_error * dt;
+ float id = _rate_error * dt * scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
- _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
+ _last_output = _bodyrate_setpoint * _k_ff * scaler +
+ _rate_error * _k_p * scaler * scaler
+ + integrator_constrained; //scaler is proportional to 1/airspeed
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
// warnx("roll: _last_output %.4f", (double)_last_output);
return math::constrain(_last_output, -1.0f, 1.0f);
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 9894a34d7..94bd26f03 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch,
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
- float id = _rate_error * dt;
+ float id = _rate_error * dt * scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch,
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
/* Apply PI rate controller and store non-limited output */
- _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
+ _last_output = _bodyrate_setpoint * _k_ff * scaler +
+ _rate_error * _k_p * scaler * scaler
+ + integrator_constrained; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f);
}
diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk
index f2aa3db6a..93a5b511f 100644
--- a/src/lib/ecl/module.mk
+++ b/src/lib/ecl/module.mk
@@ -39,3 +39,5 @@ SRCS = attitude_fw/ecl_pitch_controller.cpp \
attitude_fw/ecl_roll_controller.cpp \
attitude_fw/ecl_yaw_controller.cpp \
l1/ecl_l1_pos_controller.cpp
+
+MAXOPTIMIZATION = -Os
diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk
index 53f1629e3..29d3514f6 100644
--- a/src/lib/external_lgpl/module.mk
+++ b/src/lib/external_lgpl/module.mk
@@ -46,3 +46,5 @@
#
SRCS = tecs/tecs.cpp
+
+MAXOPTIMIZATION = -Os
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 6386e37a0..cfcc48b62 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -236,9 +236,9 @@ void TECS::_update_height_demand(float demand, float state)
// // _hgt_rate_dem);
_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
+ _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
_hgt_dem_adj_last = _hgt_dem_adj;
- _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
// Limit height rate of change
if (_hgt_rate_dem > _maxClimbRate) {
_hgt_rate_dem = _maxClimbRate;
@@ -252,6 +252,11 @@ void TECS::_update_height_demand(float demand, float state)
void TECS::_detect_underspeed(void)
{
+ if (!_detect_underspeed_enabled) {
+ _underspeed = false;
+ return;
+ }
+
if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) {
_underspeed = true;
@@ -294,11 +299,11 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
// Calculate throttle demand
// If underspeed condition is set, then demand full throttle
if (_underspeed) {
- _throttle_dem_unc = 1.0f;
+ _throttle_dem = 1.0f;
} else {
// Calculate gain scaler from specific energy error to throttle
- float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min));
+ float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min));
// Calculate feed-forward throttle
float ff_throttle = 0;
@@ -316,28 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
}
- // Calculate PD + FF throttle
+ // Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+ _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (fabsf(_throttle_slewrate) > 0.01f) {
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
-
_throttle_dem = constrain(_throttle_dem,
- _last_throttle_dem - thrRateIncr,
- _last_throttle_dem + thrRateIncr);
- _last_throttle_dem = _throttle_dem;
+ _last_throttle_dem - thrRateIncr,
+ _last_throttle_dem + thrRateIncr);
}
+ // Ensure _last_throttle_dem is always initialized properly
+ _last_throttle_dem = _throttle_dem;
// Calculate integrator state upper and lower limits
- // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
+ // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
float integ_min = (_THRminf - _throttle_dem - 0.1f);
// Calculate integrator state, constraining state
- // Set integrator to a max throttle value dduring climbout
+ // Set integrator to a max throttle value during climbout
_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
if (_climbOutDem) {
@@ -355,10 +361,10 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
} else {
_throttle_dem = ff_throttle;
}
- }
- // Constrain throttle demand
- _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
+ // Constrain throttle demand
+ _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
+ }
}
void TECS::_detect_bad_descent(void)
@@ -551,18 +557,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
// Calculate pitch demand
_update_pitch();
-// // Write internal variables to the log_tuning structure. This
-// // structure will be logged in dataflash at 10Hz
- // log_tuning.hgt_dem = _hgt_dem_adj;
- // log_tuning.hgt = _integ3_state;
- // log_tuning.dhgt_dem = _hgt_rate_dem;
- // log_tuning.dhgt = _integ2_state;
- // log_tuning.spd_dem = _TAS_dem_adj;
- // log_tuning.spd = _integ5_state;
- // log_tuning.dspd = _vel_dot;
- // log_tuning.ithr = _integ6_state;
- // log_tuning.iptch = _integ7_state;
- // log_tuning.thr = _throttle_dem;
- // log_tuning.ptch = _pitch_dem;
- // log_tuning.dspd_dem = _TAS_rate_dem;
+ _tecs_state.timestamp = now;
+
+ if (_underspeed) {
+ _tecs_state.mode = ECL_TECS_MODE_UNDERSPEED;
+ } else if (_badDescent) {
+ _tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT;
+ } else if (_climbOutDem) {
+ _tecs_state.mode = ECL_TECS_MODE_CLIMBOUT;
+ } else {
+ // If no error flag applies, conclude normal
+ _tecs_state.mode = ECL_TECS_MODE_NORMAL;
+ }
+
+ _tecs_state.hgt_dem = _hgt_dem_adj;
+ _tecs_state.hgt = _integ3_state;
+ _tecs_state.dhgt_dem = _hgt_rate_dem;
+ _tecs_state.dhgt = _integ2_state;
+ _tecs_state.spd_dem = _TAS_dem_adj;
+ _tecs_state.spd = _integ5_state;
+ _tecs_state.dspd = _vel_dot;
+ _tecs_state.ithr = _integ6_state;
+ _tecs_state.iptch = _integ7_state;
+ _tecs_state.thr = _throttle_dem;
+ _tecs_state.ptch = _pitch_dem;
+ _tecs_state.dspd_dem = _TAS_rate_dem;
+
}
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 5cafb1c79..eb45237b7 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -28,6 +28,28 @@ class __EXPORT TECS
{
public:
TECS() :
+ _tecs_state {},
+ _update_50hz_last_usec(0),
+ _update_speed_last_usec(0),
+ _update_pitch_throttle_last_usec(0),
+ // TECS tuning parameters
+ _hgtCompFiltOmega(0.0f),
+ _spdCompFiltOmega(0.0f),
+ _maxClimbRate(2.0f),
+ _minSinkRate(1.0f),
+ _maxSinkRate(2.0f),
+ _timeConst(5.0f),
+ _timeConstThrot(8.0f),
+ _ptchDamp(0.0f),
+ _thrDamp(0.0f),
+ _integGain(0.0f),
+ _vertAccLim(0.0f),
+ _rollComp(0.0f),
+ _spdWeight(0.5f),
+ _heightrate_p(0.0f),
+ _heightrate_ff(0.0f),
+ _speedrate_p(0.0f),
+ _throttle_dem(0.0f),
_pitch_dem(0.0f),
_integ1_state(0.0f),
_integ2_state(0.0f),
@@ -45,6 +67,9 @@ public:
_hgt_dem_prev(0.0f),
_TAS_dem_adj(0.0f),
_STEdotErrLast(0.0f),
+ _underspeed(false),
+ _detect_underspeed_enabled(true),
+ _badDescent(false),
_climbOutDem(false),
_SPE_dem(0.0f),
_SKE_dem(0.0f),
@@ -100,29 +125,42 @@ public:
return _spdWeight;
}
- // log data on internal state of the controller. Called at 10Hz
- // void log_data(DataFlash_Class &dataflash, uint8_t msgid);
-
- // struct PACKED log_TECS_Tuning {
- // LOG_PACKET_HEADER;
- // float hgt;
- // float dhgt;
- // float hgt_dem;
- // float dhgt_dem;
- // float spd_dem;
- // float spd;
- // float dspd;
- // float ithr;
- // float iptch;
- // float thr;
- // float ptch;
- // float dspd_dem;
- // } log_tuning;
+ enum ECL_TECS_MODE {
+ ECL_TECS_MODE_NORMAL = 0,
+ ECL_TECS_MODE_UNDERSPEED,
+ ECL_TECS_MODE_BAD_DESCENT,
+ ECL_TECS_MODE_CLIMBOUT
+ };
+
+ struct tecs_state {
+ uint64_t timestamp;
+ float hgt;
+ float dhgt;
+ float hgt_dem;
+ float dhgt_dem;
+ float spd_dem;
+ float spd;
+ float dspd;
+ float ithr;
+ float iptch;
+ float thr;
+ float ptch;
+ float dspd_dem;
+ enum ECL_TECS_MODE mode;
+ };
+
+ void get_tecs_state(struct tecs_state& state) {
+ state = _tecs_state;
+ }
void set_time_const(float time_const) {
_timeConst = time_const;
}
+ void set_time_const_throt(float time_const_throt) {
+ _timeConstThrot = time_const_throt;
+ }
+
void set_min_sink_rate(float rate) {
_minSinkRate = rate;
}
@@ -183,11 +221,22 @@ public:
_heightrate_p = heightrate_p;
}
+ void set_heightrate_ff(float heightrate_ff) {
+ _heightrate_ff = heightrate_ff;
+ }
+
void set_speedrate_p(float speedrate_p) {
_speedrate_p = speedrate_p;
}
+ void set_detect_underspeed_enabled(bool enabled) {
+ _detect_underspeed_enabled = enabled;
+ }
+
private:
+
+ struct tecs_state _tecs_state;
+
// Last time update_50Hz was called
uint64_t _update_50hz_last_usec;
@@ -204,6 +253,7 @@ private:
float _minSinkRate;
float _maxSinkRate;
float _timeConst;
+ float _timeConstThrot;
float _ptchDamp;
float _thrDamp;
float _integGain;
@@ -211,6 +261,7 @@ private:
float _rollComp;
float _spdWeight;
float _heightrate_p;
+ float _heightrate_ff;
float _speedrate_p;
// throttle demand in the range from 0.0 to 1.0
@@ -285,15 +336,15 @@ private:
// Underspeed condition
bool _underspeed;
+ // Underspeed detection enabled
+ bool _detect_underspeed_enabled;
+
// Bad descent condition caused by unachievable airspeed demand
bool _badDescent;
// climbout mode
bool _climbOutDem;
- // throttle demand before limiting
- float _throttle_dem_unc;
-
// pitch demand before limiting
float _pitch_dem_unc;
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index e600976ce..f595467b3 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012-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
@@ -49,39 +49,124 @@
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
+#include <string.h>
+#include <float.h>
+
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
/*
* Azimuthal Equidistant Projection
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
*/
-__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+static struct map_projection_reference_s mp_ref = {0.0, 0.0, 0.0, 0.0, false, 0};
+static struct globallocal_converter_reference_s gl_ref = {0.0f, false};
+
+__EXPORT bool map_projection_global_initialized()
+{
+ return map_projection_initialized(&mp_ref);
+}
+
+__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref)
{
- ref->lat = lat_0 / 180.0 * M_PI;
- ref->lon = lon_0 / 180.0 * M_PI;
+ return ref->init_done;
+}
- ref->sin_lat = sin(ref->lat);
- ref->cos_lat = cos(ref->lat);
+__EXPORT uint64_t map_projection_global_timestamp()
+{
+ return map_projection_timestamp(&mp_ref);
}
-__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
+__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
{
- double lat_rad = lat / 180.0 * M_PI;
- double lon_rad = lon / 180.0 * M_PI;
+ return ref->timestamp;
+}
+
+__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+{
+ if (strcmp("commander", getprogname()) == 0) {
+ return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp);
+ } else {
+ return -1;
+ }
+}
+
+__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+{
+
+ ref->lat_rad = lat_0 * M_DEG_TO_RAD;
+ ref->lon_rad = lon_0 * M_DEG_TO_RAD;
+ ref->sin_lat = sin(ref->lat_rad);
+ ref->cos_lat = cos(ref->lat_rad);
+
+ ref->timestamp = timestamp;
+ ref->init_done = true;
+
+ return 0;
+}
+
+__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+{
+ return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time());
+}
+
+__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad)
+{
+ return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad);
+}
+
+__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
+{
+ if (!map_projection_initialized(ref)) {
+ return -1;
+ }
+
+ *ref_lat_rad = ref->lat_rad;
+ *ref_lon_rad = ref->lon_rad;
+
+ return 0;
+}
+
+__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y)
+{
+ return map_projection_project(&mp_ref, lat, lon, x, y);
+
+}
+
+__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
+{
+ if (!map_projection_initialized(ref)) {
+ return -1;
+ }
+
+ double lat_rad = lat * M_DEG_TO_RAD;
+ double lon_rad = lon * M_DEG_TO_RAD;
double sin_lat = sin(lat_rad);
double cos_lat = cos(lat_rad);
- double cos_d_lon = cos(lon_rad - ref->lon);
+ double cos_d_lon = cos(lon_rad - ref->lon_rad);
double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
- double k = (c == 0.0) ? 1.0 : (c / sin(c));
+ double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));
*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
- *y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH;
+ *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
+
+ return 0;
}
-__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
+__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon)
{
+ return map_projection_reproject(&mp_ref, x, y, lat, lon);
+}
+
+__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
+{
+ if (!map_projection_initialized(ref)) {
+ return -1;
+ }
+
double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
@@ -91,19 +176,101 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f
double lat_rad;
double lon_rad;
- if (c != 0.0) {
+ if (fabs(c) > DBL_EPSILON) {
lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
- lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
+ lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
} else {
- lat_rad = ref->lat;
- lon_rad = ref->lon;
+ lat_rad = ref->lat_rad;
+ lon_rad = ref->lon_rad;
}
*lat = lat_rad * 180.0 / M_PI;
*lon = lon_rad * 180.0 / M_PI;
+
+ return 0;
+}
+
+__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0)
+{
+ if (!map_projection_global_initialized()) {
+ return -1;
+ }
+
+ if (lat_0 != NULL) {
+ *lat_0 = M_RAD_TO_DEG * mp_ref.lat_rad;
+ }
+
+ if (lon_0 != NULL) {
+ *lon_0 = M_RAD_TO_DEG * mp_ref.lon_rad;
+ }
+
+ return 0;
+
+}
+__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp)
+{
+ if (strcmp("commander", getprogname()) == 0) {
+ gl_ref.alt = alt_0;
+ if (!map_projection_global_init(lat_0, lon_0, timestamp))
+ {
+ gl_ref.init_done = true;
+ return 0;
+ } else {
+ gl_ref.init_done = false;
+ return -1;
+ }
+ } else {
+ return -1;
+ }
+}
+
+__EXPORT bool globallocalconverter_initialized()
+{
+ return gl_ref.init_done && map_projection_global_initialized();
+}
+
+__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z)
+{
+ if (!map_projection_global_initialized()) {
+ return -1;
+ }
+
+ map_projection_global_project(lat, lon, x, y);
+ *z = gl_ref.alt - alt;
+
+ return 0;
}
+__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt)
+{
+ if (!map_projection_global_initialized()) {
+ return -1;
+ }
+
+ map_projection_global_reproject(x, y, lat, lon);
+ *alt = gl_ref.alt - z;
+
+ return 0;
+}
+
+__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0)
+{
+ if (!map_projection_global_initialized()) {
+ return -1;
+ }
+
+ if (map_projection_global_getref(lat_0, lon_0))
+ {
+ return -1;
+ }
+
+ if (alt_0 != NULL) {
+ *alt_0 = gl_ref.alt;
+ }
+
+ return 0;
+}
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
@@ -195,8 +362,12 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
crosstrack_error->distance = 0.0f;
crosstrack_error->bearing = 0.0f;
+ dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+
// Return error if arguments are bad
- if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
+ if (dist_to_end < 0.1f) {
+ return ERROR;
+ }
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
@@ -210,7 +381,6 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
return return_value;
}
- dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
if (sin(bearing_diff) >= 0) {
@@ -247,10 +417,10 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
- if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; }
+ if (radius < 0.1f) { return return_value; }
- if (arc_sweep >= 0) {
+ if (arc_sweep >= 0.0f) {
bearing_sector_start = arc_start_bearing;
bearing_sector_end = arc_start_bearing + arc_sweep;
@@ -296,8 +466,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
double start_disp_x = (double)radius * sin(arc_start_bearing);
double start_disp_y = (double)radius * cos(arc_start_bearing);
- double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep)));
- double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep)));
+ double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
+ double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
double lon_start = lon_now + start_disp_x / 111111.0;
double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
double lon_end = lon_now + end_disp_x / 111111.0;
@@ -317,7 +487,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
}
- crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing);
+ crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing);
return_value = OK;
return return_value;
}
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 8b286af36..2311e0a7c 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -69,39 +69,162 @@ struct crosstrack_error_s {
/* lat/lon are in radians */
struct map_projection_reference_s {
- double lat;
- double lon;
+ double lat_rad;
+ double lon_rad;
double sin_lat;
double cos_lat;
+ bool init_done;
+ uint64_t timestamp;
};
+struct globallocal_converter_reference_s {
+ float alt;
+ bool init_done;
+};
+
+/**
+ * Checks if global projection was initialized
+ * @return true if map was initialized before, false else
+ */
+__EXPORT bool map_projection_global_initialized(void);
+
+/**
+ * Checks if projection given as argument was initialized
+ * @return true if map was initialized before, false else
+ */
+__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref);
+
+/**
+ * Get the timestamp of the global map projection
+ * @return the timestamp of the map_projection
+ */
+__EXPORT uint64_t map_projection_global_timestamp(void);
+
+/**
+ * Get the timestamp of the map projection given by the argument
+ * @return the timestamp of the map_projection
+ */
+__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref);
+
+/**
+ * Writes the reference values of the global projection to ref_lat and ref_lon
+ * @return 0 if map_projection_init was called before, -1 else
+ */
+__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad);
+
/**
- * Initializes the map transformation.
+ * Writes the reference values of the projection given by the argument to ref_lat and ref_lon
+ * @return 0 if map_projection_init was called before, -1 else
+ */
+__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad);
+
+/**
+ * Initializes the global map transformation.
+ *
+ * Initializes the transformation between the geographic coordinate system and
+ * the azimuthal equidistant plane
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp);
+
+/**
+ * Initializes the map transformation given by the argument.
+ *
+ * Initializes the transformation between the geographic coordinate system and
+ * the azimuthal equidistant plane
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref,
+ double lat_0, double lon_0, uint64_t timestamp);
+
+/**
+ * Initializes the map transformation given by the argument and sets the timestamp to now.
*
- * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
+ * Initializes the transformation between the geographic coordinate system and
+ * the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
-__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
+__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
/**
- * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
+ * Transforms a point in the geographic coordinate system to the local
+ * azimuthal equidistant plane using the global projection
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
+ * @return 0 if map_projection_init was called before, -1 else
*/
-__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
+__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y);
+
+
+ /* Transforms a point in the geographic coordinate system to the local
+ * azimuthal equidistant plane using the projection given by the argument
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ * @return 0 if map_projection_init was called before, -1 else
+ */
+__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
+
+/**
+ * Transforms a point in the local azimuthal equidistant plane to the
+ * geographic coordinate system using the global projection
+ *
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ * @return 0 if map_projection_init was called before, -1 else
+ */
+__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon);
/**
- * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
+ * Transforms a point in the local azimuthal equidistant plane to the
+ * geographic coordinate system using the projection given by the argument
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
+ * @return 0 if map_projection_init was called before, -1 else
+ */
+__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
+
+/**
+ * Get reference position of the global map projection
+ */
+__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0);
+
+/**
+ * Initialize the global mapping between global position (spherical) and local position (NED).
+ */
+__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp);
+
+/**
+ * Checks if globallocalconverter was initialized
+ * @return true if map was initialized before, false else
+ */
+__EXPORT bool globallocalconverter_initialized(void);
+
+/**
+ * Convert from global position coordinates to local position coordinates using the global reference
+ */
+__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z);
+
+/**
+ * Convert from local position coordinates to global position coordinates using the global reference
+ */
+__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt);
+
+/**
+ * Get reference position of the global to local converter
*/
-__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
+__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0);
/**
* Returns the distance to the next waypoint in meters.
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
index c555a0a69..2ea1c414b 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.cpp
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -49,9 +49,11 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
SuperBlock(parent, "CAT"),
last_timestamp(hrt_absolute_time()),
integrator(0.0f),
- launchDetected(false),
- threshold_accel(this, "A"),
- threshold_time(this, "T")
+ state(LAUNCHDETECTION_RES_NONE),
+ thresholdAccel(this, "A"),
+ thresholdTime(this, "T"),
+ motorDelay(this, "MDEL"),
+ pitchMaxPreThrottle(this, "PMAX")
{
}
@@ -65,34 +67,66 @@ void CatapultLaunchMethod::update(float accel_x)
float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
last_timestamp = hrt_absolute_time();
- if (accel_x > threshold_accel.get()) {
- integrator += accel_x * dt;
-// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
-// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
- if (integrator > threshold_accel.get() * threshold_time.get()) {
- launchDetected = true;
+ switch (state) {
+ case LAUNCHDETECTION_RES_NONE:
+ /* Detect a acceleration that is longer and stronger as the minimum given by the params */
+ if (accel_x > thresholdAccel.get()) {
+ integrator += dt;
+ if (integrator > thresholdTime.get()) {
+ if (motorDelay.get() > 0.0f) {
+ state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
+ warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full"
+ " throttle", (double)motorDelay.get());
+ } else {
+ /* No motor delay set: go directly to enablemotors state */
+ state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+ warnx("Launch detected: state: enablemotors (delay not activated)");
+ }
+ }
+ } else {
+ /* reset */
+ reset();
}
+ break;
+
+ case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL:
+ /* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is
+ * over to allow full throttle */
+ motorDelayCounter += dt;
+ if (motorDelayCounter > motorDelay.get()) {
+ warnx("Launch detected: state enablemotors");
+ state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+ }
+ break;
+
+ default:
+ break;
- } else {
-// warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
-// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
- /* reset integrator */
- integrator = 0.0f;
- launchDetected = false;
}
}
-bool CatapultLaunchMethod::getLaunchDetected()
+LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const
{
- return launchDetected;
+ return state;
}
void CatapultLaunchMethod::reset()
{
integrator = 0.0f;
- launchDetected = false;
+ motorDelayCounter = 0.0f;
+ state = LAUNCHDETECTION_RES_NONE;
+}
+
+float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) {
+ /* If motor is turned on do not impose the extra limit on maximum pitch */
+ if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+ return pitchMaxDefault;
+ } else {
+ return pitchMaxPreThrottle.get();
+ }
+
}
}
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
index 23757f6f3..321dfb1de 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.h
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -57,16 +57,23 @@ public:
~CatapultLaunchMethod();
void update(float accel_x);
- bool getLaunchDetected();
+ LaunchDetectionResult getLaunchDetected() const;
void reset();
+ float getPitchMax(float pitchMaxDefault);
private:
hrt_abstime last_timestamp;
float integrator;
- bool launchDetected;
+ float motorDelayCounter;
- control::BlockParamFloat threshold_accel;
- control::BlockParamFloat threshold_time;
+ LaunchDetectionResult state;
+
+ control::BlockParamFloat thresholdAccel;
+ control::BlockParamFloat thresholdTime;
+ control::BlockParamFloat motorDelay;
+ control::BlockParamFloat pitchMaxPreThrottle; /**< Upper pitch limit before throttle is turned on.
+ Can be used to make sure that the AC does not climb
+ too much while attached to a bungee */
};
diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
index 3bf47bbb0..52f5c3257 100644
--- a/src/lib/launchdetection/LaunchDetector.cpp
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -46,6 +46,7 @@ namespace launchdetection
LaunchDetector::LaunchDetector() :
SuperBlock(NULL, "LAUN"),
+ activeLaunchDetectionMethodIndex(-1),
launchdetection_on(this, "ALL_ON"),
throttlePreTakeoff(this, "THR_PRE")
{
@@ -65,7 +66,14 @@ LaunchDetector::~LaunchDetector()
void LaunchDetector::reset()
{
/* Reset all detectors */
- launchMethods[0]->reset();
+ for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+ launchMethods[i]->reset();
+ }
+
+ /* Reset active launchdetector */
+ activeLaunchDetectionMethodIndex = -1;
+
+
}
void LaunchDetector::update(float accel_x)
@@ -77,17 +85,44 @@ void LaunchDetector::update(float accel_x)
}
}
-bool LaunchDetector::getLaunchDetected()
+LaunchDetectionResult LaunchDetector::getLaunchDetected()
{
if (launchdetection_on.get() == 1) {
- for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
- if(launchMethods[i]->getLaunchDetected()) {
- return true;
+ if (activeLaunchDetectionMethodIndex < 0) {
+ /* None of the active launchmethods has detected a launch, check all launchmethods */
+ for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+ if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
+ warnx("selecting launchmethod %d", i);
+ activeLaunchDetectionMethodIndex = i; // from now on only check this method
+ return launchMethods[i]->getLaunchDetected();
+ }
}
+ } else {
+ return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected();
}
}
- return false;
+ return LAUNCHDETECTION_RES_NONE;
}
+float LaunchDetector::getPitchMax(float pitchMaxDefault) {
+ if (!launchdetection_on.get()) {
+ return pitchMaxDefault;
+ }
+
+ /* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method,
+ * otherwise use the default limit */
+ if (activeLaunchDetectionMethodIndex < 0) {
+ if (sizeof(launchMethods)/sizeof(LaunchMethod) > 1) {
+ return pitchMaxDefault;
+ } else {
+ return launchMethods[0]->getPitchMax(pitchMaxDefault);
+ }
+ } else {
+ return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault);
+ }
+
+}
+
+
}
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
index 1a214b66e..4215b49d2 100644
--- a/src/lib/launchdetection/LaunchDetector.h
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -59,14 +59,23 @@ public:
void reset();
void update(float accel_x);
- bool getLaunchDetected();
+ LaunchDetectionResult getLaunchDetected();
bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
+ /* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */
+ float getPitchMax(float pitchMaxDefault);
+
// virtual bool getLaunchDetected();
protected:
private:
+ int activeLaunchDetectionMethodIndex; /**< holds a index to the launchMethod in the array launchMethods
+ which detected a Launch. If no launchMethod has detected a launch yet the
+ value is -1. Once one launchMetthod has detected a launch only this
+ method is checked for further adavancing in the state machine (e.g. when
+ to power up the motors) */
+
LaunchMethod* launchMethods[1];
control::BlockParamInt launchdetection_on;
control::BlockParamFloat throttlePreTakeoff;
diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h
index e04467f6a..8b5220cb3 100644
--- a/src/lib/launchdetection/LaunchMethod.h
+++ b/src/lib/launchdetection/LaunchMethod.h
@@ -44,13 +44,27 @@
namespace launchdetection
{
+enum LaunchDetectionResult {
+ LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */
+ LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should
+ control the attitude. However any motors should not throttle
+ up and still be set to 'throttlePreTakeoff'.
+ For instance this is used to have a delay for the motor
+ when launching a fixed wing aircraft from a bungee */
+ LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, teh controller should control
+ attitude and also throttle up the motors. */
+};
+
class LaunchMethod
{
public:
virtual void update(float accel_x) = 0;
- virtual bool getLaunchDetected() = 0;
+ virtual LaunchDetectionResult getLaunchDetected() const = 0;
virtual void reset() = 0;
+ /* Returns a upper pitch limit if required, otherwise returns pitchMaxDefault */
+ virtual float getPitchMax(float pitchMaxDefault) = 0;
+
protected:
private:
};
diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c
index 8df8c696c..e3aa7ab2d 100644
--- a/src/lib/launchdetection/launchdetection_params.c
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -78,6 +78,31 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
/**
+ * Motor delay
+ *
+ * Delay between starting attitude control and powering up the throttle (giving throttle control to the controller)
+ * Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate
+ *
+ * @unit seconds
+ * @min 0
+ * @group Launch detection
+ */
+PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f);
+
+/**
+ * Maximum pitch before the throttle is powered up (during motor delay phase)
+ *
+ * This is an extra limit for the maximum pitch which is imposed in the phase before the throttle turns on.
+ * This allows to limit the maximum pitch angle during a bungee launch (make the launch less steep).
+ *
+ * @unit deg
+ * @min 0
+ * @max 45
+ * @group Launch detection
+ */
+PARAM_DEFINE_FLOAT(LAUN_CAT_PMAX, 30.0f);
+
+/**
* Throttle setting while detecting launch.
*
* The throttle is set to this value while the system is waiting for the take-off.
diff --git a/src/lib/rc/module.mk b/src/lib/rc/module.mk
new file mode 100644
index 000000000..e089c6965
--- /dev/null
+++ b/src/lib/rc/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+# 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.
+#
+############################################################################
+
+#
+# Yuntec ST24 transmitter protocol decoder
+#
+
+SRCS = st24.c
+
+MAXOPTIMIZATION = -Os
diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c
new file mode 100644
index 000000000..e8a791b8f
--- /dev/null
+++ b/src/lib/rc/st24.c
@@ -0,0 +1,253 @@
+/****************************************************************************
+ *
+ * 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 st24.h
+ *
+ * RC protocol implementation for Yuneec ST24 transmitter.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <stdbool.h>
+#include <stdio.h>
+#include "st24.h"
+
+enum ST24_DECODE_STATE {
+ ST24_DECODE_STATE_UNSYNCED = 0,
+ ST24_DECODE_STATE_GOT_STX1,
+ ST24_DECODE_STATE_GOT_STX2,
+ ST24_DECODE_STATE_GOT_LEN,
+ ST24_DECODE_STATE_GOT_TYPE,
+ ST24_DECODE_STATE_GOT_DATA
+};
+
+const char *decode_states[] = {"UNSYNCED",
+ "GOT_STX1",
+ "GOT_STX2",
+ "GOT_LEN",
+ "GOT_TYPE",
+ "GOT_DATA"
+ };
+
+/* define range mapping here, -+100% -> 1000..2000 */
+#define ST24_RANGE_MIN 0.0f
+#define ST24_RANGE_MAX 4096.0f
+
+#define ST24_TARGET_MIN 1000.0f
+#define ST24_TARGET_MAX 2000.0f
+
+/* pre-calculate the floating point stuff as far as possible at compile time */
+#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN))
+#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f))
+
+static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
+static unsigned _rxlen;
+
+static ReceiverFcPacket _rxpacket;
+
+uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
+{
+ uint8_t i, crc ;
+ crc = 0;
+
+ while (len--) {
+ for (i = 0x80; i != 0; i >>= 1) {
+ if ((crc & 0x80) != 0) {
+ crc <<= 1;
+ crc ^= 0x07;
+
+ } else {
+ crc <<= 1;
+ }
+
+ if ((*ptr & i) != 0) {
+ crc ^= 0x07;
+ }
+ }
+
+ ptr++;
+ }
+
+ return (crc);
+}
+
+
+int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels,
+ uint16_t max_chan_count)
+{
+
+ int ret = 1;
+
+ switch (_decode_state) {
+ case ST24_DECODE_STATE_UNSYNCED:
+ if (byte == ST24_STX1) {
+ _decode_state = ST24_DECODE_STATE_GOT_STX1;
+ } else {
+ ret = 3;
+ }
+
+ break;
+
+ case ST24_DECODE_STATE_GOT_STX1:
+ if (byte == ST24_STX2) {
+ _decode_state = ST24_DECODE_STATE_GOT_STX2;
+
+ } else {
+ _decode_state = ST24_DECODE_STATE_UNSYNCED;
+ }
+
+ break;
+
+ case ST24_DECODE_STATE_GOT_STX2:
+
+ /* ensure no data overflow failure or hack is possible */
+ if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) {
+ _rxpacket.length = byte;
+ _rxlen = 0;
+ _decode_state = ST24_DECODE_STATE_GOT_LEN;
+
+ } else {
+ _decode_state = ST24_DECODE_STATE_UNSYNCED;
+ }
+
+ break;
+
+ case ST24_DECODE_STATE_GOT_LEN:
+ _rxpacket.type = byte;
+ _rxlen++;
+ _decode_state = ST24_DECODE_STATE_GOT_TYPE;
+ break;
+
+ case ST24_DECODE_STATE_GOT_TYPE:
+ _rxpacket.st24_data[_rxlen - 1] = byte;
+ _rxlen++;
+
+ if (_rxlen == (_rxpacket.length - 1)) {
+ _decode_state = ST24_DECODE_STATE_GOT_DATA;
+ }
+
+ break;
+
+ case ST24_DECODE_STATE_GOT_DATA:
+ _rxpacket.crc8 = byte;
+ _rxlen++;
+
+ if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) {
+
+ ret = 0;
+
+ /* decode the actual packet */
+
+ switch (_rxpacket.type) {
+
+ case ST24_PACKET_TYPE_CHANNELDATA12: {
+ ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data;
+
+ *rssi = d->rssi;
+ *rx_count = d->packet_count;
+
+ /* this can lead to rounding of the strides */
+ *channel_count = (max_chan_count < 12) ? max_chan_count : 12;
+
+ unsigned stride_count = (*channel_count * 3) / 2;
+ unsigned chan_index = 0;
+
+ for (unsigned i = 0; i < stride_count; i += 3) {
+ channels[chan_index] = ((uint16_t)d->channel[i] << 4);
+ channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
+ /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+ channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+ chan_index++;
+
+ channels[chan_index] = ((uint16_t)d->channel[i + 2]);
+ channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
+ /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+ channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+ chan_index++;
+ }
+ }
+ break;
+
+ case ST24_PACKET_TYPE_CHANNELDATA24: {
+ ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data;
+
+ *rssi = d->rssi;
+ *rx_count = d->packet_count;
+
+ /* this can lead to rounding of the strides */
+ *channel_count = (max_chan_count < 24) ? max_chan_count : 24;
+
+ unsigned stride_count = (*channel_count * 3) / 2;
+ unsigned chan_index = 0;
+
+ for (unsigned i = 0; i < stride_count; i += 3) {
+ channels[chan_index] = ((uint16_t)d->channel[i] << 4);
+ channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
+ /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+ channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+ chan_index++;
+
+ channels[chan_index] = ((uint16_t)d->channel[i + 2]);
+ channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
+ /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+ channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+ chan_index++;
+ }
+ }
+ break;
+
+ case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: {
+
+ // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data;
+ /* we silently ignore this data for now, as it is unused */
+ ret = 2;
+ }
+ break;
+
+ default:
+ ret = 2;
+ break;
+ }
+
+ } else {
+ /* decoding failed */
+ ret = 4;
+ }
+
+ _decode_state = ST24_DECODE_STATE_UNSYNCED;
+ break;
+ }
+
+ return ret;
+}
diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h
new file mode 100644
index 000000000..454234601
--- /dev/null
+++ b/src/lib/rc/st24.h
@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ * 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 st24.h
+ *
+ * RC protocol definition for Yuneec ST24 transmitter
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+#define ST24_DATA_LEN_MAX 64
+#define ST24_STX1 0x55
+#define ST24_STX2 0x55
+
+enum ST24_PACKET_TYPE {
+ ST24_PACKET_TYPE_CHANNELDATA12 = 0,
+ ST24_PACKET_TYPE_CHANNELDATA24,
+ ST24_PACKET_TYPE_TRANSMITTERGPSDATA
+};
+
+#pragma pack(push, 1)
+typedef struct {
+ uint8_t header1; ///< 0x55 for a valid packet
+ uint8_t header2; ///< 0x55 for a valid packet
+ uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8)
+ uint8_t type; ///< from enum ST24_PACKET_TYPE
+ uint8_t st24_data[ST24_DATA_LEN_MAX];
+ uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
+} ReceiverFcPacket;
+
+/**
+ * RC Channel data (12 channels).
+ *
+ * This is incoming from the ST24
+ */
+typedef struct {
+ uint16_t t; ///< packet counter or clock
+ uint8_t rssi; ///< signal strength
+ uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
+ uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers)
+} ChannelData12;
+
+/**
+ * RC Channel data (12 channels).
+ *
+ */
+typedef struct {
+ uint16_t t; ///< packet counter or clock
+ uint8_t rssi; ///< signal strength
+ uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
+ uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers)
+} ChannelData24;
+
+/**
+ * Telemetry packet
+ *
+ * This is outgoing to the ST24
+ *
+ * imuStatus:
+ * 8 bit total
+ * bits 0-2 for status
+ * - value 0 is FAILED
+ * - value 1 is INITIALIZING
+ * - value 2 is RUNNING
+ * - values 3 through 7 are reserved
+ * bits 3-7 are status for sensors (0 or 1)
+ * - mpu6050
+ * - accelerometer
+ * - primary gyro x
+ * - primary gyro y
+ * - primary gyro z
+ *
+ * pressCompassStatus
+ * 8 bit total
+ * bits 0-3 for compass status
+ * - value 0 is FAILED
+ * - value 1 is INITIALIZING
+ * - value 2 is RUNNING
+ * - value 3 - 15 are reserved
+ * bits 4-7 for pressure status
+ * - value 0 is FAILED
+ * - value 1 is INITIALIZING
+ * - value 2 is RUNNING
+ * - value 3 - 15 are reserved
+ *
+ */
+typedef struct {
+ uint16_t t; ///< packet counter or clock
+ int32_t lat; ///< lattitude (degrees) +/- 90 deg
+ int32_t lon; ///< longitude (degrees) +/- 180 deg
+ int32_t alt; ///< 0.01m resolution, altitude (meters)
+ int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down
+ uint8_t nsat; ///<number of satellites
+ uint8_t voltage; ///< 25.4V voltage = 5 + 255*0.1 = 30.5V, min=5V
+ uint8_t current; ///< 0.5A resolution
+ int16_t roll, pitch, yaw; ///< 0.01 degree resolution
+ uint8_t motorStatus; ///< 1 bit per motor for status 1=good, 0= fail
+ uint8_t imuStatus; ///< inertial measurement unit status
+ uint8_t pressCompassStatus; ///< baro / compass status
+} TelemetryData;
+
+#pragma pack(pop)
+
+/**
+ * CRC8 implementation for ST24 protocol
+ *
+ * @param prt Pointer to the data to CRC
+ * @param len number of bytes to accumulate in the checksum
+ * @return the checksum of these bytes over len
+ */
+uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len);
+
+/**
+ * Decoder for ST24 protocol
+ *
+ * @param byte current char to read
+ * @param rssi pointer to a byte where the RSSI value is written back to
+ * @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to
+ * @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to
+ * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned
+ * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error
+ */
+__EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
+ uint16_t *channels, uint16_t max_chan_count);
+
+__END_DECLS
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-02 01:52:29 +0000