mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_OpticalFlow: rename for AHRS restructuring
This commit is contained in:
parent
48e9fa7ebd
commit
ed5303d1b8
@ -98,7 +98,7 @@ void AP_OpticalFlow_CXOF::update(void)
|
|||||||
// record gyro values as long as they are being used
|
// record gyro values as long as they are being used
|
||||||
// the sanity check of dt below ensures old gyro values are not used
|
// the sanity check of dt below ensures old gyro values are not used
|
||||||
if (gyro_sum_count < 1000) {
|
if (gyro_sum_count < 1000) {
|
||||||
const Vector3f& gyro = AP::ahrs_navekf().get_gyro();
|
const Vector3f& gyro = AP::ahrs().get_gyro();
|
||||||
gyro_sum.x += gyro.x;
|
gyro_sum.x += gyro.x;
|
||||||
gyro_sum.y += gyro.y;
|
gyro_sum.y += gyro.y;
|
||||||
gyro_sum_count++;
|
gyro_sum_count++;
|
||||||
|
@ -285,7 +285,7 @@ void AP_OpticalFlow_Pixart::timer(void)
|
|||||||
|
|
||||||
uint32_t dt_us = last_burst_us - integral.last_frame_us;
|
uint32_t dt_us = last_burst_us - integral.last_frame_us;
|
||||||
float dt = dt_us * 1.0e-6;
|
float dt = dt_us * 1.0e-6;
|
||||||
const Vector3f &gyro = AP::ahrs_navekf().get_gyro();
|
const Vector3f &gyro = AP::ahrs().get_gyro();
|
||||||
|
|
||||||
{
|
{
|
||||||
WITH_SEMAPHORE(_sem);
|
WITH_SEMAPHORE(_sem);
|
||||||
|
@ -100,7 +100,7 @@ void AP_OpticalFlow_UPFLOW::update(void)
|
|||||||
gyro_sum.zero();
|
gyro_sum.zero();
|
||||||
gyro_sum_count = 0;
|
gyro_sum_count = 0;
|
||||||
}
|
}
|
||||||
const Vector3f& gyro = AP::ahrs_navekf().get_gyro();
|
const Vector3f& gyro = AP::ahrs().get_gyro();
|
||||||
gyro_sum.x += gyro.x;
|
gyro_sum.x += gyro.x;
|
||||||
gyro_sum.y += gyro.y;
|
gyro_sum.y += gyro.y;
|
||||||
gyro_sum_count++;
|
gyro_sum_count++;
|
||||||
|
@ -203,11 +203,11 @@ void OpticalFlow::update_state(const OpticalFlow_state &state)
|
|||||||
_last_update_ms = AP_HAL::millis();
|
_last_update_ms = AP_HAL::millis();
|
||||||
|
|
||||||
// write to log and send to EKF if new data has arrived
|
// write to log and send to EKF if new data has arrived
|
||||||
AP::ahrs_navekf().writeOptFlowMeas(quality(),
|
AP::ahrs().writeOptFlowMeas(quality(),
|
||||||
_state.flowRate,
|
_state.flowRate,
|
||||||
_state.bodyRate,
|
_state.bodyRate,
|
||||||
_last_update_ms,
|
_last_update_ms,
|
||||||
get_pos_offset());
|
get_pos_offset());
|
||||||
Log_Write_Optflow();
|
Log_Write_Optflow();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -30,7 +30,7 @@
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
class OpticalFlow_backend;
|
class OpticalFlow_backend;
|
||||||
class AP_AHRS_NavEKF;
|
class AP_AHRS;
|
||||||
|
|
||||||
class OpticalFlow
|
class OpticalFlow
|
||||||
{
|
{
|
||||||
|
@ -27,7 +27,7 @@ public:
|
|||||||
AP_InertialSensor ins;
|
AP_InertialSensor ins;
|
||||||
AP_SerialManager serial_manager;
|
AP_SerialManager serial_manager;
|
||||||
RangeFinder sonar;
|
RangeFinder sonar;
|
||||||
AP_AHRS_NavEKF ahrs{AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
|
AP_AHRS ahrs{AP_AHRS::FLAG_ALWAYS_USE_EKF};
|
||||||
};
|
};
|
||||||
|
|
||||||
static DummyVehicle vehicle;
|
static DummyVehicle vehicle;
|
||||||
|
Loading…
Reference in New Issue
Block a user