AP_OpticalFlow_MAV.h 1.3 KB

12345678910111213141516171819202122232425262728293031323334
  1. #pragma once
  2. #include "OpticalFlow.h"
  3. #include <AP_HAL/utility/OwnPtr.h>
  4. class AP_OpticalFlow_MAV : public OpticalFlow_backend
  5. {
  6. public:
  7. /// constructor
  8. using OpticalFlow_backend::OpticalFlow_backend;
  9. // initialise the sensor
  10. void init() override {}
  11. // read latest values from sensor and fill in x,y and totals.
  12. void update(void) override;
  13. // get update from mavlink
  14. void handle_msg(const mavlink_message_t &msg) override;
  15. // detect if the sensor is available
  16. static AP_OpticalFlow_MAV *detect(OpticalFlow &_frontend);
  17. private:
  18. uint64_t prev_frame_us; // system time of last message when update was last called
  19. uint64_t latest_frame_us; // system time of most recent messages processed
  20. Vector2l flow_sum; // sum of sensor's flow_x and flow_y values since last call to update
  21. uint16_t quality_sum; // sum of sensor's quality values since last call to update
  22. uint16_t count; // number of sensor readings since last call to update
  23. uint8_t sensor_id; // sensor_id received in latest mavlink message
  24. Vector2f gyro_sum; // sum of gyro sensor values since last frame from flow sensor
  25. uint16_t gyro_sum_count; // number of gyro sensor values in sum
  26. };