|
|
|
@ -719,7 +719,7 @@ FixedwingEstimator::task_main()
@@ -719,7 +719,7 @@ FixedwingEstimator::task_main()
|
|
|
|
|
* do subscriptions |
|
|
|
|
*/ |
|
|
|
|
_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder)); |
|
|
|
|
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); |
|
|
|
|
_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); |
|
|
|
|
_airspeed_sub = orb_subscribe(ORB_ID(airspeed)); |
|
|
|
|
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); |
|
|
|
|
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); |
|
|
|
@ -1087,7 +1087,7 @@ FixedwingEstimator::task_main()
@@ -1087,7 +1087,7 @@ FixedwingEstimator::task_main()
|
|
|
|
|
|
|
|
|
|
if (baro_updated) { |
|
|
|
|
|
|
|
|
|
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); |
|
|
|
|
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); |
|
|
|
|
|
|
|
|
|
float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; |
|
|
|
|
baro_last = _baro.timestamp; |
|
|
|
@ -1216,7 +1216,7 @@ FixedwingEstimator::task_main()
@@ -1216,7 +1216,7 @@ FixedwingEstimator::task_main()
|
|
|
|
|
initVelNED[2] = _gps.vel_d_m_s; |
|
|
|
|
|
|
|
|
|
// Set up height correctly
|
|
|
|
|
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); |
|
|
|
|
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); |
|
|
|
|
_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
|
|
|
|
|
|
|
|
|
|
// init filtered gps and baro altitudes
|
|
|
|
|