forked from Archive/PX4-Autopilot
Merge branch 'master' into mpc_track
This commit is contained in:
Anton Babushkin
commit
fac2855579
|
|
@ -0,0 +1,41 @@
|
|||
The PX4 firmware is licensed generally under a permissive 3-clause BSD license. Contributions are required
|
||||
to be made under the same license. Any exception to this general rule is listed below.
|
||||
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
- PX4 middleware: BSD 3-clause
|
||||
- PX4 flight control stack: BSD 3-clause
|
||||
- NuttX operating system: BSD 3-clause
|
||||
- Exceptions: Currently only this [400 LOC file](https://github.com/PX4/Firmware/blob/master/src/lib/external_lgpl/tecs/tecs.cpp) remains LGPL, but will be replaced with a BSD implementation.
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
## PX4 Aerial Middleware and Flight Control Stack ##
|
||||
|
||||
* Official Website: http://px4.io
|
||||
* License: BSD 3-clause (see LICENSE.md)
|
||||
* Supported airframes:
|
||||
* Multicopters
|
||||
* Fixed wing
|
||||
* Binaries (always up-to-date from master):
|
||||
* [Downloads](https://pixhawk.org/downloads)
|
||||
* Mailing list: [Google Groups](http://groups.google.com/group/px4users)
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -27,8 +27,6 @@ MODULES += drivers/ms5611
|
|||
MODULES += drivers/mb12xx
|
||||
MODULES += drivers/gps
|
||||
MODULES += drivers/hil
|
||||
MODULES += drivers/hott/hott_telemetry
|
||||
MODULES += drivers/hott/hott_sensors
|
||||
MODULES += drivers/blinkm
|
||||
MODULES += drivers/rgbled
|
||||
MODULES += drivers/mkblctrl
|
||||
|
|
@ -42,7 +40,6 @@ MODULES += modules/sensors
|
|||
# System commands
|
||||
#
|
||||
MODULES += systemcmds/mtd
|
||||
MODULES += systemcmds/bl_update
|
||||
MODULES += systemcmds/mixer
|
||||
MODULES += systemcmds/param
|
||||
MODULES += systemcmds/perf
|
||||
|
|
|
|||
|
|
@ -54,6 +54,9 @@ MODULES += lib/conversion
|
|||
#
|
||||
LIBRARIES += lib/mathlib/CMSIS
|
||||
|
||||
MODULES += modules/unit_test
|
||||
MODULES += modules/mavlink/mavlink_tests
|
||||
|
||||
#
|
||||
# Transitional support - add commands from the NuttX export archive.
|
||||
#
|
||||
|
|
|
|||
|
|
@ -238,7 +238,7 @@ void TECS::_update_height_demand(float demand, float state)
|
|||
_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
|
||||
_hgt_dem_adj_last = _hgt_dem_adj;
|
||||
|
||||
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
|
||||
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
|
||||
// Limit height rate of change
|
||||
if (_hgt_rate_dem > _maxClimbRate) {
|
||||
_hgt_rate_dem = _maxClimbRate;
|
||||
|
|
|
|||
|
|
@ -47,6 +47,7 @@ public:
|
|||
_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),
|
||||
|
|
@ -220,6 +221,10 @@ 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;
|
||||
}
|
||||
|
|
@ -256,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
|
||||
|
|
|
|||
|
|
@ -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 += 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_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();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
};
|
||||
|
|
|
|||
|
|
@ -77,6 +77,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.
|
||||
*
|
||||
|
|
|
|||
|
|
@ -58,6 +58,7 @@
|
|||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
#include <drivers/drv_range_finder.h>
|
||||
#ifdef SENSOR_COMBINED_SUB
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#endif
|
||||
|
|
@ -96,7 +97,10 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
|
|||
|
||||
__EXPORT uint32_t millis();
|
||||
|
||||
__EXPORT uint64_t getMicros();
|
||||
|
||||
static uint64_t IMUmsec = 0;
|
||||
static uint64_t IMUusec = 0;
|
||||
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
|
||||
|
||||
uint32_t millis()
|
||||
|
|
@ -104,6 +108,11 @@ uint32_t millis()
|
|||
return IMUmsec;
|
||||
}
|
||||
|
||||
uint64_t getMicros()
|
||||
{
|
||||
return IMUusec;
|
||||
}
|
||||
|
||||
class FixedwingEstimator
|
||||
{
|
||||
public:
|
||||
|
|
@ -171,6 +180,7 @@ private:
|
|||
#else
|
||||
int _sensor_combined_sub;
|
||||
#endif
|
||||
int _distance_sub; /**< distance measurement */
|
||||
int _airspeed_sub; /**< airspeed subscription */
|
||||
int _baro_sub; /**< barometer subscription */
|
||||
int _gps_sub; /**< GPS subscription */
|
||||
|
|
@ -196,7 +206,8 @@ 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 wind_estimate_s _wind; /**< wind estimate */
|
||||
struct range_finder_report _distance; /**< distance estimate */
|
||||
|
||||
struct gyro_scale _gyro_offsets;
|
||||
struct accel_scale _accel_offsets;
|
||||
|
|
@ -226,6 +237,7 @@ private:
|
|||
hrt_abstime _filter_start_time;
|
||||
hrt_abstime _last_sensor_timestamp;
|
||||
hrt_abstime _last_run;
|
||||
hrt_abstime _distance_last_valid;
|
||||
bool _gyro_valid;
|
||||
bool _accel_valid;
|
||||
bool _mag_valid;
|
||||
|
|
@ -342,6 +354,7 @@ FixedwingEstimator::FixedwingEstimator() :
|
|||
#else
|
||||
_sensor_combined_sub(-1),
|
||||
#endif
|
||||
_distance_sub(-1),
|
||||
_airspeed_sub(-1),
|
||||
_baro_sub(-1),
|
||||
_gps_sub(-1),
|
||||
|
|
@ -399,6 +412,7 @@ FixedwingEstimator::FixedwingEstimator() :
|
|||
_filter_start_time(0),
|
||||
_last_sensor_timestamp(0),
|
||||
_last_run(0),
|
||||
_distance_last_valid(0),
|
||||
_gyro_valid(false),
|
||||
_accel_valid(false),
|
||||
_mag_valid(false),
|
||||
|
|
@ -549,6 +563,7 @@ FixedwingEstimator::parameters_update()
|
|||
_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
|
||||
_ekf->accelProcessNoise = _parameters.acc_pnoise;
|
||||
_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
|
||||
_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
|
||||
}
|
||||
|
||||
return OK;
|
||||
|
|
@ -704,6 +719,7 @@ FixedwingEstimator::task_main()
|
|||
/*
|
||||
* do subscriptions
|
||||
*/
|
||||
_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder));
|
||||
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
|
||||
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
|
|
@ -753,6 +769,7 @@ FixedwingEstimator::task_main()
|
|||
bool newHgtData = false;
|
||||
bool newAdsData = false;
|
||||
bool newDataMag = false;
|
||||
bool newRangeData = false;
|
||||
|
||||
float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
|
||||
|
||||
|
|
@ -850,7 +867,8 @@ FixedwingEstimator::task_main()
|
|||
}
|
||||
|
||||
_last_sensor_timestamp = _gyro.timestamp;
|
||||
IMUmsec = _gyro.timestamp / 1e3f;
|
||||
IMUmsec = _gyro.timestamp / 1e3;
|
||||
IMUusec = _gyro.timestamp;
|
||||
|
||||
float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
|
||||
_last_run = _gyro.timestamp;
|
||||
|
|
@ -914,7 +932,8 @@ FixedwingEstimator::task_main()
|
|||
|
||||
// Copy gyro and accel
|
||||
_last_sensor_timestamp = _sensor_combined.timestamp;
|
||||
IMUmsec = _sensor_combined.timestamp / 1e3f;
|
||||
IMUmsec = _sensor_combined.timestamp / 1e3;
|
||||
IMUusec = _sensor_combined.timestamp;
|
||||
|
||||
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
|
||||
|
||||
|
|
@ -994,8 +1013,6 @@ FixedwingEstimator::task_main()
|
|||
|
||||
if (gps_updated) {
|
||||
|
||||
last_gps = _gps.timestamp_position;
|
||||
|
||||
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
|
||||
perf_count(_perf_gps);
|
||||
|
||||
|
|
@ -1008,11 +1025,17 @@ FixedwingEstimator::task_main()
|
|||
_gps_start_time = hrt_absolute_time();
|
||||
|
||||
/* check if we had a GPS outage for a long time */
|
||||
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
|
||||
float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f;
|
||||
|
||||
const float pos_reset_threshold = 5.0f; // seconds
|
||||
|
||||
/* timeout of 5 seconds */
|
||||
if (gps_elapsed > pos_reset_threshold) {
|
||||
_ekf->ResetPosition();
|
||||
_ekf->ResetVelocity();
|
||||
_ekf->ResetStoredStates();
|
||||
}
|
||||
_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold));
|
||||
|
||||
/* fuse GPS updates */
|
||||
|
||||
|
|
@ -1044,6 +1067,8 @@ FixedwingEstimator::task_main()
|
|||
|
||||
newDataGps = true;
|
||||
|
||||
last_gps = _gps.timestamp_position;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1052,8 +1077,15 @@ FixedwingEstimator::task_main()
|
|||
orb_check(_baro_sub, &baro_updated);
|
||||
|
||||
if (baro_updated) {
|
||||
|
||||
hrt_abstime baro_last = _baro.timestamp;
|
||||
|
||||
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
|
||||
|
||||
float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
|
||||
|
||||
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1));
|
||||
|
||||
_ekf->baroHgt = _baro.altitude;
|
||||
|
||||
if (!_baro_init) {
|
||||
|
|
@ -1114,6 +1146,19 @@ FixedwingEstimator::task_main()
|
|||
newDataMag = false;
|
||||
}
|
||||
|
||||
orb_check(_distance_sub, &newRangeData);
|
||||
|
||||
if (newRangeData) {
|
||||
orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
|
||||
|
||||
if (_distance.valid) {
|
||||
_ekf->rngMea = _distance.distance;
|
||||
_distance_last_valid = _distance.timestamp;
|
||||
} else {
|
||||
newRangeData = false;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
|
||||
*/
|
||||
|
|
@ -1197,6 +1242,7 @@ FixedwingEstimator::task_main()
|
|||
} else if (_ekf->statesInitialised) {
|
||||
|
||||
// We're apparently initialized in this case now
|
||||
// check (and reset the filter as needed)
|
||||
int check = check_filter_state();
|
||||
|
||||
if (check) {
|
||||
|
|
@ -1206,21 +1252,7 @@ FixedwingEstimator::task_main()
|
|||
|
||||
// Run the strapdown INS equations every IMU update
|
||||
_ekf->UpdateStrapdownEquationsNED();
|
||||
#if 0
|
||||
// debug code - could be tunred into a filter mnitoring/watchdog function
|
||||
float tempQuat[4];
|
||||
|
||||
for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
|
||||
|
||||
quat2eul(eulerEst, tempQuat);
|
||||
|
||||
for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
|
||||
|
||||
if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
|
||||
|
||||
if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
|
||||
|
||||
#endif
|
||||
// store the predicted states for subsequent use by measurement fusion
|
||||
_ekf->StoreStates(IMUmsec);
|
||||
// Check if on ground - status is used by covariance prediction
|
||||
|
|
@ -1334,6 +1366,13 @@ FixedwingEstimator::task_main()
|
|||
_ekf->fuseVtasData = false;
|
||||
}
|
||||
|
||||
if (newRangeData) {
|
||||
_ekf->fuseRngData = true;
|
||||
_ekf->useRangeFinder = true;
|
||||
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f));
|
||||
_ekf->GroundEKF();
|
||||
}
|
||||
|
||||
|
||||
// Output results
|
||||
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
|
||||
|
|
@ -1447,6 +1486,10 @@ FixedwingEstimator::task_main()
|
|||
_global_pos.vel_d = _local_pos.vz;
|
||||
}
|
||||
|
||||
/* terrain altitude */
|
||||
_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
|
||||
_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
|
||||
(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
|
||||
|
||||
_global_pos.yaw = _local_pos.yaw;
|
||||
|
||||
|
|
@ -1467,8 +1510,10 @@ FixedwingEstimator::task_main()
|
|||
|
||||
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.windspeed_north = _ekf->windSpdFiltNorth;
|
||||
_wind.windspeed_east = _ekf->windSpdFiltEast;
|
||||
// XXX we need to do something smart about the covariance here
|
||||
// but we default to the estimate covariance for now
|
||||
_wind.covariance_north = _ekf->P[14][14];
|
||||
_wind.covariance_east = _ekf->P[15][15];
|
||||
|
||||
|
|
|
|||
|
|
@ -2,6 +2,11 @@
|
|||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h>
|
||||
#include <math.h>
|
||||
|
||||
#ifndef M_PI_F
|
||||
#define M_PI_F ((float)M_PI)
|
||||
#endif
|
||||
|
||||
#define EKF_COVARIANCE_DIVERGED 1.0e8f
|
||||
|
||||
|
|
@ -38,13 +43,14 @@ AttPosEKF::AttPosEKF() :
|
|||
resetStates{},
|
||||
storedStates{},
|
||||
statetimeStamp{},
|
||||
lastVelPosFusion(millis()),
|
||||
statesAtVelTime{},
|
||||
statesAtPosTime{},
|
||||
statesAtHgtTime{},
|
||||
statesAtMagMeasTime{},
|
||||
statesAtVtasMeasTime{},
|
||||
statesAtRngTime{},
|
||||
statesAtOptFlowTime{},
|
||||
statesAtFlowTime{},
|
||||
correctedDelAng(),
|
||||
correctedDelVel(),
|
||||
summedDelAng(),
|
||||
|
|
@ -59,7 +65,16 @@ AttPosEKF::AttPosEKF() :
|
|||
accel(),
|
||||
dVelIMU(),
|
||||
dAngIMU(),
|
||||
dtIMU(0),
|
||||
dtIMU(0.005f),
|
||||
dtIMUfilt(0.005f),
|
||||
dtVelPos(0.01f),
|
||||
dtVelPosFilt(0.01f),
|
||||
dtHgtFilt(0.01f),
|
||||
dtGpsFilt(0.1f),
|
||||
windSpdFiltNorth(0.0f),
|
||||
windSpdFiltEast(0.0f),
|
||||
windSpdFiltAltitude(0.0f),
|
||||
windSpdFiltClimb(0.0f),
|
||||
fusionModeGPS(0),
|
||||
innovVelPos{},
|
||||
varInnovVelPos{},
|
||||
|
|
@ -103,13 +118,13 @@ AttPosEKF::AttPosEKF() :
|
|||
|
||||
inhibitWindStates(true),
|
||||
inhibitMagStates(true),
|
||||
inhibitGndHgtState(true),
|
||||
inhibitGndState(true),
|
||||
|
||||
onGround(true),
|
||||
staticMode(true),
|
||||
useAirspeed(true),
|
||||
useCompass(true),
|
||||
useRangeFinder(false),
|
||||
useRangeFinder(true),
|
||||
useOpticalFlow(false),
|
||||
|
||||
ekfDiverged(false),
|
||||
|
|
@ -117,7 +132,24 @@ AttPosEKF::AttPosEKF() :
|
|||
current_ekf_state{},
|
||||
last_ekf_error{},
|
||||
numericalProtection(true),
|
||||
storeIndex(0)
|
||||
storeIndex(0),
|
||||
storedOmega{},
|
||||
Popt{},
|
||||
flowStates{},
|
||||
prevPosN(0.0f),
|
||||
prevPosE(0.0f),
|
||||
auxFlowObsInnov{},
|
||||
auxFlowObsInnovVar{},
|
||||
fScaleFactorVar(0.0f),
|
||||
Tnb_flow{},
|
||||
R_LOS(0.0f),
|
||||
auxFlowTestRatio{},
|
||||
auxRngTestRatio(0.0f),
|
||||
flowInnovGate(0.0f),
|
||||
auxFlowInnovGate(0.0f),
|
||||
rngInnovGate(0.0f),
|
||||
minFlowRng(0.0f),
|
||||
moCompR_LOS(0.0f)
|
||||
{
|
||||
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
|
||||
memset(¤t_ekf_state, 0, sizeof(current_ekf_state));
|
||||
|
|
@ -260,6 +292,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
|
|||
|
||||
// Constrain states (to protect against filter divergence)
|
||||
ConstrainStates();
|
||||
|
||||
// update filtered IMU time step length
|
||||
dtIMUfilt = 0.99f * dtIMUfilt + 0.01f * dtIMU;
|
||||
}
|
||||
|
||||
void AttPosEKF::CovariancePrediction(float dt)
|
||||
|
|
@ -312,7 +347,7 @@ void AttPosEKF::CovariancePrediction(float dt)
|
|||
} else {
|
||||
for (uint8_t i=16; i<=21; i++) processNoise[i] = 0;
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
if (!inhibitGndState) {
|
||||
processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma;
|
||||
} else {
|
||||
processNoise[22] = 0;
|
||||
|
|
@ -962,6 +997,21 @@ void AttPosEKF::CovariancePrediction(float dt)
|
|||
ConstrainVariances();
|
||||
}
|
||||
|
||||
void AttPosEKF::updateDtGpsFilt(float dt)
|
||||
{
|
||||
dtGpsFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtGpsFilt * 0.95f;
|
||||
}
|
||||
|
||||
void AttPosEKF::updateDtHgtFilt(float dt)
|
||||
{
|
||||
dtHgtFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtHgtFilt * 0.95f;
|
||||
}
|
||||
|
||||
void AttPosEKF::updateDtVelPosFilt(float dt)
|
||||
{
|
||||
dtVelPosFilt = ConstrainFloat(dt, 0.0005f, 2.0f) * 0.05f + dtVelPosFilt * 0.95f;
|
||||
}
|
||||
|
||||
void AttPosEKF::FuseVelposNED()
|
||||
{
|
||||
|
||||
|
|
@ -998,6 +1048,18 @@ void AttPosEKF::FuseVelposNED()
|
|||
// associated with sequential fusion
|
||||
if (fuseVelData || fusePosData || fuseHgtData)
|
||||
{
|
||||
uint64_t tNow = getMicros();
|
||||
updateDtVelPosFilt((tNow - lastVelPosFusion) / 1e6f);
|
||||
lastVelPosFusion = tNow;
|
||||
|
||||
// scaler according to the number of repetitions of the
|
||||
// same measurement in one fusion step
|
||||
float gpsVarianceScaler = dtGpsFilt / dtVelPosFilt;
|
||||
|
||||
// scaler according to the number of repetitions of the
|
||||
// same measurement in one fusion step
|
||||
float hgtVarianceScaler = dtHgtFilt / dtVelPosFilt;
|
||||
|
||||
// set the GPS data timeout depending on whether airspeed data is present
|
||||
if (useAirspeed) horizRetryTime = gpsRetryTime;
|
||||
else horizRetryTime = gpsRetryTimeNoTAS;
|
||||
|
|
@ -1010,12 +1072,12 @@ void AttPosEKF::FuseVelposNED()
|
|||
// Estimate the GPS Velocity, GPS horiz position and height measurement variances.
|
||||
velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
|
||||
posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
|
||||
R_OBS[0] = sq(vneSigma) + sq(velErr);
|
||||
R_OBS[0] = gpsVarianceScaler * sq(vneSigma) + sq(velErr);
|
||||
R_OBS[1] = R_OBS[0];
|
||||
R_OBS[2] = sq(vdSigma) + sq(velErr);
|
||||
R_OBS[3] = sq(posNeSigma) + sq(posErr);
|
||||
R_OBS[2] = gpsVarianceScaler * sq(vdSigma) + sq(velErr);
|
||||
R_OBS[3] = gpsVarianceScaler * sq(posNeSigma) + sq(posErr);
|
||||
R_OBS[4] = R_OBS[3];
|
||||
R_OBS[5] = sq(posDSigma) + sq(posErr);
|
||||
R_OBS[5] = hgtVarianceScaler * sq(posDSigma) + sq(posErr);
|
||||
|
||||
// calculate innovations and check GPS data validity using an innovation consistency check
|
||||
if (fuseVelData)
|
||||
|
|
@ -1173,7 +1235,7 @@ void AttPosEKF::FuseVelposNED()
|
|||
}
|
||||
}
|
||||
// Don't update terrain state if inhibited
|
||||
if (inhibitGndHgtState) {
|
||||
if (inhibitGndState) {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
|
||||
|
|
@ -1356,7 +1418,7 @@ void AttPosEKF::FuseMagnetometer()
|
|||
Kfusion[i] = 0;
|
||||
}
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
if (!inhibitGndState) {
|
||||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
|
|
@ -1430,11 +1492,6 @@ void AttPosEKF::FuseMagnetometer()
|
|||
Kfusion[20] = 0;
|
||||
Kfusion[21] = 0;
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
varInnovMag[1] = 1.0f/SK_MY[0];
|
||||
innovMag[1] = MagPred[1] - magData.y;
|
||||
}
|
||||
|
|
@ -1505,11 +1562,6 @@ void AttPosEKF::FuseMagnetometer()
|
|||
Kfusion[20] = 0;
|
||||
Kfusion[21] = 0;
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
varInnovMag[2] = 1.0f/SK_MZ[0];
|
||||
innovMag[2] = MagPred[2] - magData.z;
|
||||
|
||||
|
|
@ -1616,6 +1668,32 @@ void AttPosEKF::FuseAirspeed()
|
|||
// Perform fusion of True Airspeed measurement
|
||||
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
|
||||
{
|
||||
|
||||
float altDiff = fabsf(windSpdFiltAltitude - hgtMea);
|
||||
|
||||
if (isfinite(windSpdFiltClimb)) {
|
||||
windSpdFiltClimb = ((1.0f - 0.0002f) * windSpdFiltClimb) + (0.0002f * states[6]);
|
||||
} else {
|
||||
windSpdFiltClimb = states[6];
|
||||
}
|
||||
|
||||
if (altDiff < 20.0f) {
|
||||
// Lowpass the output of the wind estimate - we want a long-term
|
||||
// stable estimate, but not start to load into the overall dynamics
|
||||
// of the system (which adjusting covariances would do)
|
||||
|
||||
// Change filter coefficient based on altitude change rate
|
||||
float windFiltCoeff = ConstrainFloat(fabsf(windSpdFiltClimb) / 1000.0f, 0.00005f, 0.2f);
|
||||
|
||||
windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn);
|
||||
windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe);
|
||||
} else {
|
||||
windSpdFiltNorth = vwn;
|
||||
windSpdFiltEast = vwe;
|
||||
}
|
||||
|
||||
windSpdFiltAltitude = hgtMea;
|
||||
|
||||
// Calculate observation jacobians
|
||||
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
|
||||
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
|
||||
|
|
@ -1675,11 +1753,6 @@ void AttPosEKF::FuseAirspeed()
|
|||
Kfusion[i] = 0;
|
||||
}
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
varInnovVtas = 1.0f/SK_TAS;
|
||||
|
||||
// Calculate the measurement innovation
|
||||
|
|
@ -1811,8 +1884,11 @@ void AttPosEKF::FuseRangeFinder()
|
|||
rngPred = (ptd - pd)/cosRngTilt;
|
||||
innovRng = rngPred - rngMea;
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > 5Sigma
|
||||
if ((innovRng*innovRng*SK_RNG[0]) < 25)
|
||||
// calculate the innovation consistency test ratio
|
||||
auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if out of bounds
|
||||
if (auxRngTestRatio < 1.0f)
|
||||
{
|
||||
// correct the state vector
|
||||
states[22] = states[22] - Kfusion[22] * innovRng;
|
||||
|
|
@ -1827,285 +1903,387 @@ void AttPosEKF::FuseRangeFinder()
|
|||
|
||||
void AttPosEKF::FuseOptFlow()
|
||||
{
|
||||
static uint8_t obsIndex;
|
||||
static float SH_LOS[13];
|
||||
static float SKK_LOS[15];
|
||||
static float SK_LOS[2];
|
||||
static float q0 = 0.0f;
|
||||
static float q1 = 0.0f;
|
||||
static float q2 = 0.0f;
|
||||
static float q3 = 1.0f;
|
||||
static float vn = 0.0f;
|
||||
static float ve = 0.0f;
|
||||
static float vd = 0.0f;
|
||||
static float pd = 0.0f;
|
||||
static float ptd = 0.0f;
|
||||
static float R_LOS = 0.01f;
|
||||
static float losPred[2];
|
||||
// static uint8_t obsIndex;
|
||||
// static float SH_LOS[13];
|
||||
// static float SKK_LOS[15];
|
||||
// static float SK_LOS[2];
|
||||
// static float q0 = 0.0f;
|
||||
// static float q1 = 0.0f;
|
||||
// static float q2 = 0.0f;
|
||||
// static float q3 = 1.0f;
|
||||
// static float vn = 0.0f;
|
||||
// static float ve = 0.0f;
|
||||
// static float vd = 0.0f;
|
||||
// static float pd = 0.0f;
|
||||
// static float ptd = 0.0f;
|
||||
// static float R_LOS = 0.01f;
|
||||
// static float losPred[2];
|
||||
|
||||
// Transformation matrix from nav to body axes
|
||||
Mat3f Tnb_local;
|
||||
// Transformation matrix from body to sensor axes
|
||||
// assume camera is aligned with Z body axis plus a misalignment
|
||||
// defined by 3 small angles about X, Y and Z body axis
|
||||
Mat3f Tbs;
|
||||
Tbs.x.y = a3;
|
||||
Tbs.y.x = -a3;
|
||||
Tbs.x.z = -a2;
|
||||
Tbs.z.x = a2;
|
||||
Tbs.y.z = a1;
|
||||
Tbs.z.y = -a1;
|
||||
// Transformation matrix from navigation to sensor axes
|
||||
Mat3f Tns;
|
||||
float H_LOS[n_states];
|
||||
for (uint8_t i = 0; i < n_states; i++) {
|
||||
H_LOS[i] = 0.0f;
|
||||
// // Transformation matrix from nav to body axes
|
||||
// Mat3f Tnb_local;
|
||||
// // Transformation matrix from body to sensor axes
|
||||
// // assume camera is aligned with Z body axis plus a misalignment
|
||||
// // defined by 3 small angles about X, Y and Z body axis
|
||||
// Mat3f Tbs;
|
||||
// Tbs.x.y = a3;
|
||||
// Tbs.y.x = -a3;
|
||||
// Tbs.x.z = -a2;
|
||||
// Tbs.z.x = a2;
|
||||
// Tbs.y.z = a1;
|
||||
// Tbs.z.y = -a1;
|
||||
// // Transformation matrix from navigation to sensor axes
|
||||
// Mat3f Tns;
|
||||
// float H_LOS[n_states];
|
||||
// for (uint8_t i = 0; i < n_states; i++) {
|
||||
// H_LOS[i] = 0.0f;
|
||||
// }
|
||||
// Vector3f velNED_local;
|
||||
// Vector3f relVelSensor;
|
||||
|
||||
// // Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg.
|
||||
// if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f)
|
||||
// {
|
||||
// // Sequential fusion of XY components to spread processing load across
|
||||
// // two prediction time steps.
|
||||
|
||||
// // Calculate observation jacobians and Kalman gains
|
||||
// if (fuseOptFlowData)
|
||||
// {
|
||||
// // Copy required states to local variable names
|
||||
// q0 = statesAtOptFlowTime[0];
|
||||
// q1 = statesAtOptFlowTime[1];
|
||||
// q2 = statesAtOptFlowTime[2];
|
||||
// q3 = statesAtOptFlowTime[3];
|
||||
// vn = statesAtOptFlowTime[4];
|
||||
// ve = statesAtOptFlowTime[5];
|
||||
// vd = statesAtOptFlowTime[6];
|
||||
// pd = statesAtOptFlowTime[9];
|
||||
// ptd = statesAtOptFlowTime[22];
|
||||
// velNED_local.x = vn;
|
||||
// velNED_local.y = ve;
|
||||
// velNED_local.z = vd;
|
||||
|
||||
// // calculate rotation from NED to body axes
|
||||
// float q00 = sq(q0);
|
||||
// float q11 = sq(q1);
|
||||
// float q22 = sq(q2);
|
||||
// float q33 = sq(q3);
|
||||
// float q01 = q0 * q1;
|
||||
// float q02 = q0 * q2;
|
||||
// float q03 = q0 * q3;
|
||||
// float q12 = q1 * q2;
|
||||
// float q13 = q1 * q3;
|
||||
// float q23 = q2 * q3;
|
||||
// Tnb_local.x.x = q00 + q11 - q22 - q33;
|
||||
// Tnb_local.y.y = q00 - q11 + q22 - q33;
|
||||
// Tnb_local.z.z = q00 - q11 - q22 + q33;
|
||||
// Tnb_local.y.x = 2*(q12 - q03);
|
||||
// Tnb_local.z.x = 2*(q13 + q02);
|
||||
// Tnb_local.x.y = 2*(q12 + q03);
|
||||
// Tnb_local.z.y = 2*(q23 - q01);
|
||||
// Tnb_local.x.z = 2*(q13 - q02);
|
||||
// Tnb_local.y.z = 2*(q23 + q01);
|
||||
|
||||
// // calculate transformation from NED to sensor axes
|
||||
// Tns = Tbs*Tnb_local;
|
||||
|
||||
// // calculate range from ground plain to centre of sensor fov assuming flat earth
|
||||
// float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f);
|
||||
|
||||
// // calculate relative velocity in sensor frame
|
||||
// relVelSensor = Tns*velNED_local;
|
||||
|
||||
// // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
|
||||
// losPred[0] = relVelSensor.y/range;
|
||||
// losPred[1] = -relVelSensor.x/range;
|
||||
|
||||
// //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y);
|
||||
// //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y);
|
||||
// //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range);
|
||||
|
||||
// // Calculate observation jacobians
|
||||
// SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
|
||||
// SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
// SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
// SH_LOS[3] = 1/(pd - ptd);
|
||||
// SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2;
|
||||
// SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3;
|
||||
// SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1;
|
||||
// SH_LOS[7] = 1/sq(pd - ptd);
|
||||
// SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1;
|
||||
// SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0;
|
||||
// SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3;
|
||||
// SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0;
|
||||
// SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2;
|
||||
|
||||
// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
// H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
// H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
// H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3);
|
||||
// H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
// H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3));
|
||||
// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3));
|
||||
// H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
// H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
|
||||
// // Calculate Kalman gain
|
||||
// SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3);
|
||||
// SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3);
|
||||
// SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3);
|
||||
// SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3);
|
||||
// SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3);
|
||||
// SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3);
|
||||
// SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
// SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
// SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
// SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]);
|
||||
// SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
// SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
// SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
// SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]);
|
||||
// SKK_LOS[14] = SH_LOS[0];
|
||||
|
||||
// SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]));
|
||||
// Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// varInnovOptFlow[0] = 1.0f/SK_LOS[0];
|
||||
// innovOptFlow[0] = losPred[0] - losData[0];
|
||||
|
||||
// // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag
|
||||
// obsIndex = 1;
|
||||
// fuseOptFlowData = false;
|
||||
// }
|
||||
// else if (obsIndex == 1) // we are now fusing the Y measurement
|
||||
// {
|
||||
// // Calculate observation jacobians
|
||||
// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
// H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
// H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
// H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
// H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1);
|
||||
// H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3));
|
||||
// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3));
|
||||
// H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
// H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
|
||||
// // Calculate Kalman gains
|
||||
// SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]));
|
||||
// Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// varInnovOptFlow[1] = 1.0f/SK_LOS[1];
|
||||
// innovOptFlow[1] = losPred[1] - losData[1];
|
||||
|
||||
// // reset the observation index
|
||||
// obsIndex = 0;
|
||||
// fuseOptFlowData = false;
|
||||
// }
|
||||
|
||||
// // Check the innovation for consistency and don't fuse if > 3Sigma
|
||||
// if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f)
|
||||
// {
|
||||
// // correct the state vector
|
||||
// for (uint8_t j = 0; j < n_states; j++)
|
||||
// {
|
||||
// states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
|
||||
// }
|
||||
// // normalise the quaternion states
|
||||
// float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
|
||||
// if (quatMag > 1e-12f)
|
||||
// {
|
||||
// for (uint8_t j= 0; j<=3; j++)
|
||||
// {
|
||||
// float quatMagInv = 1.0f/quatMag;
|
||||
// states[j] = states[j] * quatMagInv;
|
||||
// }
|
||||
// }
|
||||
// // correct the covariance P = (I - K*H)*P
|
||||
// // take advantage of the empty columns in KH to reduce the
|
||||
// // number of operations
|
||||
// for (uint8_t i = 0; i < n_states; i++)
|
||||
// {
|
||||
// for (uint8_t j = 0; j <= 6; j++)
|
||||
// {
|
||||
// KH[i][j] = Kfusion[i] * H_LOS[j];
|
||||
// }
|
||||
// for (uint8_t j = 7; j <= 8; j++)
|
||||
// {
|
||||
// KH[i][j] = 0.0f;
|
||||
// }
|
||||
// KH[i][9] = Kfusion[i] * H_LOS[9];
|
||||
// for (uint8_t j = 10; j <= 21; j++)
|
||||
// {
|
||||
// KH[i][j] = 0.0f;
|
||||
// }
|
||||
// KH[i][22] = Kfusion[i] * H_LOS[22];
|
||||
// }
|
||||
// for (uint8_t i = 0; i < n_states; i++)
|
||||
// {
|
||||
// for (uint8_t j = 0; j < n_states; j++)
|
||||
// {
|
||||
// KHP[i][j] = 0.0f;
|
||||
// for (uint8_t k = 0; k <= 6; k++)
|
||||
// {
|
||||
// KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||||
// }
|
||||
// KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
|
||||
// KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// for (uint8_t i = 0; i < n_states; i++)
|
||||
// {
|
||||
// for (uint8_t j = 0; j < n_states; j++)
|
||||
// {
|
||||
// P[i][j] = P[i][j] - KHP[i][j];
|
||||
// }
|
||||
// }
|
||||
// ForceSymmetry();
|
||||
// ConstrainVariances();
|
||||
// }
|
||||
}
|
||||
|
||||
/*
|
||||
Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF
|
||||
This fiter requires optical flow rates that are not motion compensated
|
||||
Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser
|
||||
*/
|
||||
void AttPosEKF::GroundEKF()
|
||||
{
|
||||
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
|
||||
// limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
|
||||
if (!inhibitGndState) {
|
||||
float distanceTravelledSq;
|
||||
distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE);
|
||||
prevPosN = statesAtRngTime[7];
|
||||
prevPosE = statesAtRngTime[8];
|
||||
distanceTravelledSq = min(distanceTravelledSq, 100.0f);
|
||||
Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma));
|
||||
}
|
||||
Vector3f velNED_local;
|
||||
Vector3f relVelSensor;
|
||||
// we aren't using optical flow measurements in this hacked implementation so set the covariances for this state to zero to avoid numerical problems
|
||||
Popt[0][0] = 0.0f;
|
||||
Popt[0][1] = 0.0f;
|
||||
Popt[1][0] = 0.0f;
|
||||
|
||||
// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg.
|
||||
if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f)
|
||||
{
|
||||
// Sequential fusion of XY components to spread processing load across
|
||||
// two prediction time steps.
|
||||
// Fuse range finder data
|
||||
// Need to check that our range finder tilt angle is less than 30 degrees
|
||||
float cosRngTilt = - Tbn.z.x * sinf(rngFinderPitch) + Tbn.z.z * cosf(rngFinderPitch);
|
||||
if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) {
|
||||
float range; // range from camera to centre of image
|
||||
float q0; // quaternion at optical flow measurement time
|
||||
float q1; // quaternion at optical flow measurement time
|
||||
float q2; // quaternion at optical flow measurement time
|
||||
float q3; // quaternion at optical flow measurement time
|
||||
float R_RNG = 0.5; // range measurement variance (m^2) TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
|
||||
|
||||
// Calculate observation jacobians and Kalman gains
|
||||
if (fuseOptFlowData)
|
||||
{
|
||||
// Copy required states to local variable names
|
||||
q0 = statesAtOptFlowTime[0];
|
||||
q1 = statesAtOptFlowTime[1];
|
||||
q2 = statesAtOptFlowTime[2];
|
||||
q3 = statesAtOptFlowTime[3];
|
||||
vn = statesAtOptFlowTime[4];
|
||||
ve = statesAtOptFlowTime[5];
|
||||
vd = statesAtOptFlowTime[6];
|
||||
pd = statesAtOptFlowTime[9];
|
||||
ptd = statesAtOptFlowTime[22];
|
||||
velNED_local.x = vn;
|
||||
velNED_local.y = ve;
|
||||
velNED_local.z = vd;
|
||||
// Copy required states to local variable names
|
||||
q0 = statesAtRngTime[0];
|
||||
q1 = statesAtRngTime[1];
|
||||
q2 = statesAtRngTime[2];
|
||||
q3 = statesAtRngTime[3];
|
||||
|
||||
// calculate rotation from NED to body axes
|
||||
float q00 = sq(q0);
|
||||
float q11 = sq(q1);
|
||||
float q22 = sq(q2);
|
||||
float q33 = sq(q3);
|
||||
float q01 = q0 * q1;
|
||||
float q02 = q0 * q2;
|
||||
float q03 = q0 * q3;
|
||||
float q12 = q1 * q2;
|
||||
float q13 = q1 * q3;
|
||||
float q23 = q2 * q3;
|
||||
Tnb_local.x.x = q00 + q11 - q22 - q33;
|
||||
Tnb_local.y.y = q00 - q11 + q22 - q33;
|
||||
Tnb_local.z.z = q00 - q11 - q22 + q33;
|
||||
Tnb_local.y.x = 2*(q12 - q03);
|
||||
Tnb_local.z.x = 2*(q13 + q02);
|
||||
Tnb_local.x.y = 2*(q12 + q03);
|
||||
Tnb_local.z.y = 2*(q23 - q01);
|
||||
Tnb_local.x.z = 2*(q13 - q02);
|
||||
Tnb_local.y.z = 2*(q23 + q01);
|
||||
|
||||
// calculate transformation from NED to sensor axes
|
||||
Tns = Tbs*Tnb_local;
|
||||
|
||||
// calculate range from ground plain to centre of sensor fov assuming flat earth
|
||||
float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f);
|
||||
|
||||
// calculate relative velocity in sensor frame
|
||||
relVelSensor = Tns*velNED_local;
|
||||
|
||||
// divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
|
||||
losPred[0] = relVelSensor.y/range;
|
||||
losPred[1] = -relVelSensor.x/range;
|
||||
|
||||
//printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y);
|
||||
//printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y);
|
||||
//printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range);
|
||||
|
||||
// Calculate observation jacobians
|
||||
SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
|
||||
SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
SH_LOS[3] = 1/(pd - ptd);
|
||||
SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2;
|
||||
SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3;
|
||||
SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1;
|
||||
SH_LOS[7] = 1/sq(pd - ptd);
|
||||
SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1;
|
||||
SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0;
|
||||
SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3;
|
||||
SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0;
|
||||
SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2;
|
||||
|
||||
for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3);
|
||||
H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3));
|
||||
H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3));
|
||||
H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
|
||||
// Calculate Kalman gain
|
||||
SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3);
|
||||
SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3);
|
||||
SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3);
|
||||
SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3);
|
||||
SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3);
|
||||
SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3);
|
||||
SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]);
|
||||
SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]);
|
||||
SKK_LOS[14] = SH_LOS[0];
|
||||
|
||||
SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]));
|
||||
Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
varInnovOptFlow[0] = 1.0f/SK_LOS[0];
|
||||
innovOptFlow[0] = losPred[0] - losData[0];
|
||||
|
||||
// reset the observation index to 0 (we start by fusing the X
|
||||
// measurement)
|
||||
obsIndex = 0;
|
||||
fuseOptFlowData = false;
|
||||
// calculate Kalman gains
|
||||
float SK_RNG[3];
|
||||
SK_RNG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
SK_RNG[1] = 1/(R_RNG + Popt[1][1]/sq(SK_RNG[0]));
|
||||
SK_RNG[2] = 1/SK_RNG[0];
|
||||
float K_RNG[2];
|
||||
if (!inhibitScaleState) {
|
||||
K_RNG[0] = Popt[0][1]*SK_RNG[1]*SK_RNG[2];
|
||||
} else {
|
||||
K_RNG[0] = 0.0f;
|
||||
}
|
||||
else if (obsIndex == 1) // we are now fusing the Y measurement
|
||||
{
|
||||
// Calculate observation jacobians
|
||||
for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1);
|
||||
H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3));
|
||||
H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3));
|
||||
H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
|
||||
// Calculate Kalman gains
|
||||
SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]));
|
||||
Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
varInnovOptFlow[1] = 1.0f/SK_LOS[1];
|
||||
innovOptFlow[1] = losPred[1] - losData[1];
|
||||
if (!inhibitGndState) {
|
||||
K_RNG[1] = Popt[1][1]*SK_RNG[1]*SK_RNG[2];
|
||||
} else {
|
||||
K_RNG[1] = 0.0f;
|
||||
}
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > 3Sigma
|
||||
if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f)
|
||||
// Calculate the innovation variance for data logging
|
||||
varInnovRng = 1.0f/SK_RNG[1];
|
||||
|
||||
// constrain terrain height to be below the vehicle
|
||||
flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
|
||||
|
||||
// estimate range to centre of image
|
||||
range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2];
|
||||
|
||||
// Calculate the measurement innovation
|
||||
innovRng = range - rngMea;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if out of bounds
|
||||
if (auxRngTestRatio < 1.0f)
|
||||
{
|
||||
// correct the state vector
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
|
||||
}
|
||||
// normalise the quaternion states
|
||||
float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
|
||||
if (quatMag > 1e-12f)
|
||||
{
|
||||
for (uint8_t j= 0; j<=3; j++)
|
||||
{
|
||||
float quatMagInv = 1.0f/quatMag;
|
||||
states[j] = states[j] * quatMagInv;
|
||||
}
|
||||
}
|
||||
// correct the covariance P = (I - K*H)*P
|
||||
// take advantage of the empty columns in KH to reduce the
|
||||
// number of operations
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j <= 6; j++)
|
||||
{
|
||||
KH[i][j] = Kfusion[i] * H_LOS[j];
|
||||
}
|
||||
for (uint8_t j = 7; j <= 8; j++)
|
||||
{
|
||||
KH[i][j] = 0.0f;
|
||||
}
|
||||
KH[i][9] = Kfusion[i] * H_LOS[9];
|
||||
for (uint8_t j = 10; j <= 21; j++)
|
||||
{
|
||||
KH[i][j] = 0.0f;
|
||||
}
|
||||
KH[i][22] = Kfusion[i] * H_LOS[22];
|
||||
}
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
KHP[i][j] = 0.0f;
|
||||
for (uint8_t k = 0; k <= 6; k++)
|
||||
{
|
||||
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||||
}
|
||||
KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
|
||||
KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
P[i][j] = P[i][j] - KHP[i][j];
|
||||
// correct the state
|
||||
for (uint8_t i = 0; i < 2 ; i++) {
|
||||
flowStates[i] -= K_RNG[i] * innovRng;
|
||||
}
|
||||
// constrain the states
|
||||
|
||||
// constrain focal length to 0.1 to 10 mm
|
||||
flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
|
||||
// constrain altitude
|
||||
flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
|
||||
|
||||
// correct the covariance matrix
|
||||
float nextPopt[2][2];
|
||||
nextPopt[0][0] = Popt[0][0] - (Popt[0][1]*Popt[1][0]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
|
||||
nextPopt[0][1] = Popt[0][1] - (Popt[0][1]*Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
|
||||
nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
|
||||
nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
|
||||
// prevent the state variances from becoming negative and maintain symmetry
|
||||
Popt[0][0] = maxf(nextPopt[0][0],0.0f);
|
||||
Popt[1][1] = maxf(nextPopt[1][1],0.0f);
|
||||
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
|
||||
Popt[1][0] = Popt[0][1];
|
||||
}
|
||||
}
|
||||
obsIndex = obsIndex + 1;
|
||||
ForceSymmetry();
|
||||
ConstrainVariances();
|
||||
}
|
||||
|
||||
void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
|
||||
|
|
@ -2126,6 +2304,24 @@ float AttPosEKF::sq(float valIn)
|
|||
return valIn*valIn;
|
||||
}
|
||||
|
||||
float AttPosEKF::maxf(float valIn1, float valIn2)
|
||||
{
|
||||
if (valIn1 >= valIn2) {
|
||||
return valIn1;
|
||||
} else {
|
||||
return valIn2;
|
||||
}
|
||||
}
|
||||
|
||||
float AttPosEKF::min(float valIn1, float valIn2)
|
||||
{
|
||||
if (valIn1 <= valIn2) {
|
||||
return valIn1;
|
||||
} else {
|
||||
return valIn2;
|
||||
}
|
||||
}
|
||||
|
||||
// Store states in a history array along with time stamp
|
||||
void AttPosEKF::StoreStates(uint64_t timestamp_ms)
|
||||
{
|
||||
|
|
@ -2322,9 +2518,9 @@ void AttPosEKF::OnGroundCheck()
|
|||
}
|
||||
// don't update terrain offset state if on ground
|
||||
if (onGround) {
|
||||
inhibitGndHgtState = true;
|
||||
inhibitGndState = true;
|
||||
} else {
|
||||
inhibitGndHgtState = false;
|
||||
inhibitGndState = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -3006,9 +3202,14 @@ void AttPosEKF::ZeroVariables()
|
|||
{
|
||||
|
||||
// Initialize on-init initialized variables
|
||||
|
||||
dtIMUfilt = ConstrainFloat(dtIMU, 0.001f, 0.02f);
|
||||
dtVelPosFilt = ConstrainFloat(dtVelPos, 0.04f, 0.5f);
|
||||
dtGpsFilt = 1.0f / 5.0f;
|
||||
dtHgtFilt = 1.0f / 100.0f;
|
||||
storeIndex = 0;
|
||||
|
||||
lastVelPosFusion = millis();
|
||||
|
||||
// Do the data structure init
|
||||
for (unsigned i = 0; i < n_states; i++) {
|
||||
for (unsigned j = 0; j < n_states; j++) {
|
||||
|
|
@ -3028,6 +3229,13 @@ void AttPosEKF::ZeroVariables()
|
|||
dVelIMU.zero();
|
||||
lastGyroOffset.zero();
|
||||
|
||||
windSpdFiltNorth = 0.0f;
|
||||
windSpdFiltEast = 0.0f;
|
||||
// setting the altitude to zero will give us a higher
|
||||
// gain to adjust faster in the first step
|
||||
windSpdFiltAltitude = 0.0f;
|
||||
windSpdFiltClimb = 0.0f;
|
||||
|
||||
for (unsigned i = 0; i < data_buffer_size; i++) {
|
||||
|
||||
for (unsigned j = 0; j < n_states; j++) {
|
||||
|
|
|
|||
|
|
@ -80,6 +80,14 @@ public:
|
|||
airspeedMeasurementSigma = 1.4f;
|
||||
gyroProcessNoise = 1.4544411e-2f;
|
||||
accelProcessNoise = 0.5f;
|
||||
|
||||
gndHgtSigma = 0.1f; // terrain gradient 1-sigma
|
||||
R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2
|
||||
flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
|
||||
auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
|
||||
rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check
|
||||
minFlowRng = 0.01f; //minimum range between ground and flow sensor
|
||||
moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate
|
||||
}
|
||||
|
||||
struct mag_state_struct {
|
||||
|
|
@ -116,13 +124,16 @@ public:
|
|||
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
|
||||
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
|
||||
|
||||
// Times
|
||||
uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter
|
||||
|
||||
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
|
||||
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtRngTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time
|
||||
float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time
|
||||
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
|
|
@ -140,7 +151,16 @@ public:
|
|||
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||||
Vector3f dVelIMU;
|
||||
Vector3f dAngIMU;
|
||||
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
|
||||
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter
|
||||
float dtIMUfilt; // average time between IMU measurements (sec)
|
||||
float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter
|
||||
float dtVelPosFilt; // average time between position / velocity fusion steps
|
||||
float dtHgtFilt; // average time between height measurement updates
|
||||
float dtGpsFilt; // average time between gps measurement updates
|
||||
float windSpdFiltNorth; // average wind speed north component
|
||||
float windSpdFiltEast; // average wind speed east component
|
||||
float windSpdFiltAltitude; // the last altitude used to filter wind speed
|
||||
float windSpdFiltClimb; // filtered climb rate
|
||||
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
|
||||
float innovVelPos[6]; // innovation output
|
||||
float varInnovVelPos[6]; // innovation variance output
|
||||
|
|
@ -192,7 +212,8 @@ public:
|
|||
|
||||
bool inhibitWindStates; // true when wind states and covariances are to remain constant
|
||||
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
|
||||
bool inhibitGndHgtState; // true when the terrain ground height offset state and covariances are to remain constant
|
||||
bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant
|
||||
bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant
|
||||
|
||||
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool staticMode; ///< boolean true if no position feedback is fused
|
||||
|
|
@ -211,6 +232,30 @@ public:
|
|||
|
||||
unsigned storeIndex;
|
||||
|
||||
// Optical Flow error estimation
|
||||
float storedOmega[3][data_buffer_size]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
|
||||
|
||||
// Two state EKF used to estimate focal length scale factor and terrain position
|
||||
float Popt[2][2]; // state covariance matrix
|
||||
float flowStates[2]; // flow states [scale factor, terrain position]
|
||||
float prevPosN; // north position at last measurement
|
||||
float prevPosE; // east position at last measurement
|
||||
float auxFlowObsInnov[2]; // optical flow observation innovations from focal length scale factor estimator
|
||||
float auxFlowObsInnovVar[2]; // innovation variance for optical flow observations from focal length scale factor estimator
|
||||
float fScaleFactorVar; // optical flow sensor focal length scale factor variance
|
||||
Mat3f Tnb_flow; // Transformation matrix from nav to body at the time fo the optical flow measurement
|
||||
float R_LOS; // Optical flow observation noise variance (rad/sec)^2
|
||||
float auxFlowTestRatio[2]; // ratio of X and Y flow observation innovations to fault threshold
|
||||
float auxRngTestRatio; // ratio of range observation innovations to fault threshold
|
||||
float flowInnovGate; // number of standard deviations used for the innovation consistency check
|
||||
float auxFlowInnovGate; // number of standard deviations applied to the optical flow innovation consistency check
|
||||
float rngInnovGate; // number of standard deviations used for the innovation consistency check
|
||||
float minFlowRng; // minimum range over which to fuse optical flow measurements
|
||||
float moCompR_LOS; // scaler from sensor gyro rate to uncertainty in LOS rate
|
||||
|
||||
void updateDtGpsFilt(float dt);
|
||||
|
||||
void updateDtHgtFilt(float dt);
|
||||
|
||||
void UpdateStrapdownEquationsNED();
|
||||
|
||||
|
|
@ -226,6 +271,8 @@ void FuseRangeFinder();
|
|||
|
||||
void FuseOptFlow();
|
||||
|
||||
void GroundEKF();
|
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
|
@ -268,6 +315,10 @@ static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
|
|||
|
||||
static float sq(float valIn);
|
||||
|
||||
static float maxf(float valIn1, float valIn2);
|
||||
|
||||
static float min(float valIn1, float valIn2);
|
||||
|
||||
void OnGroundCheck();
|
||||
|
||||
void CovarianceInit();
|
||||
|
|
@ -300,6 +351,8 @@ void InitializeDynamic(float (&initvelNED)[3], float declination);
|
|||
|
||||
protected:
|
||||
|
||||
void updateDtVelPosFilt(float dt);
|
||||
|
||||
bool FilterHealthy();
|
||||
|
||||
bool GyroOffsetsDiverged();
|
||||
|
|
@ -314,3 +367,5 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
|
|||
|
||||
uint32_t millis();
|
||||
|
||||
uint64_t getMicros();
|
||||
|
||||
|
|
|
|||
|
|
@ -63,6 +63,7 @@
|
|||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
|
|
@ -124,6 +125,7 @@ private:
|
|||
int _params_sub; /**< notification of parameter updates */
|
||||
int _manual_sub; /**< notification of manual control updates */
|
||||
int _global_pos_sub; /**< global position subscription */
|
||||
int _vehicle_status_sub; /**< vehicle status subscription */
|
||||
|
||||
orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
|
||||
orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
|
||||
|
|
@ -139,6 +141,7 @@ private:
|
|||
struct actuator_controls_s _actuators; /**< actuator control inputs */
|
||||
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
|
||||
struct vehicle_global_position_s _global_pos; /**< global position */
|
||||
struct vehicle_status_s _vehicle_status; /**< vehicle status */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
|
||||
|
|
@ -275,6 +278,11 @@ private:
|
|||
*/
|
||||
void global_pos_poll();
|
||||
|
||||
/**
|
||||
* Check for vehicle status updates.
|
||||
*/
|
||||
void vehicle_status_poll();
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
*/
|
||||
|
|
@ -313,6 +321,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
|||
_params_sub(-1),
|
||||
_manual_sub(-1),
|
||||
_global_pos_sub(-1),
|
||||
_vehicle_status_sub(-1),
|
||||
|
||||
/* publications */
|
||||
_rate_sp_pub(-1),
|
||||
|
|
@ -338,6 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
|||
_actuators = {};
|
||||
_actuators_airframe = {};
|
||||
_global_pos = {};
|
||||
_vehicle_status = {};
|
||||
|
||||
|
||||
_parameter_handles.tconst = param_find("FW_ATT_TC");
|
||||
|
|
@ -560,6 +570,18 @@ FixedwingAttitudeControl::global_pos_poll()
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::vehicle_status_poll()
|
||||
{
|
||||
/* check if there is new status information */
|
||||
bool vehicle_status_updated;
|
||||
orb_check(_vehicle_status_sub, &vehicle_status_updated);
|
||||
|
||||
if (vehicle_status_updated) {
|
||||
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
|
||||
{
|
||||
|
|
@ -585,6 +607,7 @@ FixedwingAttitudeControl::task_main()
|
|||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
/* rate limit vehicle status updates to 5Hz */
|
||||
orb_set_interval(_vcontrol_mode_sub, 200);
|
||||
|
|
@ -599,6 +622,7 @@ FixedwingAttitudeControl::task_main()
|
|||
vehicle_accel_poll();
|
||||
vehicle_control_mode_poll();
|
||||
vehicle_manual_poll();
|
||||
vehicle_status_poll();
|
||||
|
||||
/* wakeup source(s) */
|
||||
struct pollfd fds[2];
|
||||
|
|
@ -667,6 +691,8 @@ FixedwingAttitudeControl::task_main()
|
|||
|
||||
global_pos_poll();
|
||||
|
||||
vehicle_status_poll();
|
||||
|
||||
/* lock integrator until control is started */
|
||||
bool lock_integrator;
|
||||
|
||||
|
|
@ -779,6 +805,13 @@ FixedwingAttitudeControl::task_main()
|
|||
}
|
||||
}
|
||||
|
||||
/* If the aircraft is on ground reset the integrators */
|
||||
if (_vehicle_status.condition_landed) {
|
||||
_roll_ctrl.reset_integrator();
|
||||
_pitch_ctrl.reset_integrator();
|
||||
_yaw_ctrl.reset_integrator();
|
||||
}
|
||||
|
||||
/* Prepare speed_body_u and speed_body_w */
|
||||
float speed_body_u = 0.0f;
|
||||
float speed_body_v = 0.0f;
|
||||
|
|
|
|||
|
|
@ -102,6 +102,8 @@ static int _control_task = -1; /**< task handle for sensor task */
|
|||
*/
|
||||
extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
|
||||
|
||||
using namespace launchdetection;
|
||||
|
||||
class FixedwingPositionControl
|
||||
{
|
||||
public:
|
||||
|
|
@ -171,8 +173,7 @@ private:
|
|||
bool land_onslope;
|
||||
|
||||
/* takeoff/launch states */
|
||||
bool launch_detected;
|
||||
bool usePreTakeoffThrust;
|
||||
LaunchDetectionResult launch_detection_state;
|
||||
|
||||
bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed)
|
||||
|
||||
|
|
@ -210,6 +211,7 @@ private:
|
|||
float max_climb_rate;
|
||||
float climbout_diff;
|
||||
float heightrate_p;
|
||||
float heightrate_ff;
|
||||
float speedrate_p;
|
||||
float throttle_damp;
|
||||
float integrator_gain;
|
||||
|
|
@ -255,6 +257,7 @@ private:
|
|||
param_t max_climb_rate;
|
||||
param_t climbout_diff;
|
||||
param_t heightrate_p;
|
||||
param_t heightrate_ff;
|
||||
param_t speedrate_p;
|
||||
param_t throttle_damp;
|
||||
param_t integrator_gain;
|
||||
|
|
@ -380,7 +383,8 @@ private:
|
|||
bool climbout_mode, float climbout_pitch_min_rad,
|
||||
float altitude,
|
||||
const math::Vector<3> &ground_speed,
|
||||
tecs_mode mode = TECS_MODE_NORMAL);
|
||||
tecs_mode mode = TECS_MODE_NORMAL,
|
||||
bool pitch_max_special = false);
|
||||
|
||||
};
|
||||
|
||||
|
|
@ -438,8 +442,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
|||
land_stayonground(false),
|
||||
land_motor_lim(false),
|
||||
land_onslope(false),
|
||||
launch_detected(false),
|
||||
usePreTakeoffThrust(false),
|
||||
launch_detection_state(LAUNCHDETECTION_RES_NONE),
|
||||
last_manual(false),
|
||||
landingslope(),
|
||||
flare_curve_alt_rel_last(0.0f),
|
||||
|
|
@ -494,6 +497,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
|||
_parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT");
|
||||
_parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP");
|
||||
_parameter_handles.heightrate_p = param_find("FW_T_HRATE_P");
|
||||
_parameter_handles.heightrate_ff = param_find("FW_T_HRATE_FF");
|
||||
_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
|
||||
|
||||
/* fetch initial parameter values */
|
||||
|
|
@ -563,6 +567,7 @@ FixedwingPositionControl::parameters_update()
|
|||
param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
|
||||
|
||||
param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
|
||||
param_get(_parameter_handles.heightrate_ff, &(_parameters.heightrate_ff));
|
||||
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
|
||||
|
||||
param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
|
||||
|
|
@ -600,6 +605,7 @@ FixedwingPositionControl::parameters_update()
|
|||
_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
|
||||
_tecs.set_max_climb_rate(_parameters.max_climb_rate);
|
||||
_tecs.set_heightrate_p(_parameters.heightrate_p);
|
||||
_tecs.set_heightrate_ff(_parameters.heightrate_ff);
|
||||
_tecs.set_speedrate_p(_parameters.speedrate_p);
|
||||
|
||||
/* sanity check parameters */
|
||||
|
|
@ -1082,41 +1088,46 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|||
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
|
||||
|
||||
/* Perform launch detection */
|
||||
if(!launch_detected) { //do not do further checks once a launch was detected
|
||||
if (launchDetector.launchDetectionEnabled()) {
|
||||
static hrt_abstime last_sent = 0;
|
||||
if(hrt_absolute_time() - last_sent > 4e6) {
|
||||
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
|
||||
last_sent = hrt_absolute_time();
|
||||
}
|
||||
|
||||
/* Tell the attitude controller to stop integrating while we are waiting
|
||||
* for the launch */
|
||||
_att_sp.roll_reset_integral = true;
|
||||
_att_sp.pitch_reset_integral = true;
|
||||
_att_sp.yaw_reset_integral = true;
|
||||
|
||||
/* Detect launch */
|
||||
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
|
||||
if (launchDetector.getLaunchDetected()) {
|
||||
launch_detected = true;
|
||||
mavlink_log_info(_mavlink_fd, "#audio: Takeoff");
|
||||
}
|
||||
} else {
|
||||
/* no takeoff detection --> fly */
|
||||
launch_detected = true;
|
||||
warnx("launchdetection off");
|
||||
if (launchDetector.launchDetectionEnabled() &&
|
||||
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
|
||||
/* Inform user that launchdetection is running */
|
||||
static hrt_abstime last_sent = 0;
|
||||
if(hrt_absolute_time() - last_sent > 4e6) {
|
||||
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
|
||||
last_sent = hrt_absolute_time();
|
||||
}
|
||||
|
||||
/* Detect launch */
|
||||
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
|
||||
|
||||
/* update our copy of the laucn detection state */
|
||||
launch_detection_state = launchDetector.getLaunchDetected();
|
||||
} else {
|
||||
/* no takeoff detection --> fly */
|
||||
launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
|
||||
}
|
||||
|
||||
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
|
||||
_att_sp.roll_body = _l1_control.nav_roll();
|
||||
_att_sp.yaw_body = _l1_control.nav_bearing();
|
||||
/* Set control values depending on the detection state */
|
||||
if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
|
||||
/* Launch has been detected, hence we have to control the plane. */
|
||||
|
||||
if (launch_detected) {
|
||||
usePreTakeoffThrust = false;
|
||||
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
|
||||
_att_sp.roll_body = _l1_control.nav_roll();
|
||||
_att_sp.yaw_body = _l1_control.nav_bearing();
|
||||
|
||||
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
|
||||
/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
|
||||
* full throttle, otherwise we use the preTakeOff Throttle */
|
||||
float takeoff_throttle = launch_detection_state !=
|
||||
LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
|
||||
launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
|
||||
|
||||
/* select maximum pitch: the launchdetector may impose another limit for the pitch
|
||||
* depending on the state of the launch */
|
||||
float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
|
||||
float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
|
||||
|
||||
/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
|
||||
* meters */
|
||||
if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
|
||||
|
||||
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
|
||||
|
|
@ -1124,18 +1135,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|||
calculate_target_airspeed(1.3f * _parameters.airspeed_min),
|
||||
eas2tas,
|
||||
math::radians(_parameters.pitch_limit_min),
|
||||
math::radians(_parameters.pitch_limit_max),
|
||||
_parameters.throttle_min, _parameters.throttle_max,
|
||||
takeoff_pitch_max_rad,
|
||||
_parameters.throttle_min, takeoff_throttle,
|
||||
_parameters.throttle_cruise,
|
||||
true,
|
||||
math::max(math::radians(pos_sp_triplet.current.pitch_min),
|
||||
math::radians(10.0f)),
|
||||
_global_pos.alt,
|
||||
ground_speed,
|
||||
TECS_MODE_TAKEOFF);
|
||||
TECS_MODE_TAKEOFF,
|
||||
takeoff_pitch_max_deg != _parameters.pitch_limit_max);
|
||||
|
||||
/* 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));
|
||||
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
|
||||
math::radians(15.0f));
|
||||
|
||||
} else {
|
||||
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
|
||||
|
|
@ -1144,17 +1157,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|||
math::radians(_parameters.pitch_limit_min),
|
||||
math::radians(_parameters.pitch_limit_max),
|
||||
_parameters.throttle_min,
|
||||
_parameters.throttle_max,
|
||||
takeoff_throttle,
|
||||
_parameters.throttle_cruise,
|
||||
false,
|
||||
math::radians(_parameters.pitch_limit_min),
|
||||
_global_pos.alt,
|
||||
ground_speed);
|
||||
}
|
||||
|
||||
} else {
|
||||
usePreTakeoffThrust = true;
|
||||
/* Tell the attitude controller to stop integrating while we are waiting
|
||||
* for the launch */
|
||||
_att_sp.roll_reset_integral = true;
|
||||
_att_sp.pitch_reset_integral = true;
|
||||
_att_sp.yaw_reset_integral = true;
|
||||
|
||||
/* Set default roll and pitch setpoints during detection phase */
|
||||
_att_sp.roll_body = 0.0f;
|
||||
_att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
|
||||
math::radians(10.0f));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* reset landing state */
|
||||
|
|
@ -1188,13 +1210,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|||
}
|
||||
}
|
||||
|
||||
if (usePreTakeoffThrust) {
|
||||
/* Copy thrust and pitch values from tecs
|
||||
* making sure again that the correct thrust is used,
|
||||
* without depending on library calls for safety reasons */
|
||||
if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
|
||||
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
|
||||
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
|
||||
}
|
||||
else {
|
||||
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max);
|
||||
}
|
||||
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
|
||||
/* During a takeoff waypoint while waiting for launch the pitch sp is set
|
||||
* already (not by tecs) */
|
||||
if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
|
||||
launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
|
||||
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_position_enabled) {
|
||||
last_manual = false;
|
||||
|
|
@ -1348,8 +1379,7 @@ FixedwingPositionControl::task_main()
|
|||
|
||||
void FixedwingPositionControl::reset_takeoff_state()
|
||||
{
|
||||
launch_detected = false;
|
||||
usePreTakeoffThrust = false;
|
||||
launch_detection_state = LAUNCHDETECTION_RES_NONE;
|
||||
launchDetector.reset();
|
||||
}
|
||||
|
||||
|
|
@ -1368,7 +1398,7 @@ 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,
|
||||
tecs_mode mode)
|
||||
tecs_mode mode, bool pitch_max_special)
|
||||
{
|
||||
if (_mTecs.getEnabled()) {
|
||||
/* Using mtecs library: prepare arguments for mtecs call */
|
||||
|
|
@ -1383,6 +1413,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
|
|||
} else {
|
||||
limitOverride.disablePitchMinOverride();
|
||||
}
|
||||
|
||||
if (pitch_max_special) {
|
||||
/* Use the maximum pitch from the argument */
|
||||
limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad);
|
||||
} else {
|
||||
/* use pitch max set by MT param */
|
||||
limitOverride.disablePitchMaxOverride();
|
||||
}
|
||||
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
|
||||
limitOverride);
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -131,8 +131,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
|
|||
/**
|
||||
* Throttle limit max
|
||||
*
|
||||
* This is the maximum throttle % that can be used by the controller.
|
||||
* For overpowered aircraft, this should be reduced to a value that
|
||||
* This is the maximum throttle % that can be used by the controller.
|
||||
* For overpowered aircraft, this should be reduced to a value that
|
||||
* provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX.
|
||||
*
|
||||
* @group L1 Control
|
||||
|
|
@ -142,10 +142,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
|
|||
/**
|
||||
* Throttle limit min
|
||||
*
|
||||
* This is the minimum throttle % that can be used by the controller.
|
||||
* For electric aircraft this will normally be set to zero, but can be set
|
||||
* to a small non-zero value if a folding prop is fitted to prevent the
|
||||
* prop from folding and unfolding repeatedly in-flight or to provide
|
||||
* This is the minimum throttle % that can be used by the controller.
|
||||
* For electric aircraft this will normally be set to zero, but can be set
|
||||
* to a small non-zero value if a folding prop is fitted to prevent the
|
||||
* prop from folding and unfolding repeatedly in-flight or to provide
|
||||
* some aerodynamic drag from a turning prop to improve the descent rate.
|
||||
*
|
||||
* For aircraft with internal combustion engine this parameter should be set
|
||||
|
|
@ -158,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
|
|||
/**
|
||||
* Throttle limit value before flare
|
||||
*
|
||||
* This throttle value will be set as throttle limit at FW_LND_TLALT,
|
||||
* This throttle value will be set as throttle limit at FW_LND_TLALT,
|
||||
* before arcraft will flare.
|
||||
*
|
||||
* @group L1 Control
|
||||
|
|
@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
|
|||
/**
|
||||
* Maximum climb rate
|
||||
*
|
||||
* This is the best climb rate that the aircraft can achieve with
|
||||
* the throttle set to THR_MAX and the airspeed set to the
|
||||
* default value. For electric aircraft make sure this number can be
|
||||
* achieved towards the end of flight when the battery voltage has reduced.
|
||||
* The setting of this parameter can be checked by commanding a positive
|
||||
* altitude change of 100m in loiter, RTL or guided mode. If the throttle
|
||||
* required to climb is close to THR_MAX and the aircraft is maintaining
|
||||
* airspeed, then this parameter is set correctly. If the airspeed starts
|
||||
* to reduce, then the parameter is set to high, and if the throttle
|
||||
* demand required to climb and maintain speed is noticeably less than
|
||||
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
|
||||
* This is the best climb rate that the aircraft can achieve with
|
||||
* the throttle set to THR_MAX and the airspeed set to the
|
||||
* default value. For electric aircraft make sure this number can be
|
||||
* achieved towards the end of flight when the battery voltage has reduced.
|
||||
* The setting of this parameter can be checked by commanding a positive
|
||||
* altitude change of 100m in loiter, RTL or guided mode. If the throttle
|
||||
* required to climb is close to THR_MAX and the aircraft is maintaining
|
||||
* airspeed, then this parameter is set correctly. If the airspeed starts
|
||||
* to reduce, then the parameter is set to high, and if the throttle
|
||||
* demand required to climb and maintain speed is noticeably less than
|
||||
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
|
||||
* FW_THR_MAX reduced.
|
||||
*
|
||||
* @group L1 Control
|
||||
|
|
@ -200,8 +200,8 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
|
|||
/**
|
||||
* Minimum descent rate
|
||||
*
|
||||
* This is the sink rate of the aircraft with the throttle
|
||||
* set to THR_MIN and flown at the same airspeed as used
|
||||
* This is the sink rate of the aircraft with the throttle
|
||||
* set to THR_MIN and flown at the same airspeed as used
|
||||
* to measure FW_T_CLMB_MAX.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
|
|
@ -211,10 +211,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
|
|||
/**
|
||||
* Maximum descent rate
|
||||
*
|
||||
* This sets the maximum descent rate that the controller will use.
|
||||
* If this value is too large, the aircraft can over-speed on descent.
|
||||
* This should be set to a value that can be achieved without
|
||||
* exceeding the lower pitch angle limit and without over-speeding
|
||||
* This sets the maximum descent rate that the controller will use.
|
||||
* If this value is too large, the aircraft can over-speed on descent.
|
||||
* This should be set to a value that can be achieved without
|
||||
* exceeding the lower pitch angle limit and without over-speeding
|
||||
* the aircraft.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
|
|
@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
|
|||
/**
|
||||
* TECS time constant
|
||||
*
|
||||
* This is the time constant of the TECS control algorithm (in seconds).
|
||||
* This is the time constant of the TECS control algorithm (in seconds).
|
||||
* Smaller values make it faster to respond, larger values make it slower
|
||||
* to respond.
|
||||
*
|
||||
|
|
@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
|
|||
/**
|
||||
* TECS Throttle time constant
|
||||
*
|
||||
* This is the time constant of the TECS throttle control algorithm (in seconds).
|
||||
* This is the time constant of the TECS throttle control algorithm (in seconds).
|
||||
* Smaller values make it faster to respond, larger values make it slower
|
||||
* to respond.
|
||||
*
|
||||
|
|
@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
|
|||
/**
|
||||
* Throttle damping factor
|
||||
*
|
||||
* This is the damping gain for the throttle demand loop.
|
||||
* This is the damping gain for the throttle demand loop.
|
||||
* Increase to add damping to correct for oscillations in speed and height.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
|
|
@ -256,9 +256,9 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
|
|||
/**
|
||||
* Integrator gain
|
||||
*
|
||||
* This is the integrator gain on the control loop.
|
||||
* Increasing this gain increases the speed at which speed
|
||||
* and height offsets are trimmed out, but reduces damping and
|
||||
* This is the integrator gain on the control loop.
|
||||
* Increasing this gain increases the speed at which speed
|
||||
* and height offsets are trimmed out, but reduces damping and
|
||||
* increases overshoot.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
|
|
@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
|
|||
* Maximum vertical acceleration
|
||||
*
|
||||
* This is the maximum vertical acceleration (in metres/second square)
|
||||
* either up or down that the controller will use to correct speed
|
||||
* or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
|
||||
* allows for reasonably aggressive pitch changes if required to recover
|
||||
* either up or down that the controller will use to correct speed
|
||||
* or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
|
||||
* allows for reasonably aggressive pitch changes if required to recover
|
||||
* from under-speed conditions.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
|
|
@ -281,10 +281,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
|
|||
/**
|
||||
* Complementary filter "omega" parameter for height
|
||||
*
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse vertical acceleration and barometric height to obtain
|
||||
* an estimate of height rate and height. Increasing this frequency weights
|
||||
* the solution more towards use of the barometer, whilst reducing it weights
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse vertical acceleration and barometric height to obtain
|
||||
* an estimate of height rate and height. Increasing this frequency weights
|
||||
* the solution more towards use of the barometer, whilst reducing it weights
|
||||
* the solution more towards use of the accelerometer data.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
|
|
@ -294,10 +294,10 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
|
|||
/**
|
||||
* Complementary filter "omega" parameter for speed
|
||||
*
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse longitudinal acceleration and airspeed to obtain an
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse longitudinal acceleration and airspeed to obtain an
|
||||
* improved airspeed estimate. Increasing this frequency weights the solution
|
||||
* more towards use of the arispeed sensor, whilst reducing it weights the
|
||||
* more towards use of the arispeed sensor, whilst reducing it weights the
|
||||
* solution more towards use of the accelerometer data.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
|
|
@ -307,13 +307,13 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
|
|||
/**
|
||||
* Roll -> Throttle feedforward
|
||||
*
|
||||
* Increasing this gain turn increases the amount of throttle that will
|
||||
* be used to compensate for the additional drag created by turning.
|
||||
* Ideally this should be set to approximately 10 x the extra sink rate
|
||||
* in m/s created by a 45 degree bank turn. Increase this gain if
|
||||
* the aircraft initially loses energy in turns and reduce if the
|
||||
* aircraft initially gains energy in turns. Efficient high aspect-ratio
|
||||
* aircraft (eg powered sailplanes) can use a lower value, whereas
|
||||
* Increasing this gain turn increases the amount of throttle that will
|
||||
* be used to compensate for the additional drag created by turning.
|
||||
* Ideally this should be set to approximately 10 x the extra sink rate
|
||||
* in m/s created by a 45 degree bank turn. Increase this gain if
|
||||
* the aircraft initially loses energy in turns and reduce if the
|
||||
* aircraft initially gains energy in turns. Efficient high aspect-ratio
|
||||
* aircraft (eg powered sailplanes) can use a lower value, whereas
|
||||
* inefficient low aspect-ratio models (eg delta wings) can use a higher value.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
|
|
@ -323,15 +323,15 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
|
|||
/**
|
||||
* Speed <--> Altitude priority
|
||||
*
|
||||
* This parameter adjusts the amount of weighting that the pitch control
|
||||
* applies to speed vs height errors. Setting it to 0.0 will cause the
|
||||
* pitch control to control height and ignore speed errors. This will
|
||||
* normally improve height accuracy but give larger airspeed errors.
|
||||
* Setting it to 2.0 will cause the pitch control loop to control speed
|
||||
* and ignore height errors. This will normally reduce airspeed errors,
|
||||
* but give larger height errors. The default value of 1.0 allows the pitch
|
||||
* control to simultaneously control height and speed.
|
||||
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
|
||||
* This parameter adjusts the amount of weighting that the pitch control
|
||||
* applies to speed vs height errors. Setting it to 0.0 will cause the
|
||||
* pitch control to control height and ignore speed errors. This will
|
||||
* normally improve height accuracy but give larger airspeed errors.
|
||||
* Setting it to 2.0 will cause the pitch control loop to control speed
|
||||
* and ignore height errors. This will normally reduce airspeed errors,
|
||||
* but give larger height errors. The default value of 1.0 allows the pitch
|
||||
* control to simultaneously control height and speed.
|
||||
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
|
||||
* adjust its pitch angle to maintain airspeed, ignoring changes in height).
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
|
|
@ -341,9 +341,9 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
|
|||
/**
|
||||
* Pitch damping factor
|
||||
*
|
||||
* This is the damping gain for the pitch demand loop. Increase to add
|
||||
* damping to correct for oscillations in height. The default value of 0.0
|
||||
* will work well provided the pitch to servo controller has been tuned
|
||||
* This is the damping gain for the pitch demand loop. Increase to add
|
||||
* damping to correct for oscillations in height. The default value of 0.0
|
||||
* will work well provided the pitch to servo controller has been tuned
|
||||
* properly.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
|
|
@ -357,6 +357,13 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
|
||||
|
||||
/**
|
||||
* Height rate FF factor
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Speed rate P factor
|
||||
*
|
||||
|
|
|
|||
|
|
@ -31,38 +31,38 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <crc32.h>
|
||||
/// @file mavlink_ftp.cpp
|
||||
/// @author px4dev, Don Gagne <don@thegagnes.com>
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "mavlink_ftp.h"
|
||||
#include "mavlink_tests/mavlink_ftp_test.h"
|
||||
|
||||
// Uncomment the line below to get better debug output. Never commit with this left on.
|
||||
//#define MAVLINK_FTP_DEBUG
|
||||
|
||||
MavlinkFTP *MavlinkFTP::_server;
|
||||
|
||||
MavlinkFTP *
|
||||
MavlinkFTP::getServer()
|
||||
MavlinkFTP::get_server(void)
|
||||
{
|
||||
// XXX this really cries out for some locking...
|
||||
if (_server == nullptr) {
|
||||
_server = new MavlinkFTP;
|
||||
}
|
||||
return _server;
|
||||
static MavlinkFTP server;
|
||||
return &server;
|
||||
}
|
||||
|
||||
MavlinkFTP::MavlinkFTP() :
|
||||
_session_fds{},
|
||||
_workBufs{},
|
||||
_workFree{},
|
||||
_lock{}
|
||||
_request_bufs{},
|
||||
_request_queue{},
|
||||
_request_queue_sem{},
|
||||
_utRcvMsgFunc{},
|
||||
_ftp_test{}
|
||||
{
|
||||
// initialise the request freelist
|
||||
dq_init(&_workFree);
|
||||
sem_init(&_lock, 0, 1);
|
||||
dq_init(&_request_queue);
|
||||
sem_init(&_request_queue_sem, 0, 1);
|
||||
|
||||
// initialize session list
|
||||
for (size_t i=0; i<kMaxSession; i++) {
|
||||
|
|
@ -71,167 +71,240 @@ MavlinkFTP::MavlinkFTP() :
|
|||
|
||||
// drop work entries onto the free list
|
||||
for (unsigned i = 0; i < kRequestQueueSize; i++) {
|
||||
_qFree(&_workBufs[i]);
|
||||
_return_request(&_request_bufs[i]);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
void
|
||||
MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test)
|
||||
{
|
||||
_utRcvMsgFunc = rcvMsgFunc;
|
||||
_ftp_test = ftp_test;
|
||||
}
|
||||
#endif
|
||||
|
||||
void
|
||||
MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
|
||||
{
|
||||
// get a free request
|
||||
auto req = _dqFree();
|
||||
struct Request* req = _get_request();
|
||||
|
||||
// if we couldn't get a request slot, just drop it
|
||||
if (req != nullptr) {
|
||||
if (req == nullptr) {
|
||||
warnx("Dropping FTP request: queue full\n");
|
||||
return;
|
||||
}
|
||||
|
||||
// decode the request
|
||||
if (req->decode(mavlink, msg)) {
|
||||
|
||||
// and queue it for the worker
|
||||
work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
|
||||
} else {
|
||||
_qFree(req);
|
||||
if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
|
||||
mavlink_msg_file_transfer_protocol_decode(msg, &req->message);
|
||||
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
if (!_utRcvMsgFunc) {
|
||||
warnx("Incorrectly written unit test\n");
|
||||
return;
|
||||
}
|
||||
// We use fake ids when unit testing
|
||||
req->serverSystemId = MavlinkFtpTest::serverSystemId;
|
||||
req->serverComponentId = MavlinkFtpTest::serverComponentId;
|
||||
req->serverChannel = MavlinkFtpTest::serverChannel;
|
||||
#else
|
||||
// Not unit testing, use the real thing
|
||||
req->serverSystemId = mavlink->get_system_id();
|
||||
req->serverComponentId = mavlink->get_component_id();
|
||||
req->serverChannel = mavlink->get_channel();
|
||||
#endif
|
||||
|
||||
// This is the system id we want to target when sending
|
||||
req->targetSystemId = msg->sysid;
|
||||
|
||||
if (req->message.target_system == req->serverSystemId) {
|
||||
req->mavlink = mavlink;
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
// We are running in Unit Test mode. Don't queue, just call _worket directly.
|
||||
_process_request(req);
|
||||
#else
|
||||
// We are running in normal mode. Queue the request to the worker
|
||||
work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
_return_request(req);
|
||||
}
|
||||
|
||||
/// @brief Queued static work queue routine to handle mavlink messages
|
||||
void
|
||||
MavlinkFTP::_workerTrampoline(void *arg)
|
||||
MavlinkFTP::_worker_trampoline(void *arg)
|
||||
{
|
||||
auto req = reinterpret_cast<Request *>(arg);
|
||||
auto server = MavlinkFTP::getServer();
|
||||
Request* req = reinterpret_cast<Request *>(arg);
|
||||
MavlinkFTP* server = MavlinkFTP::get_server();
|
||||
|
||||
// call the server worker with the work item
|
||||
server->_worker(req);
|
||||
server->_process_request(req);
|
||||
}
|
||||
|
||||
/// @brief Processes an FTP message
|
||||
void
|
||||
MavlinkFTP::_worker(Request *req)
|
||||
MavlinkFTP::_process_request(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
|
||||
|
||||
ErrorCode errorCode = kErrNone;
|
||||
uint32_t messageCRC;
|
||||
|
||||
// basic sanity checks; must validate length before use
|
||||
if (hdr->size > kMaxDataLength) {
|
||||
errorCode = kErrNoRequest;
|
||||
if (payload->size > kMaxDataLength) {
|
||||
errorCode = kErrInvalidDataSize;
|
||||
goto out;
|
||||
}
|
||||
|
||||
// check request CRC to make sure this is one of ours
|
||||
messageCRC = hdr->crc32;
|
||||
hdr->crc32 = 0;
|
||||
hdr->padding[0] = 0;
|
||||
hdr->padding[1] = 0;
|
||||
hdr->padding[2] = 0;
|
||||
if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
|
||||
errorCode = kErrNoRequest;
|
||||
goto out;
|
||||
warnx("ftp: bad crc");
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
|
||||
printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset);
|
||||
#endif
|
||||
|
||||
switch (hdr->opcode) {
|
||||
switch (payload->opcode) {
|
||||
case kCmdNone:
|
||||
break;
|
||||
|
||||
case kCmdTerminate:
|
||||
errorCode = _workTerminate(req);
|
||||
case kCmdTerminateSession:
|
||||
errorCode = _workTerminate(payload);
|
||||
break;
|
||||
|
||||
case kCmdReset:
|
||||
errorCode = _workReset();
|
||||
case kCmdResetSessions:
|
||||
errorCode = _workReset(payload);
|
||||
break;
|
||||
|
||||
case kCmdList:
|
||||
errorCode = _workList(req);
|
||||
case kCmdListDirectory:
|
||||
errorCode = _workList(payload);
|
||||
break;
|
||||
|
||||
case kCmdOpen:
|
||||
errorCode = _workOpen(req, false);
|
||||
case kCmdOpenFile:
|
||||
errorCode = _workOpen(payload, false);
|
||||
break;
|
||||
|
||||
case kCmdCreate:
|
||||
errorCode = _workOpen(req, true);
|
||||
case kCmdCreateFile:
|
||||
errorCode = _workOpen(payload, true);
|
||||
break;
|
||||
|
||||
case kCmdRead:
|
||||
errorCode = _workRead(req);
|
||||
case kCmdReadFile:
|
||||
errorCode = _workRead(payload);
|
||||
break;
|
||||
|
||||
case kCmdWrite:
|
||||
errorCode = _workWrite(req);
|
||||
case kCmdWriteFile:
|
||||
errorCode = _workWrite(payload);
|
||||
break;
|
||||
|
||||
case kCmdRemove:
|
||||
errorCode = _workRemove(req);
|
||||
case kCmdRemoveFile:
|
||||
errorCode = _workRemoveFile(payload);
|
||||
break;
|
||||
|
||||
case kCmdCreateDirectory:
|
||||
errorCode = _workCreateDirectory(payload);
|
||||
break;
|
||||
|
||||
case kCmdRemoveDirectory:
|
||||
errorCode = _workRemoveDirectory(payload);
|
||||
break;
|
||||
|
||||
default:
|
||||
errorCode = kErrNoRequest;
|
||||
errorCode = kErrUnknownCommand;
|
||||
break;
|
||||
}
|
||||
|
||||
out:
|
||||
// handle success vs. error
|
||||
if (errorCode == kErrNone) {
|
||||
hdr->opcode = kRspAck;
|
||||
payload->req_opcode = payload->opcode;
|
||||
payload->opcode = kRspAck;
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("FTP: ack\n");
|
||||
#endif
|
||||
} else {
|
||||
warnx("FTP: nak %u", errorCode);
|
||||
hdr->opcode = kRspNak;
|
||||
hdr->size = 1;
|
||||
hdr->data[0] = errorCode;
|
||||
payload->req_opcode = payload->opcode;
|
||||
payload->opcode = kRspNak;
|
||||
payload->size = 1;
|
||||
payload->data[0] = errorCode;
|
||||
if (errorCode == kErrFailErrno) {
|
||||
payload->size = 2;
|
||||
payload->data[1] = errno;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// respond to the request
|
||||
_reply(req);
|
||||
|
||||
// free the request buffer back to the freelist
|
||||
_qFree(req);
|
||||
_return_request(req);
|
||||
}
|
||||
|
||||
/// @brief Sends the specified FTP reponse message out through mavlink
|
||||
void
|
||||
MavlinkFTP::_reply(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
|
||||
|
||||
hdr->seqNumber = req->header()->seqNumber + 1;
|
||||
payload->seqNumber = payload->seqNumber + 1;
|
||||
|
||||
// message is assumed to be already constructed in the request buffer, so generate the CRC
|
||||
hdr->crc32 = 0;
|
||||
hdr->padding[0] = 0;
|
||||
hdr->padding[1] = 0;
|
||||
hdr->padding[2] = 0;
|
||||
hdr->crc32 = crc32(req->rawData(), req->dataSize());
|
||||
mavlink_message_t msg;
|
||||
msg.checksum = 0;
|
||||
#ifndef MAVLINK_FTP_UNIT_TEST
|
||||
uint16_t len =
|
||||
#endif
|
||||
mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId, // Sender system id
|
||||
req->serverComponentId, // Sender component id
|
||||
req->serverChannel, // Channel to send on
|
||||
&msg, // Message to pack payload into
|
||||
0, // Target network
|
||||
req->targetSystemId, // Target system id
|
||||
0, // Target component id
|
||||
(const uint8_t*)payload); // Payload to pack into message
|
||||
|
||||
bool success = true;
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
// Unit test hook is set, call that instead
|
||||
_utRcvMsgFunc(&msg, _ftp_test);
|
||||
#else
|
||||
Mavlink *mavlink = req->mavlink;
|
||||
|
||||
mavlink->lockMessageBufferMutex();
|
||||
success = mavlink->message_buffer_write(&msg, len);
|
||||
mavlink->unlockMessageBufferMutex();
|
||||
|
||||
#endif
|
||||
|
||||
// then pack and send the reply back to the request source
|
||||
req->reply();
|
||||
if (!success) {
|
||||
warnx("FTP TX ERR");
|
||||
}
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
else {
|
||||
warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d",
|
||||
req->serverSystemId,
|
||||
req->serverComponentId,
|
||||
req->serverChannel,
|
||||
msg.checksum);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/// @brief Responds to a List command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workList(Request *req)
|
||||
MavlinkFTP::_workList(PayloadHeader* payload)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
char dirPath[kMaxDataLength];
|
||||
strncpy(dirPath, req->dataAsCString(), kMaxDataLength);
|
||||
strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
DIR *dp = opendir(dirPath);
|
||||
|
||||
if (dp == nullptr) {
|
||||
warnx("FTP: can't open path '%s'", dirPath);
|
||||
return kErrNotDir;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("FTP: list %s offset %d", dirPath, hdr->offset);
|
||||
warnx("FTP: list %s offset %d", dirPath, payload->offset);
|
||||
#endif
|
||||
|
||||
ErrorCode errorCode = kErrNone;
|
||||
|
|
@ -239,19 +312,19 @@ MavlinkFTP::_workList(Request *req)
|
|||
unsigned offset = 0;
|
||||
|
||||
// move to the requested offset
|
||||
seekdir(dp, hdr->offset);
|
||||
seekdir(dp, payload->offset);
|
||||
|
||||
for (;;) {
|
||||
// read the directory entry
|
||||
if (readdir_r(dp, &entry, &result)) {
|
||||
warnx("FTP: list %s readdir_r failure\n", dirPath);
|
||||
errorCode = kErrIO;
|
||||
errorCode = kErrFailErrno;
|
||||
break;
|
||||
}
|
||||
|
||||
// no more entries?
|
||||
if (result == nullptr) {
|
||||
if (hdr->offset != 0 && offset == 0) {
|
||||
if (payload->offset != 0 && offset == 0) {
|
||||
// User is requesting subsequent dir entries but there were none. This means the user asked
|
||||
// to seek past EOF.
|
||||
errorCode = kErrEOF;
|
||||
|
|
@ -276,14 +349,22 @@ MavlinkFTP::_workList(Request *req)
|
|||
}
|
||||
break;
|
||||
case DTYPE_DIRECTORY:
|
||||
direntType = kDirentDir;
|
||||
if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
|
||||
// Don't bother sending these back
|
||||
direntType = kDirentSkip;
|
||||
} else {
|
||||
direntType = kDirentDir;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
direntType = kDirentUnknown;
|
||||
break;
|
||||
// We only send back file and diretory entries, skip everything else
|
||||
direntType = kDirentSkip;
|
||||
}
|
||||
|
||||
if (entry.d_type == DTYPE_FILE) {
|
||||
if (direntType == kDirentSkip) {
|
||||
// Skip send only dirent identifier
|
||||
buf[0] = '\0';
|
||||
} else if (direntType == kDirentFile) {
|
||||
// Files send filename and file length
|
||||
snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
|
||||
} else {
|
||||
|
|
@ -299,94 +380,95 @@ MavlinkFTP::_workList(Request *req)
|
|||
}
|
||||
|
||||
// Move the data into the buffer
|
||||
hdr->data[offset++] = direntType;
|
||||
strcpy((char *)&hdr->data[offset], buf);
|
||||
payload->data[offset++] = direntType;
|
||||
strcpy((char *)&payload->data[offset], buf);
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
|
||||
printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]);
|
||||
#endif
|
||||
offset += nameLen + 1;
|
||||
}
|
||||
|
||||
closedir(dp);
|
||||
hdr->size = offset;
|
||||
payload->size = offset;
|
||||
|
||||
return errorCode;
|
||||
}
|
||||
|
||||
/// @brief Responds to an Open command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workOpen(Request *req, bool create)
|
||||
MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
int session_index = _findUnusedSession();
|
||||
int session_index = _find_unused_session();
|
||||
if (session_index < 0) {
|
||||
return kErrNoSession;
|
||||
warnx("FTP: Open failed - out of sessions\n");
|
||||
return kErrNoSessionsAvailable;
|
||||
}
|
||||
|
||||
|
||||
char *filename = _data_as_cstring(payload);
|
||||
|
||||
uint32_t fileSize = 0;
|
||||
if (!create) {
|
||||
struct stat st;
|
||||
if (stat(req->dataAsCString(), &st) != 0) {
|
||||
return kErrNotFile;
|
||||
if (stat(filename, &st) != 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
fileSize = st.st_size;
|
||||
}
|
||||
|
||||
int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
|
||||
|
||||
int fd = ::open(req->dataAsCString(), oflag);
|
||||
int fd = ::open(filename, oflag);
|
||||
if (fd < 0) {
|
||||
return create ? kErrPerm : kErrNotFile;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
_session_fds[session_index] = fd;
|
||||
|
||||
hdr->session = session_index;
|
||||
payload->session = session_index;
|
||||
if (create) {
|
||||
hdr->size = 0;
|
||||
payload->size = 0;
|
||||
} else {
|
||||
hdr->size = sizeof(uint32_t);
|
||||
*((uint32_t*)hdr->data) = fileSize;
|
||||
payload->size = sizeof(uint32_t);
|
||||
*((uint32_t*)payload->data) = fileSize;
|
||||
}
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Read command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRead(Request *req)
|
||||
MavlinkFTP::_workRead(PayloadHeader* payload)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
int session_index = payload->session;
|
||||
|
||||
int session_index = hdr->session;
|
||||
|
||||
if (!_validSession(session_index)) {
|
||||
return kErrNoSession;
|
||||
if (!_valid_session(session_index)) {
|
||||
return kErrInvalidSession;
|
||||
}
|
||||
|
||||
// Seek to the specified position
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("seek %d", hdr->offset);
|
||||
warnx("seek %d", payload->offset);
|
||||
#endif
|
||||
if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
|
||||
if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
|
||||
// Unable to see to the specified location
|
||||
warnx("seek fail");
|
||||
return kErrEOF;
|
||||
}
|
||||
|
||||
int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
|
||||
int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength);
|
||||
if (bytes_read < 0) {
|
||||
// Negative return indicates error other than eof
|
||||
warnx("read fail %d", bytes_read);
|
||||
return kErrIO;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
hdr->size = bytes_read;
|
||||
payload->size = bytes_read;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Write command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workWrite(Request *req)
|
||||
MavlinkFTP::_workWrite(PayloadHeader* payload)
|
||||
{
|
||||
#if 0
|
||||
// NYI: Coming soon
|
||||
|
|
@ -409,35 +491,44 @@ MavlinkFTP::_workWrite(Request *req)
|
|||
hdr->size = result;
|
||||
return kErrNone;
|
||||
#else
|
||||
return kErrPerm;
|
||||
return kErrUnknownCommand;
|
||||
#endif
|
||||
}
|
||||
|
||||
/// @brief Responds to a RemoveFile command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRemove(Request *req)
|
||||
MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
|
||||
{
|
||||
//auto hdr = req->header();
|
||||
|
||||
// for now, send error reply
|
||||
return kErrPerm;
|
||||
char file[kMaxDataLength];
|
||||
strncpy(file, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
if (unlink(file) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Responds to a Terminate command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workTerminate(Request *req)
|
||||
MavlinkFTP::_workTerminate(PayloadHeader* payload)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
if (!_validSession(hdr->session)) {
|
||||
return kErrNoSession;
|
||||
if (!_valid_session(payload->session)) {
|
||||
return kErrInvalidSession;
|
||||
}
|
||||
|
||||
::close(_session_fds[hdr->session]);
|
||||
::close(_session_fds[payload->session]);
|
||||
_session_fds[payload->session] = -1;
|
||||
|
||||
payload->size = 0;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Reset command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workReset(void)
|
||||
MavlinkFTP::_workReset(PayloadHeader* payload)
|
||||
{
|
||||
for (size_t i=0; i<kMaxSession; i++) {
|
||||
if (_session_fds[i] != -1) {
|
||||
|
|
@ -446,11 +537,44 @@ MavlinkFTP::_workReset(void)
|
|||
}
|
||||
}
|
||||
|
||||
payload->size = 0;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a RemoveDirectory command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
|
||||
{
|
||||
char dir[kMaxDataLength];
|
||||
strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
if (rmdir(dir) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Responds to a CreateDirectory command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
|
||||
{
|
||||
char dir[kMaxDataLength];
|
||||
strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Returns true if the specified session is a valid open session
|
||||
bool
|
||||
MavlinkFTP::_validSession(unsigned index)
|
||||
MavlinkFTP::_valid_session(unsigned index)
|
||||
{
|
||||
if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
|
||||
return false;
|
||||
|
|
@ -458,8 +582,9 @@ MavlinkFTP::_validSession(unsigned index)
|
|||
return true;
|
||||
}
|
||||
|
||||
/// @brief Returns an unused session index
|
||||
int
|
||||
MavlinkFTP::_findUnusedSession(void)
|
||||
MavlinkFTP::_find_unused_session(void)
|
||||
{
|
||||
for (size_t i=0; i<kMaxSession; i++) {
|
||||
if (_session_fds[i] == -1) {
|
||||
|
|
@ -470,16 +595,53 @@ MavlinkFTP::_findUnusedSession(void)
|
|||
return -1;
|
||||
}
|
||||
|
||||
/// @brief Guarantees that the payload data is null terminated.
|
||||
/// @return Returns a pointer to the payload data as a char *
|
||||
char *
|
||||
MavlinkFTP::Request::dataAsCString()
|
||||
MavlinkFTP::_data_as_cstring(PayloadHeader* payload)
|
||||
{
|
||||
// guarantee nul termination
|
||||
if (header()->size < kMaxDataLength) {
|
||||
requestData()[header()->size] = '\0';
|
||||
if (payload->size < kMaxDataLength) {
|
||||
payload->data[payload->size] = '\0';
|
||||
} else {
|
||||
requestData()[kMaxDataLength - 1] = '\0';
|
||||
payload->data[kMaxDataLength - 1] = '\0';
|
||||
}
|
||||
|
||||
// and return data
|
||||
return (char *)&(header()->data[0]);
|
||||
return (char *)&(payload->data[0]);
|
||||
}
|
||||
|
||||
/// @brief Returns a unused Request entry. NULL if none available.
|
||||
MavlinkFTP::Request *
|
||||
MavlinkFTP::_get_request(void)
|
||||
{
|
||||
_lock_request_queue();
|
||||
Request* req = reinterpret_cast<Request *>(dq_remfirst(&_request_queue));
|
||||
_unlock_request_queue();
|
||||
return req;
|
||||
}
|
||||
|
||||
/// @brief Locks a semaphore to provide exclusive access to the request queue
|
||||
void
|
||||
MavlinkFTP::_lock_request_queue(void)
|
||||
{
|
||||
do {}
|
||||
while (sem_wait(&_request_queue_sem) != 0);
|
||||
}
|
||||
|
||||
/// @brief Unlocks the semaphore providing exclusive access to the request queue
|
||||
void
|
||||
MavlinkFTP::_unlock_request_queue(void)
|
||||
{
|
||||
sem_post(&_request_queue_sem);
|
||||
}
|
||||
|
||||
/// @brief Returns a no longer needed request to the queue
|
||||
void
|
||||
MavlinkFTP::_return_request(Request *req)
|
||||
{
|
||||
_lock_request_queue();
|
||||
dq_addlast(&req->work.dq, &_request_queue);
|
||||
_unlock_request_queue();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -33,17 +33,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* @file mavlink_ftp.h
|
||||
*
|
||||
* MAVLink remote file server.
|
||||
*
|
||||
* A limited number of requests (currently 2) may be outstanding at a time.
|
||||
* Additional messages will be discarded.
|
||||
*
|
||||
* Messages consist of a fixed header, followed by a data area.
|
||||
*
|
||||
*/
|
||||
/// @file mavlink_ftp.h
|
||||
/// @author px4dev, Don Gagne <don@thegagnes.com>
|
||||
|
||||
#include <dirent.h>
|
||||
#include <queue.h>
|
||||
|
|
@ -54,183 +45,128 @@
|
|||
#include "mavlink_messages.h"
|
||||
#include "mavlink_main.h"
|
||||
|
||||
class MavlinkFtpTest;
|
||||
|
||||
/// @brief MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message.
|
||||
/// A limited number of requests (kRequestQueueSize) may be outstanding at a time. Additional messages will be discarded.
|
||||
class MavlinkFTP
|
||||
{
|
||||
public:
|
||||
/// @brief Returns the one Mavlink FTP server in the system.
|
||||
static MavlinkFTP* get_server(void);
|
||||
|
||||
/// @brief Contructor is only public so unit test code can new objects.
|
||||
MavlinkFTP();
|
||||
|
||||
/// @brief Adds the specified message to the work queue.
|
||||
void handle_message(Mavlink* mavlink, mavlink_message_t *msg);
|
||||
|
||||
typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
|
||||
|
||||
/// @brief Sets up the server to run in unit test mode.
|
||||
/// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages.
|
||||
/// @param ftp_test MavlinkFtpTest object which the function is associated with
|
||||
void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test);
|
||||
|
||||
static MavlinkFTP *getServer();
|
||||
|
||||
// static interface
|
||||
void handle_message(Mavlink* mavlink,
|
||||
mavlink_message_t *msg);
|
||||
|
||||
private:
|
||||
|
||||
static const unsigned kRequestQueueSize = 2;
|
||||
|
||||
static MavlinkFTP *_server;
|
||||
|
||||
/// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the
|
||||
/// structure ourselves to 32 bit alignment which should get us what we want.
|
||||
struct RequestHeader
|
||||
/// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to
|
||||
/// 32 bit alignment to avoid usage of any pack pragmas.
|
||||
struct PayloadHeader
|
||||
{
|
||||
uint16_t seqNumber; ///< sequence number for message
|
||||
uint8_t session; ///< Session id for read and write commands
|
||||
uint8_t opcode; ///< Command opcode
|
||||
uint8_t size; ///< Size of data
|
||||
uint8_t padding[3];
|
||||
uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0
|
||||
uint32_t offset; ///< Offsets for List and Read commands
|
||||
uint8_t data[];
|
||||
uint16_t seqNumber; ///< sequence number for message
|
||||
uint8_t session; ///< Session id for read and write commands
|
||||
uint8_t opcode; ///< Command opcode
|
||||
uint8_t size; ///< Size of data
|
||||
uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message
|
||||
uint8_t padding[2]; ///< 32 bit aligment padding
|
||||
uint32_t offset; ///< Offsets for List and Read commands
|
||||
uint8_t data[]; ///< command data, varies by Opcode
|
||||
};
|
||||
|
||||
|
||||
/// @brief Command opcodes
|
||||
enum Opcode : uint8_t
|
||||
{
|
||||
kCmdNone, // ignored, always acked
|
||||
kCmdTerminate, // releases sessionID, closes file
|
||||
kCmdReset, // terminates all sessions
|
||||
kCmdList, // list files in <path> from <offset>
|
||||
kCmdOpen, // opens <path> for reading, returns <session>
|
||||
kCmdRead, // reads <size> bytes from <offset> in <session>
|
||||
kCmdCreate, // creates <path> for writing, returns <session>
|
||||
kCmdWrite, // appends <size> bytes at <offset> in <session>
|
||||
kCmdRemove, // remove file (only if created by server?)
|
||||
|
||||
kRspAck,
|
||||
kRspNak
|
||||
kCmdNone, ///< ignored, always acked
|
||||
kCmdTerminateSession, ///< Terminates open Read session
|
||||
kCmdResetSessions, ///< Terminates all open Read sessions
|
||||
kCmdListDirectory, ///< List files in <path> from <offset>
|
||||
kCmdOpenFile, ///< Opens file at <path> for reading, returns <session>
|
||||
kCmdReadFile, ///< Reads <size> bytes from <offset> in <session>
|
||||
kCmdCreateFile, ///< Creates file at <path> for writing, returns <session>
|
||||
kCmdWriteFile, ///< Appends <size> bytes to file in <session>
|
||||
kCmdRemoveFile, ///< Remove file at <path>
|
||||
kCmdCreateDirectory, ///< Creates directory at <path>
|
||||
kCmdRemoveDirectory, ///< Removes Directory at <path>, must be empty
|
||||
|
||||
kRspAck = 128, ///< Ack response
|
||||
kRspNak ///< Nak response
|
||||
};
|
||||
|
||||
|
||||
/// @brief Error codes returned in Nak response PayloadHeader.data[0].
|
||||
enum ErrorCode : uint8_t
|
||||
{
|
||||
{
|
||||
kErrNone,
|
||||
kErrNoRequest,
|
||||
kErrNoSession,
|
||||
kErrSequence,
|
||||
kErrNotDir,
|
||||
kErrNotFile,
|
||||
kErrEOF,
|
||||
kErrNotAppend,
|
||||
kErrTooBig,
|
||||
kErrIO,
|
||||
kErrPerm
|
||||
};
|
||||
|
||||
int _findUnusedSession(void);
|
||||
bool _validSession(unsigned index);
|
||||
|
||||
static const unsigned kMaxSession = 2;
|
||||
int _session_fds[kMaxSession];
|
||||
|
||||
class Request
|
||||
kErrFail, ///< Unknown failure
|
||||
kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1]
|
||||
kErrInvalidDataSize, ///< PayloadHeader.size is invalid
|
||||
kErrInvalidSession, ///< Session is not currently open
|
||||
kErrNoSessionsAvailable, ///< All available Sessions in use
|
||||
kErrEOF, ///< Offset past end of file for List and Read commands
|
||||
kErrUnknownCommand ///< Unknown command opcode
|
||||
};
|
||||
|
||||
private:
|
||||
/// @brief Unit of work which is queued to work_queue
|
||||
struct Request
|
||||
{
|
||||
public:
|
||||
union {
|
||||
dq_entry_t entry;
|
||||
work_s work;
|
||||
};
|
||||
|
||||
bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
|
||||
if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
|
||||
_systemId = fromMessage->sysid;
|
||||
_mavlink = mavlink;
|
||||
mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message);
|
||||
return _message.target_system == _mavlink->get_system_id();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void reply() {
|
||||
|
||||
// XXX the proper way would be an IOCTL / uORB call, rather than exploiting the
|
||||
// flat memory architecture, as we're operating between threads here.
|
||||
mavlink_message_t msg;
|
||||
msg.checksum = 0;
|
||||
unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(), // Sender system id
|
||||
_mavlink->get_component_id(), // Sender component id
|
||||
_mavlink->get_channel(), // Channel to send on
|
||||
&msg, // Message to pack payload into
|
||||
0, // Target network
|
||||
_systemId, // Target system id
|
||||
0, // Target component id
|
||||
rawData()); // Payload to pack into message
|
||||
|
||||
_mavlink->lockMessageBufferMutex();
|
||||
bool success = _mavlink->message_buffer_write(&msg, len);
|
||||
_mavlink->unlockMessageBufferMutex();
|
||||
|
||||
if (!success) {
|
||||
warnx("FTP TX ERR");
|
||||
}
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
else {
|
||||
warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
|
||||
_mavlink->get_system_id(),
|
||||
_mavlink->get_component_id(),
|
||||
_mavlink->get_channel(),
|
||||
len,
|
||||
msg.checksum);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
uint8_t *rawData() { return &_message.payload[0]; }
|
||||
RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.payload[0]); }
|
||||
uint8_t *requestData() { return &(header()->data[0]); }
|
||||
unsigned dataSize() { return header()->size + sizeof(RequestHeader); }
|
||||
mavlink_channel_t channel() { return _mavlink->get_channel(); }
|
||||
|
||||
char *dataAsCString();
|
||||
|
||||
private:
|
||||
Mavlink *_mavlink;
|
||||
mavlink_file_transfer_protocol_t _message;
|
||||
uint8_t _systemId;
|
||||
work_s work; ///< work queue entry
|
||||
Mavlink *mavlink; ///< Mavlink to reply to
|
||||
uint8_t serverSystemId; ///< System ID to send from
|
||||
uint8_t serverComponentId; ///< Component ID to send from
|
||||
uint8_t serverChannel; ///< Channel to send to
|
||||
uint8_t targetSystemId; ///< System ID to target reply to
|
||||
|
||||
mavlink_file_transfer_protocol_t message; ///< Protocol message
|
||||
};
|
||||
|
||||
Request *_get_request(void);
|
||||
void _return_request(Request *req);
|
||||
void _lock_request_queue(void);
|
||||
void _unlock_request_queue(void);
|
||||
|
||||
char *_data_as_cstring(PayloadHeader* payload);
|
||||
|
||||
static void _worker_trampoline(void *arg);
|
||||
void _process_request(Request *req);
|
||||
void _reply(Request *req);
|
||||
|
||||
static const char kDirentFile = 'F';
|
||||
static const char kDirentDir = 'D';
|
||||
static const char kDirentUnknown = 'U';
|
||||
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader);
|
||||
ErrorCode _workList(PayloadHeader *payload);
|
||||
ErrorCode _workOpen(PayloadHeader *payload, bool create);
|
||||
ErrorCode _workRead(PayloadHeader *payload);
|
||||
ErrorCode _workWrite(PayloadHeader *payload);
|
||||
ErrorCode _workTerminate(PayloadHeader *payload);
|
||||
ErrorCode _workReset(PayloadHeader* payload);
|
||||
ErrorCode _workRemoveDirectory(PayloadHeader *payload);
|
||||
ErrorCode _workCreateDirectory(PayloadHeader *payload);
|
||||
ErrorCode _workRemoveFile(PayloadHeader *payload);
|
||||
|
||||
/// Request worker; runs on the low-priority work queue to service
|
||||
/// remote requests.
|
||||
///
|
||||
static void _workerTrampoline(void *arg);
|
||||
void _worker(Request *req);
|
||||
|
||||
/// Reply to a request (XXX should be a Request method)
|
||||
///
|
||||
void _reply(Request *req);
|
||||
|
||||
ErrorCode _workList(Request *req);
|
||||
ErrorCode _workOpen(Request *req, bool create);
|
||||
ErrorCode _workRead(Request *req);
|
||||
ErrorCode _workWrite(Request *req);
|
||||
ErrorCode _workRemove(Request *req);
|
||||
ErrorCode _workTerminate(Request *req);
|
||||
ErrorCode _workReset();
|
||||
|
||||
// work freelist
|
||||
Request _workBufs[kRequestQueueSize];
|
||||
dq_queue_t _workFree;
|
||||
sem_t _lock;
|
||||
|
||||
void _qLock() { do {} while (sem_wait(&_lock) != 0); }
|
||||
void _qUnlock() { sem_post(&_lock); }
|
||||
|
||||
void _qFree(Request *req) {
|
||||
_qLock();
|
||||
dq_addlast(&req->entry, &_workFree);
|
||||
_qUnlock();
|
||||
}
|
||||
|
||||
Request *_dqFree() {
|
||||
_qLock();
|
||||
auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
|
||||
_qUnlock();
|
||||
return req;
|
||||
}
|
||||
|
||||
static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests
|
||||
Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work
|
||||
dq_queue_t _request_queue; ///< Queue of available Request buffers
|
||||
sem_t _request_queue_sem; ///< Semaphore for locking access to _request_queue
|
||||
|
||||
int _find_unused_session(void);
|
||||
bool _valid_session(unsigned index);
|
||||
|
||||
static const char kDirentFile = 'F'; ///< Identifies File returned from List command
|
||||
static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command
|
||||
static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command
|
||||
|
||||
/// @brief Maximum data size in RequestHeader::data
|
||||
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader);
|
||||
|
||||
static const unsigned kMaxSession = 2; ///< Max number of active sessions
|
||||
int _session_fds[kMaxSession]; ///< Session file descriptors, 0 for empty slot
|
||||
|
||||
ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending
|
||||
MavlinkFtpTest *_ftp_test; ///< Additional parameter to _utRcvMsgFunc;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -798,10 +798,10 @@ int
|
|||
MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
|
||||
{
|
||||
if (mission_item->altitude_is_relative) {
|
||||
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
|
||||
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
||||
|
||||
} else {
|
||||
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
||||
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
|
||||
}
|
||||
|
||||
switch (mission_item->nav_cmd) {
|
||||
|
|
|
|||
|
|
@ -54,6 +54,7 @@
|
|||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
#include <drivers/drv_range_finder.h>
|
||||
#include <time.h>
|
||||
#include <float.h>
|
||||
#include <unistd.h>
|
||||
|
|
@ -102,6 +103,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
|||
_battery_pub(-1),
|
||||
_cmd_pub(-1),
|
||||
_flow_pub(-1),
|
||||
_range_pub(-1),
|
||||
_offboard_control_sp_pub(-1),
|
||||
_local_pos_sp_pub(-1),
|
||||
_global_vel_sp_pub(-1),
|
||||
|
|
@ -121,7 +123,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
|||
{
|
||||
|
||||
// make sure the FTP server is started
|
||||
(void)MavlinkFTP::getServer();
|
||||
(void)MavlinkFTP::get_server();
|
||||
}
|
||||
|
||||
MavlinkReceiver::~MavlinkReceiver()
|
||||
|
|
@ -173,7 +175,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
|
||||
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
|
||||
MavlinkFTP::get_server()->handle_message(_mavlink, msg);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -200,6 +202,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|||
handle_message_hil_state_quaternion(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
|
||||
handle_message_hil_optical_flow(msg);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
|
@ -363,6 +369,52 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
|
||||
{
|
||||
/* optical flow */
|
||||
mavlink_hil_optical_flow_t flow;
|
||||
mavlink_msg_hil_optical_flow_decode(msg, &flow);
|
||||
|
||||
struct optical_flow_s f;
|
||||
memset(&f, 0, sizeof(f));
|
||||
|
||||
f.timestamp = hrt_absolute_time();
|
||||
f.flow_timestamp = flow.time_usec;
|
||||
f.flow_raw_x = flow.flow_x;
|
||||
f.flow_raw_y = flow.flow_y;
|
||||
f.flow_comp_x_m = flow.flow_comp_m_x;
|
||||
f.flow_comp_y_m = flow.flow_comp_m_y;
|
||||
f.ground_distance_m = flow.ground_distance;
|
||||
f.quality = flow.quality;
|
||||
f.sensor_id = flow.sensor_id;
|
||||
|
||||
if (_flow_pub < 0) {
|
||||
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
|
||||
}
|
||||
|
||||
/* Use distance value for range finder report */
|
||||
struct range_finder_report r;
|
||||
memset(&r, 0, sizeof(f));
|
||||
|
||||
r.timestamp = hrt_absolute_time();
|
||||
r.error_count = 0;
|
||||
r.type = RANGE_FINDER_TYPE_LASER;
|
||||
r.distance = flow.ground_distance;
|
||||
r.minimum_distance = 0.0f;
|
||||
r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable
|
||||
r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance);
|
||||
|
||||
if (_range_pub < 0) {
|
||||
_range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r);
|
||||
} else {
|
||||
orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
|
||||
{
|
||||
|
|
@ -444,7 +496,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
|
|||
vision_position.vx = 0.0f;
|
||||
vision_position.vy = 0.0f;
|
||||
vision_position.vz = 0.0f;
|
||||
|
||||
|
||||
math::Quaternion q;
|
||||
q.from_euler(pos.roll, pos.pitch, pos.yaw);
|
||||
|
||||
|
|
|
|||
|
|
@ -112,6 +112,7 @@ private:
|
|||
void handle_message_command_long(mavlink_message_t *msg);
|
||||
void handle_message_command_int(mavlink_message_t *msg);
|
||||
void handle_message_optical_flow(mavlink_message_t *msg);
|
||||
void handle_message_hil_optical_flow(mavlink_message_t *msg);
|
||||
void handle_message_set_mode(mavlink_message_t *msg);
|
||||
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
|
||||
void handle_message_vision_position_estimate(mavlink_message_t *msg);
|
||||
|
|
@ -142,6 +143,7 @@ private:
|
|||
orb_advert_t _battery_pub;
|
||||
orb_advert_t _cmd_pub;
|
||||
orb_advert_t _flow_pub;
|
||||
orb_advert_t _range_pub;
|
||||
orb_advert_t _offboard_control_sp_pub;
|
||||
orb_advert_t _local_pos_sp_pub;
|
||||
orb_advert_t _global_vel_sp_pub;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,757 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 mavlink_ftp_test.cpp
|
||||
/// @author Don Gagne <don@thegagnes.com>
|
||||
|
||||
#include <sys/stat.h>
|
||||
#include <crc32.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include "mavlink_ftp_test.h"
|
||||
#include "../mavlink_ftp.h"
|
||||
|
||||
/// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py
|
||||
const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = {
|
||||
{ "/etc/unit_test_data/mavlink_tests/test_238.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1}, // Read takes less than single packet
|
||||
{ "/etc/unit_test_data/mavlink_tests/test_239.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) }, // Read completely fills single packet
|
||||
{ "/etc/unit_test_data/mavlink_tests/test_240.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 }, // Read take two packets
|
||||
};
|
||||
|
||||
const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir";
|
||||
const char MavlinkFtpTest::_unittest_microsd_file[] = "/fs/microsd/ftp_unit_test_dir/file";
|
||||
|
||||
MavlinkFtpTest::MavlinkFtpTest() :
|
||||
_ftp_server{},
|
||||
_reply_msg{},
|
||||
_lastOutgoingSeqNumber{}
|
||||
{
|
||||
}
|
||||
|
||||
MavlinkFtpTest::~MavlinkFtpTest()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/// @brief Called before every test to initialize the FTP Server.
|
||||
void MavlinkFtpTest::init(void)
|
||||
{
|
||||
_ftp_server = new MavlinkFTP;;
|
||||
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
|
||||
|
||||
_cleanup_microsd();
|
||||
}
|
||||
|
||||
/// @brief Called after every test to take down the FTP Server.
|
||||
void MavlinkFtpTest::cleanup(void)
|
||||
{
|
||||
delete _ftp_server;
|
||||
|
||||
_cleanup_microsd();
|
||||
}
|
||||
|
||||
/// @brief Tests for correct behavior of an Ack response.
|
||||
bool MavlinkFtpTest::_ack_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdNone;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct response to an invalid opcpde.
|
||||
bool MavlinkFtpTest::_bad_opcode_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
payload.opcode = 0xFF; // bogus opcode
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrUnknownCommand);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a payload which an invalid data size field.
|
||||
bool MavlinkFtpTest::_bad_datasize_test(void)
|
||||
{
|
||||
mavlink_message_t msg;
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdListDirectory;
|
||||
|
||||
_setup_ftp_msg(&payload, 0, nullptr, &msg);
|
||||
|
||||
// Set the data size to be one larger than is legal
|
||||
((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1;
|
||||
|
||||
_ftp_server->handle_message(nullptr /* mavlink */, &msg);
|
||||
|
||||
if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidDataSize);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MavlinkFtpTest::_list_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240";
|
||||
char response2[] = "Ddev|Detc|Dfs|Dobj";
|
||||
|
||||
struct _testCase {
|
||||
const char *dir; ///< Directory to run List command on
|
||||
char *response; ///< Expected response entries from List command
|
||||
int response_count; ///< Number of directories that should be returned
|
||||
bool success; ///< true: List command should succeed, false: List command should fail
|
||||
};
|
||||
struct _testCase rgTestCases[] = {
|
||||
{ "/bogus", nullptr, 0, false },
|
||||
{ "/etc/unit_test_data/mavlink_tests", response1, 4, true },
|
||||
{ "/", response2, 4, true },
|
||||
};
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
const struct _testCase *test = &rgTestCases[i];
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdListDirectory;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->dir)+1, // size in bytes of data
|
||||
(uint8_t*)test->dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test->success) {
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, strlen(test->response) + 1);
|
||||
|
||||
// The return order of directories from the List command is not repeatable. So we can't do a direct comparison
|
||||
// to a hardcoded return result string.
|
||||
|
||||
// Convert null terminators to seperator char so we can use strok to parse returned data
|
||||
for (uint8_t j=0; j<reply->size-1; j++) {
|
||||
if (reply->data[j] == 0) {
|
||||
reply->data[j] = '|';
|
||||
}
|
||||
}
|
||||
|
||||
// Loop over returned directory entries trying to find then in the response list
|
||||
char *dir;
|
||||
int response_count = 0;
|
||||
dir = strtok((char *)&reply->data[0], "|");
|
||||
while (dir != nullptr) {
|
||||
ut_assert("Returned directory not found in expected response", strstr(test->response, dir));
|
||||
response_count++;
|
||||
dir = strtok(nullptr, "|");
|
||||
}
|
||||
|
||||
ut_compare("Incorrect number of directory entires returned", test->response_count, response_count);
|
||||
} else {
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that
|
||||
/// is beyond the last directory entry.
|
||||
bool MavlinkFtpTest::_list_eof_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
const char *dir = "/";
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdListDirectory;
|
||||
payload.offset = 4; // offset past top level dirs
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(dir)+1, // size in bytes of data
|
||||
(uint8_t*)dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrEOF);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to an Open command on a file which does not exist.
|
||||
bool MavlinkFtpTest::_open_badfile_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
const char *dir = "/foo"; // non-existent file
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFile;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(dir)+1, // size in bytes of data
|
||||
(uint8_t*)dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to an Open command on a file, followed by Terminate
|
||||
bool MavlinkFtpTest::_open_terminate_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
|
||||
struct stat st;
|
||||
const ReadTestCase *test = &_rgReadTestCases[i];
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFile;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->file)+1, // size in bytes of data
|
||||
(uint8_t*)test->file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("stat failed", stat(test->file, &st), 0);
|
||||
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, sizeof(uint32_t));
|
||||
ut_compare("File size incorrect", *((uint32_t*)&reply->data[0]), (uint32_t)st.st_size);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdTerminateSession;
|
||||
payload.session = reply->session;
|
||||
payload.size = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a Terminate command on an invalid session.
|
||||
bool MavlinkFtpTest::_terminate_badsession_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
const char *file = _rgReadTestCases[0].file;
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFile;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(file)+1, // size in bytes of data
|
||||
(uint8_t*)file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdTerminateSession;
|
||||
payload.session = reply->session + 1;
|
||||
payload.size = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a Read command on an open session.
|
||||
bool MavlinkFtpTest::_read_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
|
||||
struct stat st;
|
||||
const ReadTestCase *test = &_rgReadTestCases[i];
|
||||
|
||||
// Read in the file so we can compare it to what we get back
|
||||
ut_compare("stat failed", stat(test->file, &st), 0);
|
||||
uint8_t *bytes = new uint8_t[st.st_size];
|
||||
ut_assert("new failed", bytes != nullptr);
|
||||
int fd = ::open(test->file, O_RDONLY);
|
||||
ut_assert("open failed", fd != -1);
|
||||
int bytes_read = ::read(fd, bytes, st.st_size);
|
||||
ut_compare("read failed", bytes_read, st.st_size);
|
||||
::close(fd);
|
||||
|
||||
// Test case data files are created for specific boundary conditions
|
||||
ut_compare("Test case data files are out of date", test->length, st.st_size);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFile;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->file)+1, // size in bytes of data
|
||||
(uint8_t*)test->file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdReadFile;
|
||||
payload.session = reply->session;
|
||||
payload.offset = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Offset incorrect", reply->offset, 0);
|
||||
|
||||
if (test->length <= MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader)) {
|
||||
ut_compare("Payload size incorrect", reply->size, (uint32_t)st.st_size);
|
||||
ut_compare("File contents differ", memcmp(reply->data, bytes, st.st_size), 0);
|
||||
}
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdTerminateSession;
|
||||
payload.session = reply->session;
|
||||
payload.size = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a Read command on an invalid session.
|
||||
bool MavlinkFtpTest::_read_badsession_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
const char *file = _rgReadTestCases[0].file;
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFile;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(file)+1, // size in bytes of data
|
||||
(uint8_t*)file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdReadFile;
|
||||
payload.session = reply->session + 1; // Invalid session
|
||||
payload.offset = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MavlinkFtpTest::_removedirectory_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
int fd;
|
||||
|
||||
struct _testCase {
|
||||
const char *dir;
|
||||
bool success;
|
||||
bool deleteFile;
|
||||
};
|
||||
static const struct _testCase rgTestCases[] = {
|
||||
{ "/bogus", false, false },
|
||||
{ "/etc/unit_test_data/mavlink_tests/empty_dir", false, false },
|
||||
{ _unittest_microsd_dir, false, false },
|
||||
{ _unittest_microsd_file, false, false },
|
||||
{ _unittest_microsd_dir, true, true },
|
||||
};
|
||||
|
||||
ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
|
||||
ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
|
||||
::close(fd);
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
const struct _testCase *test = &rgTestCases[i];
|
||||
|
||||
if (test->deleteFile) {
|
||||
ut_compare("unlink failed", ::unlink(_unittest_microsd_file), 0);
|
||||
}
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdRemoveDirectory;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->dir)+1, // size in bytes of data
|
||||
(uint8_t*)test->dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test->success) {
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
} else {
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MavlinkFtpTest::_createdirectory_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
struct _testCase {
|
||||
const char *dir;
|
||||
bool success;
|
||||
};
|
||||
static const struct _testCase rgTestCases[] = {
|
||||
{ "/etc/bogus", false },
|
||||
{ _unittest_microsd_dir, true },
|
||||
{ _unittest_microsd_dir, false },
|
||||
{ "/fs/microsd/bogus/bogus", false },
|
||||
};
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
const struct _testCase *test = &rgTestCases[i];
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdCreateDirectory;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->dir)+1, // size in bytes of data
|
||||
(uint8_t*)test->dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test->success) {
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
} else {
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MavlinkFtpTest::_removefile_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
int fd;
|
||||
|
||||
struct _testCase {
|
||||
const char *file;
|
||||
bool success;
|
||||
};
|
||||
static const struct _testCase rgTestCases[] = {
|
||||
{ "/bogus", false },
|
||||
{ _rgReadTestCases[0].file, false },
|
||||
{ _unittest_microsd_dir, false },
|
||||
{ _unittest_microsd_file, true },
|
||||
{ _unittest_microsd_file, false },
|
||||
};
|
||||
|
||||
ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
|
||||
ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
|
||||
::close(fd);
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
const struct _testCase *test = &rgTestCases[i];
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdRemoveFile;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->file)+1, // size in bytes of data
|
||||
(uint8_t*)test->file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test->success) {
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
} else {
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Static method used as callback from MavlinkFTP. This method will be called by MavlinkFTP when
|
||||
/// it needs to send a message out on Mavlink.
|
||||
void MavlinkFtpTest::receive_message(const mavlink_message_t *msg, MavlinkFtpTest *ftp_test)
|
||||
{
|
||||
ftp_test->_receive_message(msg);
|
||||
}
|
||||
|
||||
/// @brief Non-Static version of receive_message
|
||||
void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg)
|
||||
{
|
||||
// Move the message into our own member variable
|
||||
memcpy(&_reply_msg, msg, sizeof(mavlink_message_t));
|
||||
}
|
||||
|
||||
/// @brief Decode and validate the incoming message
|
||||
bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlink message to decode
|
||||
mavlink_file_transfer_protocol_t *ftp_msg, ///< Decoded FTP message
|
||||
MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response
|
||||
{
|
||||
mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg);
|
||||
|
||||
// Make sure the targets are correct
|
||||
ut_compare("Target network non-zero", ftp_msg->target_network, 0);
|
||||
ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId);
|
||||
ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId);
|
||||
|
||||
*payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
|
||||
|
||||
// Make sure we have a good sequence number
|
||||
ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1);
|
||||
|
||||
// Bump sequence number for next outgoing message
|
||||
_lastOutgoingSeqNumber++;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Initializes an FTP message into a mavlink message
|
||||
void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header
|
||||
uint8_t size, ///< size in bytes of data
|
||||
const uint8_t *data, ///< Data to start into FTP message payload
|
||||
mavlink_message_t *msg) ///< Returned mavlink message
|
||||
{
|
||||
uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN];
|
||||
MavlinkFTP::PayloadHeader *payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(payload_bytes);
|
||||
|
||||
memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader));
|
||||
|
||||
payload->seqNumber = _lastOutgoingSeqNumber;
|
||||
payload->size = size;
|
||||
if (size != 0) {
|
||||
memcpy(payload->data, data, size);
|
||||
}
|
||||
|
||||
payload->padding[0] = 0;
|
||||
payload->padding[1] = 0;
|
||||
|
||||
msg->checksum = 0;
|
||||
mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id
|
||||
clientComponentId, // Sender component id
|
||||
msg, // Message to pack payload into
|
||||
0, // Target network
|
||||
serverSystemId, // Target system id
|
||||
serverComponentId, // Target component id
|
||||
payload_bytes); // Payload to pack into message
|
||||
}
|
||||
|
||||
/// @brief Sends the specified FTP message to the server and returns response
|
||||
bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header
|
||||
uint8_t size, ///< size in bytes of data
|
||||
const uint8_t *data, ///< Data to start into FTP message payload
|
||||
mavlink_file_transfer_protocol_t *ftp_msg_reply, ///< Response from server
|
||||
MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response
|
||||
{
|
||||
mavlink_message_t msg;
|
||||
|
||||
_setup_ftp_msg(payload_header, size, data, &msg);
|
||||
_ftp_server->handle_message(nullptr /* mavlink */, &msg);
|
||||
return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply);
|
||||
}
|
||||
|
||||
/// @brief Cleans up an files created on microsd during testing
|
||||
void MavlinkFtpTest::_cleanup_microsd(void)
|
||||
{
|
||||
::unlink(_unittest_microsd_file);
|
||||
::rmdir(_unittest_microsd_dir);
|
||||
}
|
||||
|
||||
/// @brief Runs all the unit tests
|
||||
void MavlinkFtpTest::runTests(void)
|
||||
{
|
||||
ut_run_test(_ack_test);
|
||||
ut_run_test(_bad_opcode_test);
|
||||
ut_run_test(_bad_datasize_test);
|
||||
ut_run_test(_list_test);
|
||||
ut_run_test(_list_eof_test);
|
||||
ut_run_test(_open_badfile_test);
|
||||
ut_run_test(_open_terminate_test);
|
||||
ut_run_test(_terminate_badsession_test);
|
||||
ut_run_test(_read_test);
|
||||
ut_run_test(_read_badsession_test);
|
||||
ut_run_test(_removedirectory_test);
|
||||
ut_run_test(_createdirectory_test);
|
||||
ut_run_test(_removefile_test);
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,107 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 mavlink_ftp_test.h
|
||||
/// @author Don Gagne <don@thegagnes.com>
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <unit_test/unit_test.h>
|
||||
#include "../mavlink_bridge_header.h"
|
||||
#include "../mavlink_ftp.h"
|
||||
|
||||
class MavlinkFtpTest : public UnitTest
|
||||
{
|
||||
public:
|
||||
MavlinkFtpTest();
|
||||
virtual ~MavlinkFtpTest();
|
||||
|
||||
virtual void init(void);
|
||||
virtual void cleanup(void);
|
||||
|
||||
virtual void runTests(void);
|
||||
|
||||
static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
|
||||
|
||||
static const uint8_t serverSystemId = 50; ///< System ID for server
|
||||
static const uint8_t serverComponentId = 1; ///< Component ID for server
|
||||
static const uint8_t serverChannel = 0; ///< Channel to send to
|
||||
|
||||
static const uint8_t clientSystemId = 1; ///< System ID for client
|
||||
static const uint8_t clientComponentId = 0; ///< Component ID for client
|
||||
|
||||
// We don't want any of these
|
||||
MavlinkFtpTest(const MavlinkFtpTest&);
|
||||
MavlinkFtpTest& operator=(const MavlinkFtpTest&);
|
||||
|
||||
private:
|
||||
bool _ack_test(void);
|
||||
bool _bad_opcode_test(void);
|
||||
bool _bad_datasize_test(void);
|
||||
bool _list_test(void);
|
||||
bool _list_eof_test(void);
|
||||
bool _open_badfile_test(void);
|
||||
bool _open_terminate_test(void);
|
||||
bool _terminate_badsession_test(void);
|
||||
bool _read_test(void);
|
||||
bool _read_badsession_test(void);
|
||||
bool _removedirectory_test(void);
|
||||
bool _createdirectory_test(void);
|
||||
bool _removefile_test(void);
|
||||
|
||||
void _receive_message(const mavlink_message_t *msg);
|
||||
void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
|
||||
bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload);
|
||||
bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header,
|
||||
uint8_t size,
|
||||
const uint8_t *data,
|
||||
mavlink_file_transfer_protocol_t *ftp_msg_reply,
|
||||
MavlinkFTP::PayloadHeader **payload_reply);
|
||||
void _cleanup_microsd(void);
|
||||
|
||||
MavlinkFTP *_ftp_server;
|
||||
|
||||
mavlink_message_t _reply_msg;
|
||||
|
||||
uint16_t _lastOutgoingSeqNumber;
|
||||
|
||||
struct ReadTestCase {
|
||||
const char *file;
|
||||
const uint16_t length;
|
||||
};
|
||||
static const ReadTestCase _rgReadTestCases[];
|
||||
|
||||
static const char _unittest_microsd_dir[];
|
||||
static const char _unittest_microsd_file[];
|
||||
};
|
||||
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
import sys
|
||||
print 'Arguments: file - ' + sys.argv[1] + ', length - ' + sys.argv[2]
|
||||
file = open(sys.argv[1], 'w')
|
||||
for i in range(int(sys.argv[2])):
|
||||
b = bytearray([i & 0xFF])
|
||||
file.write(b)
|
||||
file.close()
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 mavlink_ftp_tests.cpp
|
||||
*/
|
||||
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include "mavlink_ftp_test.h"
|
||||
|
||||
extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]);
|
||||
|
||||
int mavlink_tests_main(int argc, char *argv[])
|
||||
{
|
||||
MavlinkFtpTest* test = new MavlinkFtpTest;
|
||||
|
||||
test->runTests();
|
||||
test->printResults();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,48 @@
|
|||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# System state machine tests.
|
||||
#
|
||||
|
||||
MODULE_COMMAND = mavlink_tests
|
||||
SRCS = mavlink_tests.cpp \
|
||||
mavlink_ftp_test.cpp \
|
||||
../mavlink_ftp.cpp \
|
||||
../mavlink.c
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
|
||||
MODULE_STACKSIZE = 5000
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST
|
||||
|
|
@ -36,12 +36,14 @@
|
|||
* Helper class to access missions
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <float.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
|
|
@ -49,6 +51,7 @@
|
|||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <geo/geo.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
|
|
@ -62,6 +65,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
|
|||
_param_onboard_enabled(this, "ONBOARD_EN"),
|
||||
_param_takeoff_alt(this, "TAKEOFF_ALT"),
|
||||
_param_dist_1wp(this, "DIST_1WP"),
|
||||
_param_altmode(this, "ALTMODE"),
|
||||
_onboard_mission({0}),
|
||||
_offboard_mission({0}),
|
||||
_current_onboard_mission_index(-1),
|
||||
|
|
@ -72,7 +76,11 @@ Mission::Mission(Navigator *navigator, const char *name) :
|
|||
_mission_result({0}),
|
||||
_mission_type(MISSION_TYPE_NONE),
|
||||
_inited(false),
|
||||
_dist_1wp_ok(false)
|
||||
_dist_1wp_ok(false),
|
||||
_missionFeasiblityChecker(),
|
||||
_min_current_sp_distance_xy(FLT_MAX),
|
||||
_mission_item_previous_alt(NAN),
|
||||
_distance_current_previous(0.0f)
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
|
|
@ -144,6 +152,8 @@ Mission::on_active()
|
|||
advance_mission();
|
||||
set_mission_items();
|
||||
|
||||
} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
|
||||
altitude_sp_foh_update();
|
||||
} else {
|
||||
/* if waypoint position reached allow loiter on the setpoint */
|
||||
if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
|
||||
|
|
@ -202,7 +212,7 @@ Mission::update_offboard_mission()
|
|||
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
|
||||
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
|
||||
|
||||
missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
|
||||
_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
|
||||
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
|
||||
_navigator->get_home_position()->alt);
|
||||
|
||||
|
|
@ -313,11 +323,19 @@ Mission::set_mission_items()
|
|||
/* make sure param is up to date */
|
||||
updateParams();
|
||||
|
||||
/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
|
||||
altitude_sp_foh_reset();
|
||||
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
/* set previous position setpoint to current */
|
||||
set_previous_pos_setpoint();
|
||||
|
||||
/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
|
||||
if (pos_sp_triplet->previous.valid) {
|
||||
_mission_item_previous_alt = _mission_item.altitude;
|
||||
}
|
||||
|
||||
/* get home distance state */
|
||||
bool home_dist_ok = check_dist_1wp();
|
||||
/* the home dist check provides user feedback, so we initialize it to this */
|
||||
|
|
@ -446,9 +464,75 @@ Mission::set_mission_items()
|
|||
}
|
||||
}
|
||||
|
||||
/* Save the distance between the current sp and the previous one */
|
||||
if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
|
||||
_distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat,
|
||||
pos_sp_triplet->current.lon,
|
||||
pos_sp_triplet->previous.lat,
|
||||
pos_sp_triplet->previous.lon);
|
||||
}
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
}
|
||||
|
||||
void
|
||||
Mission::altitude_sp_foh_update()
|
||||
{
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
/* Don't change setpoint if last and current waypoint are not valid */
|
||||
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
|
||||
!isfinite(_mission_item_previous_alt)) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
|
||||
if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Don't do FOH for landing and takeoff waypoints, the ground may be near
|
||||
* and the FW controller has a custom landing logic */
|
||||
if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
/* Calculate distance to current waypoint */
|
||||
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
|
||||
|
||||
/* Save distance to waypoint if it is the smallest ever achieved, however make sure that
|
||||
* _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */
|
||||
_min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy),
|
||||
_distance_current_previous);
|
||||
|
||||
/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
|
||||
* navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
|
||||
if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) {
|
||||
pos_sp_triplet->current.alt = _mission_item.altitude;
|
||||
} else {
|
||||
/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
|
||||
* of the mission item as it is used to check if the mission item is reached
|
||||
* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
|
||||
* radius around the current waypoint
|
||||
**/
|
||||
float delta_alt = (_mission_item.altitude - _mission_item_previous_alt);
|
||||
float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius);
|
||||
float a = _mission_item_previous_alt - grad * _distance_current_previous;
|
||||
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
|
||||
|
||||
}
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
}
|
||||
|
||||
void
|
||||
Mission::altitude_sp_foh_reset()
|
||||
{
|
||||
_min_current_sp_distance_xy = FLT_MAX;
|
||||
}
|
||||
|
||||
bool
|
||||
Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -36,6 +36,7 @@
|
|||
* Navigator mode to access missions
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_MISSION_H
|
||||
|
|
@ -75,6 +76,11 @@ public:
|
|||
|
||||
virtual void on_active();
|
||||
|
||||
enum mission_altitude_mode {
|
||||
MISSION_ALTMODE_ZOH = 0,
|
||||
MISSION_ALTMODE_FOH = 1
|
||||
};
|
||||
|
||||
private:
|
||||
/**
|
||||
* Update onboard mission topic
|
||||
|
|
@ -102,6 +108,16 @@ private:
|
|||
*/
|
||||
void set_mission_items();
|
||||
|
||||
/**
|
||||
* Updates the altitude sp to follow a foh
|
||||
*/
|
||||
void altitude_sp_foh_update();
|
||||
|
||||
/**
|
||||
* Resets the altitude sp foh logic
|
||||
*/
|
||||
void altitude_sp_foh_reset();
|
||||
|
||||
/**
|
||||
* Read current or next mission item from the dataman and watch out for DO_JUMPS
|
||||
* @return true if successful
|
||||
|
|
@ -136,6 +152,7 @@ private:
|
|||
control::BlockParamInt _param_onboard_enabled;
|
||||
control::BlockParamFloat _param_takeoff_alt;
|
||||
control::BlockParamFloat _param_dist_1wp;
|
||||
control::BlockParamInt _param_altmode;
|
||||
|
||||
struct mission_s _onboard_mission;
|
||||
struct mission_s _offboard_mission;
|
||||
|
|
@ -157,7 +174,13 @@ private:
|
|||
bool _inited;
|
||||
bool _dist_1wp_ok;
|
||||
|
||||
MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
|
||||
MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */
|
||||
|
||||
float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */
|
||||
float _mission_item_previous_alt; /**< holds the altitude of the previous mission item,
|
||||
can be replaced by a full copy of the previous mission item if needed*/
|
||||
float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
|
||||
only use if current and previous are valid */
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -113,6 +113,19 @@ MissionBlock::is_mission_item_reached()
|
|||
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
|
||||
(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
|
||||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
||||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) {
|
||||
/* Loiter mission item on a non rotary wing: the aircraft is going to circle the
|
||||
* coordinates with a radius equal to the loiter_radius field. It is not flying
|
||||
* through the waypoint center.
|
||||
* Therefore the item is marked as reached once the system reaches the loiter
|
||||
* radius (+ some margin). Time inside and turn count is handled elsewhere.
|
||||
*/
|
||||
if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
} else {
|
||||
/* for normal mission items used their acceptance radius */
|
||||
if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
|
||||
|
|
|
|||
|
|
@ -82,3 +82,16 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
|
|||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
|
||||
|
||||
/**
|
||||
* Altitude setpoint mode
|
||||
*
|
||||
* 0: the system will follow a zero order hold altitude setpoint
|
||||
* 1: the system will follow a first order hold altitude setpoint
|
||||
* values follow the definition in enum mission_altitude_mode
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MIS_ALTMODE, 0);
|
||||
|
|
|
|||
|
|
@ -1432,6 +1432,11 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
|
||||
log_msg.body.log_GPOS.eph = buf.global_pos.eph;
|
||||
log_msg.body.log_GPOS.epv = buf.global_pos.epv;
|
||||
if (buf.global_pos.terrain_alt_valid) {
|
||||
log_msg.body.log_GPOS.terrain_alt = buf.global_pos.terrain_alt;
|
||||
} else {
|
||||
log_msg.body.log_GPOS.terrain_alt = -1.0f;
|
||||
}
|
||||
LOGBUFFER_WRITE_AND_COUNT(GPOS);
|
||||
}
|
||||
|
||||
|
|
@ -1464,7 +1469,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
|
||||
LOGBUFFER_WRITE_AND_COUNT(VICN);
|
||||
}
|
||||
|
||||
|
||||
/* --- VISION POSITION --- */
|
||||
if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) {
|
||||
log_msg.msg_type = LOG_VISN_MSG;
|
||||
|
|
|
|||
|
|
@ -220,6 +220,7 @@ struct log_GPOS_s {
|
|||
float vel_d;
|
||||
float eph;
|
||||
float epv;
|
||||
float terrain_alt;
|
||||
};
|
||||
|
||||
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
|
||||
|
|
@ -449,7 +450,7 @@ static const struct log_format_s log_formats[] = {
|
|||
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
|
||||
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
|
||||
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
|
||||
LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
|
||||
LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
|
||||
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
|
||||
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
|
||||
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
|
||||
|
|
|
|||
|
|
@ -322,7 +322,8 @@ param_get_value_ptr(param_t param)
|
|||
v = ¶m_info_base[param].val;
|
||||
}
|
||||
|
||||
if (param_type(param) == PARAM_TYPE_STRUCT) {
|
||||
if (param_type(param) >= PARAM_TYPE_STRUCT
|
||||
&& param_type(param) <= PARAM_TYPE_STRUCT_MAX) {
|
||||
result = v->p;
|
||||
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -307,7 +307,7 @@ __EXPORT int param_load_default(void);
|
|||
struct param_info_s __param__##_name = { \
|
||||
#_name, \
|
||||
PARAM_TYPE_STRUCT + sizeof(_default), \
|
||||
.val.p = &_default; \
|
||||
.val.p = &_default \
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -72,6 +72,8 @@ struct vehicle_global_position_s {
|
|||
float yaw; /**< Yaw in radians -PI..+PI. */
|
||||
float eph; /**< Standard deviation of position estimate horizontally */
|
||||
float epv; /**< Standard deviation of position vertically */
|
||||
float terrain_alt; /**< Terrain altitude in m, WGS84 */
|
||||
bool terrain_alt_valid; /**< Terrain altitude estimate is valid */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -43,8 +43,6 @@
|
|||
#include <systemlib/err.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#define MM_PER_CM 10 // Millimeters per centimeter
|
||||
|
||||
const char *const UavcanGnssBridge::NAME = "gnss";
|
||||
|
||||
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
|
||||
|
|
@ -95,9 +93,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
|
|||
auto report = ::vehicle_gps_position_s();
|
||||
|
||||
report.timestamp_position = hrt_absolute_time();
|
||||
report.lat = msg.lat_1e7;
|
||||
report.lon = msg.lon_1e7;
|
||||
report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3)
|
||||
report.lat = msg.latitude_deg_1e8 / 10;
|
||||
report.lon = msg.longitude_deg_1e8 / 10;
|
||||
report.alt = msg.height_msl_mm;
|
||||
|
||||
report.timestamp_variance = report.timestamp_position;
|
||||
|
||||
|
|
|
|||
|
|
@ -56,3 +56,8 @@ void UnitTest::printAssert(const char* msg, const char* test, const char* file,
|
|||
{
|
||||
warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
|
||||
}
|
||||
|
||||
void UnitTest::printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line)
|
||||
{
|
||||
warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -52,11 +52,15 @@ INLINE_GLOBAL(const char*, mu_last_test)
|
|||
|
||||
UnitTest();
|
||||
virtual ~UnitTest();
|
||||
|
||||
virtual void init(void) { };
|
||||
virtual void cleanup(void) { };
|
||||
|
||||
virtual void runTests(void) = 0;
|
||||
void printResults(void);
|
||||
|
||||
void printAssert(const char* msg, const char* test, const char* file, int line);
|
||||
void printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line);
|
||||
|
||||
#define ut_assert(message, test) \
|
||||
do { \
|
||||
|
|
@ -68,10 +72,23 @@ INLINE_GLOBAL(const char*, mu_last_test)
|
|||
} \
|
||||
} while (0)
|
||||
|
||||
#define ut_compare(message, v1, v2) \
|
||||
do { \
|
||||
int _v1 = v1; \
|
||||
int _v2 = v2; \
|
||||
if (_v1 != _v2) { \
|
||||
printCompare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \
|
||||
return false; \
|
||||
} else { \
|
||||
mu_assertion()++; \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
#define ut_run_test(test) \
|
||||
do { \
|
||||
warnx("RUNNING TEST: %s", #test); \
|
||||
mu_tests_run()++; \
|
||||
init(); \
|
||||
if (!test()) { \
|
||||
warnx("TEST FAILED: %s", #test); \
|
||||
mu_tests_failed()++; \
|
||||
|
|
@ -79,6 +96,7 @@ INLINE_GLOBAL(const char*, mu_last_test)
|
|||
warnx("TEST PASSED: %s", #test); \
|
||||
mu_tests_passed()++; \
|
||||
} \
|
||||
cleanup(); \
|
||||
} while (0)
|
||||
|
||||
};
|
||||
|
|
|
|||
2
uavcan
2
uavcan
|
|
@ -1 +1 @@
|
|||
Subproject commit c4c14c60fbbd9acd281ee97d5bb2a4027d0ae2d9
|
||||
Subproject commit 286adbcc56c4b093143b647ec7546abb149cd53b
|
||||
Loading…
Reference in New Issue