Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Allow EKF to pull data from range finder object

Browse Source
This commit is contained in:
Paul Riseborough 2015-04-16 11:31:31 +10:00 committed by Randy Mackay
parent f21aa17980
commit 133b8f5ad7
2 changed files with 9 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduCopter/ArduCopter.pde
View File

@ -261,9 +261,16 @@ static Compass compass;
AP_InertialSensor ins;
////////////////////////////////////////////////////////////////////////////////
// SONAR
#if CONFIG_SONAR == ENABLED
static RangeFinder sonar;
static bool sonar_enabled = true; // enable user switch for sonar
#endif
// Inertial Navigation EKF
#if AP_AHRS_NAVEKF_AVAILABLE
AP_AHRS_NavEKF ahrs(ins, barometer, gps);
AP_AHRS_NavEKF ahrs(ins, barometer, gps, sonar);
#else
AP_AHRS_DCM ahrs(ins, barometer, gps);
#endif
@ -298,13 +305,6 @@ static AP_SerialManager serial_manager;
static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
////////////////////////////////////////////////////////////////////////////////
// SONAR
#if CONFIG_SONAR == ENABLED
static RangeFinder sonar;
static bool sonar_enabled = true; // enable user switch for sonar
#endif
////////////////////////////////////////////////////////////////////////////////
// User variables
////////////////////////////////////////////////////////////////////////////////

4
ArduCopter/sensors.pde
View File

@ -115,9 +115,7 @@ static void update_optical_flow(void)
uint8_t flowQuality = optflow.quality();
Vector2f flowRate = optflow.flowRate();
Vector2f bodyRate = optflow.bodyRate();
// Use range from a separate range finder if available, not the PX4Flows built in sensor which is ineffective
float ground_distance_m = 0.01f*float(sonar_alt);
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, sonar_alt_health, ground_distance_m);
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update);
if (g.log_bitmask & MASK_LOG_OPTFLOW) {
Log_Write_Optflow();
}