Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 17:48:35 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: Allow EKF to pull data from range finder object

Browse Source
This commit is contained in:
Paul Riseborough 2015-04-16 11:50:40 +10:00 committed by Randy Mackay
parent 133b8f5ad7
commit 32f87d008a
1 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/ArduPlane.pde
View File

@ -189,6 +189,7 @@ static int32_t pitch_limit_min_cd;
// GPS driver // GPS driver
static AP_GPS gps; static AP_GPS gps;
static RangeFinder rng;
// flight modes convenience array // flight modes convenience array
static AP_Int8 *flight_modes = &g.flight_mode1; static AP_Int8 *flight_modes = &g.flight_mode1;
@ -205,7 +206,7 @@ AP_InertialSensor ins;
// Inertial Navigation EKF // Inertial Navigation EKF
#if AP_AHRS_NAVEKF_AVAILABLE #if AP_AHRS_NAVEKF_AVAILABLE
AP_AHRS_NavEKF ahrs(ins, barometer, gps); AP_AHRS_NavEKF ahrs(ins, barometer, gps, rng);
#else #else
AP_AHRS_DCM ahrs(ins, barometer, gps); AP_AHRS_DCM ahrs(ins, barometer, gps);
#endif #endif
@ -1622,9 +1623,7 @@ static void update_optical_flow(void)
uint8_t flowQuality = optflow.quality(); uint8_t flowQuality = optflow.quality();
Vector2f flowRate = optflow.flowRate(); Vector2f flowRate = optflow.flowRate();
Vector2f bodyRate = optflow.bodyRate(); Vector2f bodyRate = optflow.bodyRate();
// Use range from a separate range finder if available, not the PX4Flows built in sensor which is ineffective ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update);
float ground_distance_m = 0.01f*rangefinder.distance_cm();
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, rangefinder_state.in_range_count, ground_distance_m);
Log_Write_Optflow(); Log_Write_Optflow();
} }
} }