fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 17:17:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'paul_estimator' of github.com:PX4/Firmware into paul_estimator

Browse Source
This commit is contained in:
Lorenz Meier
2014-02-01 17:23:24 +01:00
parent 0962d041f1 80d645ada8
commit 7c74229a1f
7 changed files with 180 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.autostart
+1 -1
View File
@@ -23,7 +23,7 @@
if param compare SYS_AUTOSTART 1000
then
#sh /etc/init.d/1000_rc_fw_easystar.hil
sh /etc/init.d/1000_rc_fw_easystar.hil
fi
if param compare SYS_AUTOSTART 1001
ROMFS/px4fmu_common/mixers/FMU_Q.mix
+4 -4
View File
@@ -25,13 +25,13 @@ for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -5000 -8000 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
S: 0 0 -8000 -8000 0 -10000 10000
S: 0 1 6000 6000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -8000 -5000 0 -10000 10000
S: 0 1 -8000 -8000 0 -10000 10000
S: 0 0 -8000 -8000 0 -10000 10000
S: 0 1 -6000 -6000 0 -10000 10000
Output 2
--------
src/modules/fw_att_pos_estimator/estimator.cpp
+37 -12
View File
@@ -1,5 +1,8 @@
#include "estimator.h"
// For debugging only
#include <stdio.h>
// Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates
float KHP[n_states][n_states]; // intermediate result used for covariance updates
@@ -32,14 +35,15 @@ float posNED[3]; // North, East Down position (m)
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 innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float innovVtas; // innovation output
float varInnovVtas; // innovation variance output
float VtasMeas; // true airspeed measurement (m/s)
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
float latRef; // WGS-84 latitude of reference point (rad)
float lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
@@ -1125,6 +1129,8 @@ void FuseVelposNED()
}
}
}
//printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL"));
}
void FuseMagnetometer()
@@ -1586,13 +1592,15 @@ float sq(float valIn)
}
// Store states in a history array along with time stamp
void StoreStates()
void StoreStates(uint64_t timestamp_ms)
{
static uint8_t storeIndex = 0;
if (storeIndex == data_buffer_size) storeIndex = 0;
for (uint8_t i=0; i<=n_states; i++) storedStates[i][storeIndex] = states[i];
statetimeStamp[storeIndex] = millis();
storeIndex = storeIndex + 1;
for (unsigned i=0; i<n_states; i++)
storedStates[i][storeIndex] = states[i];
statetimeStamp[storeIndex] = timestamp_ms;
storeIndex++;
if (storeIndex == data_buffer_size)
storeIndex = 0;
}
// Output the state vector stored at the time that best matches that specified by msec
@@ -1791,8 +1799,29 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
}
void InitialiseFilter()
void InitialiseFilter(float initvelNED[3])
{
// Do the data structure init
for (unsigned i = 0; i < n_states; i++) {
for (unsigned j = 0; j < n_states; j++) {
KH[i][j] = 0.0f; // intermediate result used for covariance updates
KHP[i][j] = 0.0f; // intermediate result used for covariance updates
P[i][j] = 0.0f; // covariance matrix
}
Kfusion[i] = 0.0f; // Kalman gains
states[i] = 0.0f; // state matrix
}
for (unsigned i = 0; i < data_buffer_size; i++) {
for (unsigned j = 0; j < n_states; j++) {
}
statetimeStamp[i] = 0;
}
// Calculate initial filter quaternion states from raw measurements
float initQuat[4];
Vector3f initMagXYZ;
@@ -1809,10 +1838,6 @@ void InitialiseFilter()
initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
// calculate initial velocities
float initvelNED[3];
calcvelNED(initvelNED, gpsCourse, gpsGndSpd, gpsVelD);
//store initial lat,long and height
latRef = gpsLat;
lonRef = gpsLon;
src/modules/fw_att_pos_estimator/estimator.h
+3 -4
View File
@@ -110,7 +110,6 @@ extern float EAS2TAS; // ratio f true to equivalent airspeed
// GPS input data variables
extern float gpsCourse;
extern float gpsGndSpd;
extern float gpsVelD;
extern float gpsLat;
extern float gpsLon;
@@ -122,7 +121,7 @@ extern float baroHgt;
extern bool statesInitialised;
const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
const float covTimeStepMax = 0.02f; // maximum time allowed between covariance predictions
const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions
void UpdateStrapdownEquationsNED();
@@ -144,7 +143,7 @@ float sq(float valIn);
void quatNorm(float quatOut[4], float quatIn[4]);
// store staes along with system time stamp in msces
void StoreStates();
void StoreStates(uint64_t timestamp_ms);
// recall stste vector stored at closest time to the one specified by msec
void RecallStates(float statesForFusion[n_states], uint32_t msec);
@@ -169,7 +168,7 @@ void OnGroundCheck();
void CovarianceInit();
void InitialiseFilter();
void InitialiseFilter(float initvelNED[3]);
uint32_t millis();
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+134 -115
View File
@@ -92,7 +92,7 @@ extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]);
__EXPORT uint32_t millis();
static uint64_t last_run = 0;
static uint32_t IMUmsec = 0;
static uint64_t IMUmsec = 0;
uint32_t millis()
{
@@ -383,11 +383,11 @@ FixedwingEstimator::task_main()
/* rate limit gyro updates to 50 Hz */
/* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_gyro_sub, 17);
orb_set_interval(_gyro_sub, 6);
#else
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
/* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_sensor_combined_sub, 17);
orb_set_interval(_sensor_combined_sub, 6);
#endif
parameters_update();
@@ -459,6 +459,12 @@ FixedwingEstimator::task_main()
perf_count(_perf_gyro);
/**
* PART ONE: COLLECT ALL DATA
**/
hrt_abstime last_sensor_timestamp;
/* load local copies */
#ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
@@ -471,7 +477,7 @@ FixedwingEstimator::task_main()
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
}
last_sensor_timestamp = _gyro.timestamp;
IMUmsec = _gyro.timestamp / 1e3f;
float deltaT = (_gyro.timestamp - last_run) / 1e6f;
@@ -496,7 +502,10 @@ FixedwingEstimator::task_main()
dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
lastAngRate = angRate;
dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
// dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
dVelIMU.x = 0.0f;
dVelIMU.y = 0.0f;
dVelIMU.z = 0.0f;
lastAccel = accel;
@@ -513,14 +522,14 @@ FixedwingEstimator::task_main()
// Copy gyro and accel
last_sensor_timestamp = _sensor_combined.timestamp;
IMUmsec = _sensor_combined.timestamp / 1e3f;
float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f;
last_run = _sensor_combined.timestamp;
/* guard against too large deltaT's */
if (deltaT > 1.0f)
if (deltaT > 1.0f || deltaT < 0.000001f)
deltaT = 0.01f;
// Always store data, independent of init status
@@ -547,41 +556,13 @@ FixedwingEstimator::task_main()
#endif
if (_initialized) {
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
/* predict states and covariances */
/* run the strapdown INS every sensor update */
UpdateStrapdownEquationsNED();
/* store the predictions */
StoreStates();
/* evaluate if on ground */
OnGroundCheck();
/* prepare the delta angles and time used by the covariance prediction */
summedDelAng = summedDelAng + correctedDelAng;
summedDelVel = summedDelVel + correctedDelVel;
dt += dtIMU;
/* predict the covairance if the total delta angle has exceeded the threshold
* or the time limit will be exceeded on the next measurement update
*/
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
CovariancePrediction();
summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero();
dt = 0.0f;
}
}
bool baro_updated;
orb_check(_baro_sub, &baro_updated);
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
baroHgt = _baro.altitude;
VtasMeas = _airspeed.true_airspeed_m_s;
}
bool gps_updated;
@@ -590,65 +571,36 @@ FixedwingEstimator::task_main()
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
perf_count(_perf_gps);
if (_gps.fix_type > 2) {
if (_gps.fix_type < 3) {
gps_updated = false;
} else {
/* fuse GPS updates */
//_gps.timestamp / 1e3;
GPSstatus = _gps.fix_type;
gpsCourse = _gps.cog_rad;
gpsGndSpd = sqrtf(_gps.vel_n_m_s * _gps.vel_n_m_s + _gps.vel_e_m_s * _gps.vel_e_m_s);
gpsVelD = _gps.vel_d_m_s;
velNED[0] = _gps.vel_n_m_s;
velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s;
// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
gpsLat = math::radians(_gps.lat / (double)1e7);
gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
gpsHgt = _gps.alt / 1e3f;
if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) {
InitialiseFilter();
_initialized = true;
warnx("init done.");
continue;
}
if (_initialized) {
/* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */
velNED[0] = _gps.vel_n_m_s;
velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s;
calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
posNE[0] = posNED[0];
posNE[1] = posNED[1];
hgtMea = -posNED[2];
// set flags for further processing
fuseVelData = true;
fusePosData = true;
fuseHgtData = true;
/* recall states after adjusting for delays */
RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
/* run the actual fusion */
FuseVelposNED();
}
} else {
/* do not fuse anything, we got no position / vel update */
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
}
} else {
/* do not fuse anything, we got no position / vel update */
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
}
bool baro_updated;
orb_check(_baro_sub, &baro_updated);
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
baroHgt = _baro.altitude;
// Could use a blend of GPS and baro alt data if desired
hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt;
}
#ifndef SENSOR_COMBINED_SUB
@@ -687,41 +639,107 @@ FixedwingEstimator::task_main()
#endif
if (_initialized) {
}
fuseMagData = true;
RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms));
FuseMagnetometer();
/**
* PART TWO: EXECUTE THE FILTER
**/
if (hrt_elapsed_time(&start_time) > 500000 && !_initialized && (GPSstatus == 3)) {
InitialiseFilter(velNED);
_initialized = true;
warnx("init done.");
}
if (_initialized) {
/* predict states and covariances */
/* run the strapdown INS every sensor update */
UpdateStrapdownEquationsNED();
/* store the predictions */
StoreStates(IMUmsec);
/* evaluate if on ground */
OnGroundCheck();
/* prepare the delta angles and time used by the covariance prediction */
summedDelAng = summedDelAng + correctedDelAng;
summedDelVel = summedDelVel + correctedDelVel;
dt += dtIMU;
/* predict the covairance if the total delta angle has exceeded the threshold
* or the time limit will be exceeded on the next measurement update
*/
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
CovariancePrediction();
summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero();
dt = 0.0f;
}
}
if (gps_updated && _initialized) {
/* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */
calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
posNE[0] = posNED[0];
posNE[1] = posNED[1];
// set flags for further processing
fuseVelData = true;
fusePosData = true;
/* recall states after adjusting for delays */
RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
/* run the actual fusion */
FuseVelposNED();
} else {
fuseVelData = false;
fusePosData = false;
}
if (baro_updated && _initialized) {
fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
// run the fusion step
FuseVelposNED();
} else {
fuseHgtData = false;
}
if (mag_updated && _initialized) {
fuseMagData = true;
RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms));
} else {
fuseMagData = false;
}
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated && _initialized) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
if (_initialized) {
FuseMagnetometer();
}
if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) {
VtasMeas = _airspeed.true_airspeed_m_s;
if (_initialized) {
fuseVtasData = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
FuseAirspeed();
}
} else {
fuseVtasData = false;
}
if (airspeed_updated && _initialized
&& _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) {
fuseVtasData = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
FuseAirspeed();
} else {
fuseVtasData = false;
}
// Publish results
if (_initialized) {
// State vector:
@@ -740,7 +758,7 @@ FixedwingEstimator::task_main()
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_att.R[i][j] = R(i, j);
_att.timestamp = _gyro.timestamp;
_att.timestamp = last_sensor_timestamp;
_att.q[0] = states[0];
_att.q[1] = states[1];
_att.q[2] = states[2];
@@ -748,7 +766,7 @@ FixedwingEstimator::task_main()
_att.q_valid = true;
_att.R_valid = true;
_att.timestamp = _gyro.timestamp;
_att.timestamp = last_sensor_timestamp;
_att.roll = euler.getPhi();
_att.pitch = euler.getTheta();
_att.yaw = euler.getPsi();
@@ -771,7 +789,7 @@ FixedwingEstimator::task_main()
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
}
_local_pos.timestamp = _gyro.timestamp;
_local_pos.timestamp = last_sensor_timestamp;
_local_pos.x = states[7];
_local_pos.y = states[8];
_local_pos.z = states[9];
@@ -892,6 +910,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
printf("dt: %8.6f\n", dtIMU);
printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]);
printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]);
printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]);
src/systemcmds/mtd/mtd.c
-4
View File
@@ -257,7 +257,6 @@ mtd_start(char *partition_names[], unsigned n_partitions)
/* Now create MTD FLASH partitions */
warnx("Creating partitions");
FAR struct mtd_dev_s *part[n_partitions];
char blockname[32];
@@ -266,9 +265,6 @@ mtd_start(char *partition_names[], unsigned n_partitions)
for (offset = 0, i = 0; i < n_partitions; offset += nblocks, i++) {
warnx(" Partition %d. Block offset=%lu, size=%lu",
i, (unsigned long)offset, (unsigned long)nblocks);
/* Create the partition */
part[i] = mtd_partition(mtd_dev, offset, nblocks);
src/systemcmds/param/param.c
+1 -1
View File
@@ -320,7 +320,7 @@ do_set(const char* name, const char* val)
char* end;
f = strtod(val,&end);
param_set(param, &f);
printf(" -> new: %f\n", f);
printf(" -> new: %4.4f\n", (double)f);
}