2020-12-28 22:29:40 -04:00
|
|
|
#pragma once
|
|
|
|
|
|
|
|
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
|
|
|
|
|
|
|
|
#if HAL_EXTERNAL_AHRS_ENABLED
|
|
|
|
|
|
|
|
#include "AP_InertialSensor.h"
|
|
|
|
#include "AP_InertialSensor_Backend.h"
|
|
|
|
|
|
|
|
class AP_InertialSensor_ExternalAHRS : public AP_InertialSensor_Backend
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
AP_InertialSensor_ExternalAHRS(AP_InertialSensor &imu, uint8_t serial_port);
|
|
|
|
|
|
|
|
/* update accel and gyro state */
|
|
|
|
bool update() override;
|
|
|
|
void start() override;
|
|
|
|
void accumulate() override;
|
|
|
|
|
|
|
|
void handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt) override;
|
2022-12-21 11:52:22 -04:00
|
|
|
bool get_output_banner(char* banner, uint8_t banner_len) override;
|
2020-12-28 22:29:40 -04:00
|
|
|
|
|
|
|
private:
|
|
|
|
uint8_t gyro_instance;
|
|
|
|
uint8_t accel_instance;
|
|
|
|
const uint8_t serial_port;
|
2021-03-17 22:36:41 -03:00
|
|
|
bool started;
|
2020-12-28 22:29:40 -04:00
|
|
|
};
|
|
|
|
#endif // HAL_EXTERNAL_AHRS_ENABLED
|
|
|
|
|