From 6b931a2738f4e6100fbd3cb7bdc168489c4f8b85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Feb 2014 08:58:36 +0100 Subject: [PATCH 1/3] Testing: Autostart fake GPS in HIL, to be REMOVED --- ROMFS/px4fmu_common/init.d/rcS | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6f4e1f3b56..2a3941e594 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -386,6 +386,7 @@ then then sleep 1 mavlink start -b 230400 -d /dev/ttyACM0 + gps start -f usleep 5000 else if [ $TTYS1_BUSY == yes ] From 2fd5c9d277d8c78a19e38e2a44c9d91c6612aed7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Feb 2014 09:04:51 +0100 Subject: [PATCH 2/3] Removed some debugging, added other, still WIP --- src/modules/fw_att_pos_estimator/estimator.h | 2 +- .../fw_att_pos_estimator_main.cpp | 107 ++++++++++-------- 2 files changed, 60 insertions(+), 49 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 01a1ace996..ce3050cbe6 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -121,7 +121,7 @@ extern float baroHgt; extern bool statesInitialised; -const float covTimeStepMax = 0.02f; // maximum time allowed between covariance predictions +const float covTimeStepMax = 0.2f; // maximum time allowed between covariance predictions const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions void UpdateStrapdownEquationsNED(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 71df13d274..28e547cbe2 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -99,6 +99,8 @@ uint32_t millis() return IMUmsec; } +static void print_status(); + class FixedwingEstimator { public: @@ -383,11 +385,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, 6); + orb_set_interval(_gyro_sub, 4); #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, 6); + orb_set_interval(_sensor_combined_sub, 4); #endif parameters_update(); @@ -502,10 +504,7 @@ FixedwingEstimator::task_main() dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; lastAngRate = angRate; - // dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; - dVelIMU.x = 0.0f; - dVelIMU.y = 0.0f; - dVelIMU.z = 0.0f; + dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; lastAccel = accel; @@ -585,7 +584,7 @@ FixedwingEstimator::task_main() // 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; + gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; gpsHgt = _gps.alt / 1e3f; } @@ -616,26 +615,26 @@ FixedwingEstimator::task_main() // XXX we compensate the offsets upfront - should be close to zero. // 0.001f magData.x = _mag.x; - magBias.x = 0.0001f; // _mag_offsets.x_offset + magBias.x = 0.000001f; // _mag_offsets.x_offset magData.y = _mag.y; - magBias.y = 0.0001f; // _mag_offsets.y_offset + magBias.y = 0.000001f; // _mag_offsets.y_offset magData.z = _mag.z; - magBias.z = 0.0001f; // _mag_offsets.y_offset + magBias.z = 0.000001f; // _mag_offsets.y_offset #else // XXX we compensate the offsets upfront - should be close to zero. // 0.001f magData.x = _sensor_combined.magnetometer_ga[0]; - magBias.x = 0.0001f; // _mag_offsets.x_offset + magBias.x = 0.000001f; // _mag_offsets.x_offset magData.y = _sensor_combined.magnetometer_ga[1]; - magBias.y = 0.0001f; // _mag_offsets.y_offset + magBias.y = 0.000001f; // _mag_offsets.y_offset magData.z = _sensor_combined.magnetometer_ga[2]; - magBias.z = 0.0001f; // _mag_offsets.y_offset + magBias.z = 0.000001f; // _mag_offsets.y_offset #endif @@ -664,11 +663,13 @@ FixedwingEstimator::task_main() StoreStates(IMUmsec); /* evaluate if on ground */ - OnGroundCheck(); + // OnGroundCheck(); + onGround = false; /* 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 @@ -680,6 +681,7 @@ FixedwingEstimator::task_main() summedDelVel = summedDelVel.zero(); dt = 0.0f; } + } @@ -730,7 +732,7 @@ FixedwingEstimator::task_main() } if (airspeed_updated && _initialized - && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + && _airspeed.true_airspeed_m_s > 6.0f /* XXX magic number */) { fuseVtasData = true; RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data @@ -742,6 +744,8 @@ FixedwingEstimator::task_main() // Publish results if (_initialized) { + + // State vector: // 0-3: quaternions (q0, q1, q2, q3) // 4-6: Velocity - m/sec (North, East, Down) @@ -858,6 +862,45 @@ FixedwingEstimator::start() return OK; } +void print_status() +{ + math::Quaternion q(states[0], states[1], states[2], states[3]); + math::EulerAngles euler(q); + + printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", + (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); + + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + + printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", dtIMU, dt, (int)IMUmsec); + printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)dVelIMU.x, (double)dVelIMU.y, (double)dVelIMU.z, (double)accel.x, (double)accel.y, (double)accel.z); + printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)dAngIMU.x, (double)dAngIMU.y, (double)dAngIMU.z, (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); + 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]); + printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); + 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: %s %s %s %s %s %s %s %s %s\n", + (statesInitialised) ? "INITIALIZED" : "NON_INIT", + (onGround) ? "ON_GROUND" : "AIRBORNE", + (fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (fusePosData) ? "FUSE_POS" : "INH_POS", + (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); +} + int fw_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 1) @@ -895,39 +938,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) if (estimator::g_estimator) { warnx("running"); - math::Quaternion q(states[0], states[1], states[2], states[3]); - math::EulerAngles euler(q); - - printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", - (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 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]); - printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); - 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: %s %s %s %s %s %s %s %s %s", - (statesInitialised) ? "INITIALIZED" : "NON_INIT", - (onGround) ? "ON_GROUND" : "AIRBORNE", - (fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (fusePosData) ? "FUSE_POS" : "INH_POS", - (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); + print_status(); exit(0); From d572424996124b1f2dafdcbd48baf1abc85ee627 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Feb 2014 09:06:47 +0100 Subject: [PATCH 3/3] Build fix hackery --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 9c98a49bbf..b32b3686fd 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -357,6 +357,8 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) estimator::g_estimator->task_main(); } +static float dt = 0.0f; // time lapsed since last covariance prediction + void FixedwingEstimator::task_main() { @@ -407,8 +409,6 @@ FixedwingEstimator::task_main() Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; - float dt = 0.0f; // time lapsed since last covariance prediction - /* wakeup source(s) */ struct pollfd fds[2];