Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 18:52:46 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'private_swissfang/master' into launchpitchlimit_swissfang

Browse Source
This commit is contained in:
Thomas Gubler
2014-09-08 12:45:09 +02:00
parent daf1618420 cf601c09bf
commit 33c0cd2e56
24 changed files with 1872 additions and 403 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.autostart
+6
View File
@@ -21,6 +21,12 @@
# Simulation setups
#
if param compare SYS_AUTOSTART 901
then
sh /etc/init.d/901_bottle_drop_test.hil
set MODE custom
fi
if param compare SYS_AUTOSTART 1000
then
sh /etc/init.d/1000_rc_fw_easystar.hil
ROMFS/px4fmu_common/init.d/rc.fw_apps
+2
View File
@@ -13,3 +13,5 @@ ekf_att_pos_estimator start
#
fw_att_control start
fw_pos_control_l1 start
bottle_drop start
ROMFS/px4fmu_common/init.d/rc.sensors
+5
View File
@@ -68,6 +68,11 @@ else
fi
fi
# Check for flow sensor
if px4flow start
then
fi
#
# Start the sensor collection task.
# IMPORTANT: this also loads param offsets
ROMFS/px4fmu_common/mixers/FMU_FX79.mix
+5 -8
View File
@@ -52,21 +52,18 @@ M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / flaps / payload mixer for last four channels
Inputs to the mixer come from channel group 2 (payload), channels 0
(bay servo 1), 1 (bay servo 2) and 3 (drop release).
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
S: 2 0 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
S: 2 1 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
S: 2 2 -10000 -10000 0 -10000 10000
makefiles/config_px4fmu-v1_default.mk
+4 -4
View File
@@ -67,17 +67,17 @@ MODULES += modules/gpio_led
#
# Estimation modules (EKF / other filters)
#
MODULES += modules/attitude_estimator_ekf
#MODULES += modules/attitude_estimator_ekf
MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav
#MODULES += modules/position_estimator_inav
#
# Vehicle Control
#
MODULES += modules/fw_pos_control_l1
MODULES += modules/fw_att_control
MODULES += modules/mc_att_control
MODULES += modules/mc_pos_control
#MODULES += modules/mc_att_control
#MODULES += modules/mc_pos_control
#
# Logging
makefiles/config_px4fmu-v2_default.mk
+5
View File
@@ -128,6 +128,11 @@ MODULES += lib/geo_lookup
MODULES += lib/conversion
MODULES += lib/launchdetection
#
# OBC challenge
#
MODULES += modules/bottle_drop
#
# Demo apps
#
src/drivers/hil/hil.cpp
+2 -1
View File
@@ -392,7 +392,8 @@ HIL::task_main()
if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* can we mix? */
if (_mixers != nullptr) {
src/lib/external_lgpl/tecs/tecs.cpp
+1 -2
View File
@@ -236,9 +236,8 @@ void TECS::_update_height_demand(float demand, float state)
// // _hgt_rate_dem);
_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
_hgt_dem_adj_last = _hgt_dem_adj;
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
// Limit height rate of change
if (_hgt_rate_dem > _maxClimbRate) {
_hgt_rate_dem = _maxClimbRate;
src/lib/external_lgpl/tecs/tecs.h
+6
View File
@@ -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
src/modules/bottle_drop/bottle_drop.cpp
+899
View File
File diff suppressed because it is too large Load Diff
src/modules/bottle_drop/bottle_drop_params.c
+131
View File
@@ -0,0 +1,131 @@
/****************************************************************************
*
* Copyright (c) 2013, 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 bottle_drop_params.c
* Bottle drop parameters
*
* @author Dominik Juchli <juchlid@ethz.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/**
* Ground drag property
*
* This parameter encodes the ground drag coefficient and the corresponding
* decrease in wind speed from the plane altitude to ground altitude.
*
* @unit unknown
* @min 0.001
* @max 0.1
* @group Payload drop
*/
PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f);
/**
* Plane turn radius
*
* The planes known minimal turn radius - use a higher value
* to make the plane maneuver more distant from the actual drop
* position. This is to ensure the wings are level during the drop.
*
* @unit meter
* @min 30.0
* @max 500.0
* @group Payload drop
*/
PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 120.0f);
/**
* Drop precision
*
* If the system is closer than this distance on passing over the
* drop position, it will release the payload. This is a safeguard
* to prevent a drop out of the required accuracy.
*
* @unit meter
* @min 1.0
* @max 80.0
* @group Payload drop
*/
PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f);
/**
* Payload drag coefficient of the dropped object
*
* The drag coefficient (cd) is the typical drag
* constant for air. It is in general object specific,
* but the closest primitive shape to the actual object
* should give good results:
* http://en.wikipedia.org/wiki/Drag_coefficient
*
* @unit meter
* @min 1.0
* @max 80.0
* @group Payload drop
*/
PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.86f);
/**
* Payload mass
*
* A typical small toy ball:
* 0.025 kg
*
* OBC water bottle:
* 0.6 kg
*
* @unit kilogram
* @min 0.001
* @max 5.0
* @group Payload drop
*/
PARAM_DEFINE_FLOAT(BD_OBJ_MASS, 0.6f);
/**
* Payload front surface area
*
* A typical small toy ball:
* (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2
*
* OBC water bottle:
* (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2
*
* @unit m^2
* @min 0.001
* @max 0.5
* @group Payload drop
*/
PARAM_DEFINE_FLOAT(BD_OBJ_SURFACE, 0.00311724531f);
src/modules/bottle_drop/module.mk
+41
View File
@@ -0,0 +1,41 @@
############################################################################
#
# Copyright (c) 2013, 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.
#
############################################################################
#
# Daemon application
#
MODULE_COMMAND = bottle_drop
SRCS = bottle_drop.cpp \
bottle_drop_params.c
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+67 -22
View File
@@ -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];
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+507 -299
View File
File diff suppressed because one or more lines are too long
src/modules/ekf_att_pos_estimator/estimator_23states.h
+58 -3
View File
@@ -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();
src/modules/fw_att_control/fw_att_control_main.cpp
+33
View File
@@ -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;
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+5
View File
@@ -211,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;
@@ -256,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;
@@ -494,6 +496,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 +566,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 +604,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 */
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+66 -59
View File
@@ -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
*
src/modules/mavlink/mavlink_bridge_header.h
+1 -1
View File
@@ -84,7 +84,7 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng
extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
#include <v1.0/common/mavlink.h>
#include <v1.0/pixhawk/mavlink.h>
__END_DECLS
src/modules/mavlink/mavlink_main.cpp
+5 -2
View File
@@ -1387,23 +1387,26 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
configure_stream("HIGHRES_IMU", 1.0f);
configure_stream("ATTITUDE", 10.0f);
configure_stream("ATTITUDE", 15.0f);
configure_stream("VFR_HUD", 8.0f);
configure_stream("GPS_RAW_INT", 1.0f);
configure_stream("GLOBAL_POSITION_INT", 3.0f);
configure_stream("LOCAL_POSITION_NED", 3.0f);
configure_stream("RC_CHANNELS_RAW", 1.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("ATTITUDE_TARGET", 15.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
configure_stream("OPTICAL_FLOW", 20.0f);
break;
case MAVLINK_MODE_ONBOARD:
configure_stream("SYS_STATUS", 1.0f);
// XXX OBC change back: We need to be bandwidth-efficient here too
configure_stream("ATTITUDE", 50.0f);
configure_stream("GLOBAL_POSITION_INT", 50.0f);
configure_stream("CAMERA_CAPTURE", 2.0f);
configure_stream("ATTITUDE_TARGET", 50.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 20.0f);
break;
default:
src/modules/navigator/mission_block.cpp
+13
View File
@@ -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) {
src/modules/sdlog2/sdlog2.c
+6 -1
View File
@@ -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;
src/modules/sdlog2/sdlog2_messages.h
+2 -1
View File
@@ -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"),
src/modules/uORB/topics/vehicle_global_position.h
+2
View File
@@ -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 */
};
/**