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

Pure code style formatting

Browse Source
This commit is contained in:
Lorenz Meier
2014-02-18 08:24:30 +01:00
parent 350acde509
commit 7e9fcac057
1 changed files with 142 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+142 -136
View File
@@ -129,9 +129,9 @@ private:
int _gyro_sub; /**< gyro sensor subscription */ int _gyro_sub; /**< gyro sensor subscription */
int _accel_sub; /**< accel sensor subscription */ int _accel_sub; /**< accel sensor subscription */
int _mag_sub; /**< mag sensor subscription */ int _mag_sub; /**< mag sensor subscription */
#else #else
int _sensor_combined_sub; int _sensor_combined_sub;
#endif #endif
int _airspeed_sub; /**< airspeed subscription */ int _airspeed_sub; /**< airspeed subscription */
int _baro_sub; /**< barometer subscription */ int _baro_sub; /**< barometer subscription */
int _gps_sub; /**< GPS subscription */ int _gps_sub; /**< GPS subscription */
@@ -233,13 +233,13 @@ FixedwingEstimator::FixedwingEstimator() :
_estimator_task(-1), _estimator_task(-1),
/* subscriptions */ /* subscriptions */
#ifndef SENSOR_COMBINED_SUB #ifndef SENSOR_COMBINED_SUB
_gyro_sub(-1), _gyro_sub(-1),
_accel_sub(-1), _accel_sub(-1),
_mag_sub(-1), _mag_sub(-1),
#else #else
_sensor_combined_sub(-1), _sensor_combined_sub(-1),
#endif #endif
_airspeed_sub(-1), _airspeed_sub(-1),
_baro_sub(-1), _baro_sub(-1),
_gps_sub(-1), _gps_sub(-1),
@@ -382,7 +382,7 @@ FixedwingEstimator::task_main()
/* rate limit vehicle status updates to 5Hz */ /* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200); orb_set_interval(_vstatus_sub, 200);
#ifndef SENSOR_COMBINED_SUB #ifndef SENSOR_COMBINED_SUB
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel));
@@ -391,11 +391,11 @@ FixedwingEstimator::task_main()
/* rate limit gyro updates to 50 Hz */ /* rate limit gyro updates to 50 Hz */
/* XXX remove this!, BUT increase the data buffer size! */ /* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_gyro_sub, 4); orb_set_interval(_gyro_sub, 4);
#else #else
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
/* XXX remove this!, BUT increase the data buffer size! */ /* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_sensor_combined_sub, 4); orb_set_interval(_sensor_combined_sub, 4);
#endif #endif
parameters_update(); parameters_update();
@@ -424,13 +424,13 @@ FixedwingEstimator::task_main()
/* Setup of loop */ /* Setup of loop */
fds[0].fd = _params_sub; fds[0].fd = _params_sub;
fds[0].events = POLLIN; fds[0].events = POLLIN;
#ifndef SENSOR_COMBINED_SUB #ifndef SENSOR_COMBINED_SUB
fds[1].fd = _gyro_sub; fds[1].fd = _gyro_sub;
fds[1].events = POLLIN; fds[1].events = POLLIN;
#else #else
fds[1].fd = _sensor_combined_sub; fds[1].fd = _sensor_combined_sub;
fds[1].events = POLLIN; fds[1].events = POLLIN;
#endif #endif
hrt_abstime start_time = hrt_absolute_time(); hrt_abstime start_time = hrt_absolute_time();
@@ -483,9 +483,9 @@ FixedwingEstimator::task_main()
hrt_abstime last_sensor_timestamp; hrt_abstime last_sensor_timestamp;
/* load local copies */ /* load local copies */
#ifndef SENSOR_COMBINED_SUB #ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
orb_check(_accel_sub, &accel_updated); orb_check(_accel_sub, &accel_updated);
@@ -523,7 +523,7 @@ FixedwingEstimator::task_main()
lastAccel = accel; lastAccel = accel;
#else #else
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
static hrt_abstime last_accel = 0; static hrt_abstime last_accel = 0;
@@ -532,6 +532,7 @@ FixedwingEstimator::task_main()
if (last_accel != _sensor_combined.accelerometer_timestamp) { if (last_accel != _sensor_combined.accelerometer_timestamp) {
accel_updated = true; accel_updated = true;
} }
last_accel = _sensor_combined.accelerometer_timestamp; last_accel = _sensor_combined.accelerometer_timestamp;
@@ -566,27 +567,32 @@ FixedwingEstimator::task_main()
if (last_mag != _sensor_combined.magnetometer_timestamp) { if (last_mag != _sensor_combined.magnetometer_timestamp) {
mag_updated = true; mag_updated = true;
newDataMag = true; newDataMag = true;
} else { } else {
newDataMag = false; newDataMag = false;
} }
last_mag = _sensor_combined.magnetometer_timestamp; last_mag = _sensor_combined.magnetometer_timestamp;
#endif #endif
bool airspeed_updated; bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated); orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) { if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed); perf_count(_perf_airspeed);
VtasMeas = _airspeed.true_airspeed_m_s; VtasMeas = _airspeed.true_airspeed_m_s;
newAdsData = true; newAdsData = true;
} else { } else {
newAdsData = false; newAdsData = false;
} }
bool gps_updated; bool gps_updated;
orb_check(_gps_sub, &gps_updated); orb_check(_gps_sub, &gps_updated);
if (gps_updated) { if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
perf_count(_perf_gps); perf_count(_perf_gps);
@@ -594,6 +600,7 @@ FixedwingEstimator::task_main()
if (_gps.fix_type < 3) { if (_gps.fix_type < 3) {
gps_updated = false; gps_updated = false;
newDataGps = false; newDataGps = false;
} else { } else {
/* fuse GPS updates */ /* fuse GPS updates */
@@ -616,23 +623,25 @@ FixedwingEstimator::task_main()
bool baro_updated; bool baro_updated;
orb_check(_baro_sub, &baro_updated); orb_check(_baro_sub, &baro_updated);
if (baro_updated) { if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
baroHgt = _baro.altitude; baroHgt = _baro.altitude;
// Could use a blend of GPS and baro alt data if desired // Could use a blend of GPS and baro alt data if desired
hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt; hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt;
} }
#ifndef SENSOR_COMBINED_SUB #ifndef SENSOR_COMBINED_SUB
orb_check(_mag_sub, &mag_updated); orb_check(_mag_sub, &mag_updated);
#endif #endif
if (mag_updated) { if (mag_updated) {
perf_count(_perf_mag); perf_count(_perf_mag);
#ifndef SENSOR_COMBINED_SUB #ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
// XXX we compensate the offsets upfront - should be close to zero. // XXX we compensate the offsets upfront - should be close to zero.
@@ -645,8 +654,8 @@ FixedwingEstimator::task_main()
magData.z = _mag.z; magData.z = _mag.z;
magBias.z = 0.000001f; // _mag_offsets.y_offset magBias.z = 0.000001f; // _mag_offsets.y_offset
#else #else
// XXX we compensate the offsets upfront - should be close to zero. // XXX we compensate the offsets upfront - should be close to zero.
// 0.001f // 0.001f
@@ -659,7 +668,7 @@ FixedwingEstimator::task_main()
magData.z = _sensor_combined.magnetometer_ga[2]; magData.z = _sensor_combined.magnetometer_ga[2];
magBias.z = 0.000001f; // _mag_offsets.y_offset magBias.z = 0.000001f; // _mag_offsets.y_offset
#endif #endif
newDataMag = true; newDataMag = true;
@@ -672,114 +681,111 @@ FixedwingEstimator::task_main()
* PART TWO: EXECUTE THE FILTER * PART TWO: EXECUTE THE FILTER
**/ **/
if (/*(IMUmsec > msecAlignTime) &&*/ !statesInitialised && (GPSstatus == 3)) if (/*(IMUmsec > msecAlignTime) &&*/ !statesInitialised && (GPSstatus == 3)) {
{ velNED[0] = _gps.vel_n_m_s;
velNED[0] = _gps.vel_n_m_s; velNED[1] = _gps.vel_e_m_s;
velNED[1] = _gps.vel_e_m_s; velNED[2] = _gps.vel_d_m_s;
velNED[2] = _gps.vel_d_m_s; InitialiseFilter(velNED);
InitialiseFilter(velNED); }
}
// If valid IMU data and states initialised, predict states and covariances // If valid IMU data and states initialised, predict states and covariances
if (statesInitialised) if (statesInitialised) {
{ // Run the strapdown INS equations every IMU update
// Run the strapdown INS equations every IMU update UpdateStrapdownEquationsNED();
UpdateStrapdownEquationsNED(); #if 0
#if 0 // debug code - could be tunred into a filter mnitoring/watchdog function
// debug code - could be tunred into a filter mnitoring/watchdog function float tempQuat[4];
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
StoreStates(IMUmsec);
// Check if on ground - status is used by covariance prediction
OnGroundCheck();
onGround = false;
// sum delta angles and time used by covariance prediction
summedDelAng = summedDelAng + correctedDelAng;
summedDelVel = summedDelVel + dVelIMU;
dt += dtIMU;
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax))
{
CovariancePrediction(dt);
summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero();
dt = 0.0f;
}
_initialized = true; for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
}
// Fuse GPS Measurements quat2eul(eulerEst, tempQuat);
if (newDataGps && statesInitialised)
{
// Convert GPS measurements to Pos NE, hgt and Vel 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]; for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
posNE[1] = posNED[1];
// set fusion flags
fuseVelData = true;
fusePosData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay));
RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay));
// run the fusion step
FuseVelposNED();
}
else
{
fuseVelData = false;
fusePosData = false;
}
if (newAdsData && statesInitialised) if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
{
// Could use a blend of GPS and baro alt data if desired
hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt;
fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
// run the fusion step
FuseVelposNED();
}
else
{
fuseHgtData = false;
}
// Fuse Magnetometer Measurements if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
if (newDataMag && statesInitialised)
{
fuseMagData = true;
RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); // Assume 50 msec avg delay for magnetometer data
}
else
{
fuseMagData = false;
}
if (statesInitialised) FuseMagnetometer();
// Fuse Airspeed Measurements #endif
if (newAdsData && statesInitialised && VtasMeas > 8.0f) // store the predicted states for subsequent use by measurement fusion
{ StoreStates(IMUmsec);
fuseVtasData = true; // Check if on ground - status is used by covariance prediction
RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data OnGroundCheck();
FuseAirspeed(); onGround = false;
} // sum delta angles and time used by covariance prediction
else summedDelAng = summedDelAng + correctedDelAng;
{ summedDelVel = summedDelVel + dVelIMU;
fuseVtasData = false; dt += dtIMU;
}
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
CovariancePrediction(dt);
summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero();
dt = 0.0f;
}
_initialized = true;
}
// Fuse GPS Measurements
if (newDataGps && statesInitialised) {
// Convert GPS measurements to Pos NE, hgt and Vel 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];
// set fusion flags
fuseVelData = true;
fusePosData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay));
RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay));
// run the fusion step
FuseVelposNED();
} else {
fuseVelData = false;
fusePosData = false;
}
if (newAdsData && statesInitialised) {
// Could use a blend of GPS and baro alt data if desired
hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt;
fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
// run the fusion step
FuseVelposNED();
} else {
fuseHgtData = false;
}
// Fuse Magnetometer Measurements
if (newDataMag && statesInitialised) {
fuseMagData = true;
RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); // Assume 50 msec avg delay for magnetometer data
} else {
fuseMagData = false;
}
if (statesInitialised) FuseMagnetometer();
// Fuse Airspeed Measurements
if (newAdsData && statesInitialised && VtasMeas > 8.0f) {
fuseVtasData = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data
FuseAirspeed();
} else {
fuseVtasData = false;
}
// if (hrt_elapsed_time(&start_time) > 100000 && !_initialized && (GPSstatus == 3)) { // if (hrt_elapsed_time(&start_time) > 100000 && !_initialized && (GPSstatus == 3)) {
// InitialiseFilter(velNED); // InitialiseFilter(velNED);
@@ -805,7 +811,7 @@ FixedwingEstimator::task_main()
// /* prepare the delta angles and time used by the covariance prediction */ // /* prepare the delta angles and time used by the covariance prediction */
// summedDelAng = summedDelAng + correctedDelAng; // summedDelAng = summedDelAng + correctedDelAng;
// summedDelVel = summedDelVel + correctedDelVel; // summedDelVel = summedDelVel + correctedDelVel;
// dt += dtIMU; // dt += dtIMU;
// /* predict the covairance if the total delta angle has exceeded the threshold // /* predict the covairance if the total delta angle has exceeded the threshold
@@ -829,7 +835,7 @@ FixedwingEstimator::task_main()
// posNE[0] = posNED[0]; // posNE[0] = posNED[0];
// posNE[1] = posNED[1]; // posNE[1] = posNED[1];
// // set flags for further processing // // set flags for further processing
// fuseVelData = true; // fuseVelData = true;
// fusePosData = true; // fusePosData = true;
@@ -896,7 +902,7 @@ FixedwingEstimator::task_main()
math::Vector<3> euler = R.to_euler(); math::Vector<3> euler = R.to_euler();
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_att.R[i][j] = R(i, j); _att.R[i][j] = R(i, j);
_att.timestamp = last_sensor_timestamp; _att.timestamp = last_sensor_timestamp;
_att.q[0] = states[0]; _att.q[0] = states[0];
@@ -1005,7 +1011,7 @@ void print_status()
math::Vector<3> euler = R.to_euler(); math::Vector<3> euler = R.to_euler();
printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n",
(double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));
// State vector: // State vector:
// 0-3: quaternions (q0, q1, q2, q3) // 0-3: quaternions (q0, q1, q2, q3)
@@ -1027,15 +1033,15 @@ void print_status()
printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]);
printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]);
printf("states: %s %s %s %s %s %s %s %s %s\n", printf("states: %s %s %s %s %s %s %s %s %s\n",
(statesInitialised) ? "INITIALIZED" : "NON_INIT", (statesInitialised) ? "INITIALIZED" : "NON_INIT",
(onGround) ? "ON_GROUND" : "AIRBORNE", (onGround) ? "ON_GROUND" : "AIRBORNE",
(fuseVelData) ? "FUSE_VEL" : "INH_VEL", (fuseVelData) ? "FUSE_VEL" : "INH_VEL",
(fusePosData) ? "FUSE_POS" : "INH_POS", (fusePosData) ? "FUSE_POS" : "INH_POS",
(fuseHgtData) ? "FUSE_HGT" : "INH_HGT", (fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
(fuseMagData) ? "FUSE_MAG" : "INH_MAG", (fuseMagData) ? "FUSE_MAG" : "INH_MAG",
(fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
(useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
(useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); (useCompass) ? "USE_COMPASS" : "IGN_COMPASS");
} }
int fw_att_pos_estimator_main(int argc, char *argv[]) int fw_att_pos_estimator_main(int argc, char *argv[])