diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-02 16:59:11 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-02 16:59:11 +0100 |
commit | f0740df53ae0f1c79f2f1298611a7a8c7e0ae8e7 (patch) | |
tree | 31af6e847c8e2e4b91d25037a90e7f2f480ff7d3 /src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | |
parent | 3d68c37d3cb6eb0d177df5e90e6365cf27a5b483 (diff) | |
download | px4-firmware-f0740df53ae0f1c79f2f1298611a7a8c7e0ae8e7.tar.gz px4-firmware-f0740df53ae0f1c79f2f1298611a7a8c7e0ae8e7.tar.bz2 px4-firmware-f0740df53ae0f1c79f2f1298611a7a8c7e0ae8e7.zip |
Set new filter as default in HIL and fixed wing, fixes in estimator, WIP
Diffstat (limited to 'src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp')
-rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 76 |
1 files changed, 44 insertions, 32 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 b359348ed..adf679d7e 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 @@ -334,6 +334,7 @@ FixedwingEstimator::task_main() _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -349,6 +350,12 @@ FixedwingEstimator::task_main() Vector3f lastAngRate; Vector3f lastAccel; + /* set initial filter state */ + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; /* wakeup source(s) */ struct pollfd fds[2]; @@ -359,6 +366,8 @@ FixedwingEstimator::task_main() fds[1].fd = _gyro_sub; fds[1].events = POLLIN; + hrt_abstime start_time = hrt_absolute_time(); + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -376,9 +385,6 @@ FixedwingEstimator::task_main() perf_begin(_loop_perf); - /* check vehicle status for changes to publication state */ - vehicle_status_poll(); - /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ @@ -392,7 +398,8 @@ FixedwingEstimator::task_main() /* only run estimator if gyro updated */ if (fds[1].revents & POLLIN) { - + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); /* load local copies */ orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -407,12 +414,10 @@ FixedwingEstimator::task_main() if (deltaT > 1.0f) deltaT = 0.01f; - dt = deltaT; - if (_initialized) { /* fill in last data set */ - dtIMU = dt; + dtIMU = deltaT; angRate.x = _gyro.x; angRate.y = _gyro.y; @@ -456,7 +461,6 @@ FixedwingEstimator::task_main() bool gps_updated; orb_check(_gps_sub, &gps_updated); - if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); @@ -474,8 +478,10 @@ FixedwingEstimator::task_main() newDataGps = true; - if (!_initialized) { + if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { InitialiseFilter(); + _initialized = true; + warnx("init done."); continue; } @@ -487,7 +493,7 @@ FixedwingEstimator::task_main() posNE[1] = posNED[1]; hgtMea = -posNED[2]; - /* set flags for further processing */ + // set flags for further processing fuseVelData = true; fusePosData = true; fuseHgtData = true; @@ -507,7 +513,7 @@ FixedwingEstimator::task_main() fusePosData = false; fuseHgtData = false; - newDataGps = true; + newDataGps = false; } } else { @@ -517,8 +523,7 @@ FixedwingEstimator::task_main() bool mag_updated; orb_check(_mag_sub, &mag_updated); - - if (mag_updated) { + if (mag_updated && _initialized) { orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); if (_initialized) { @@ -543,41 +548,48 @@ FixedwingEstimator::task_main() bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { + if (airspeed_updated && _initialized) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - if (_initialized && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { VtasMeas = _airspeed.true_airspeed_m_s; fuseVtasData = true; RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data FuseAirspeed(); + } else { + fuseVtasData = false; } } else { fuseVtasData = false; } - /* lazily publish the attitude only once available */ - if (_att_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + if (_initialized) { - } else { - /* advertise and publish */ - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); - } + math::EulerAngles euler(eulerEst[0], eulerEst[1], eulerEst[2]); + //warnx("est: %8.4f, %8.4f, %8.4f", eulerEst[0], eulerEst[1], eulerEst[2]); - /* lazily publish the position only once available */ - if (_global_pos_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + // /* lazily publish the attitude only once available */ + // if (_att_pub > 0) { + // /* publish the attitude setpoint */ + // orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); - } else { - /* advertise and publish */ - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + // } else { + // /* advertise and publish */ + // _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + // } + + // /* lazily publish the position only once available */ + // if (_global_pos_pub > 0) { + // /* publish the attitude setpoint */ + // orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + + // } else { + // /* advertise and publish */ + // _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + // } } } @@ -599,7 +611,7 @@ FixedwingEstimator::start() /* start the task */ _estimator_task = task_spawn_cmd("fw_att_pos_estimator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_MAX - 40, 12048, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); |