You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
115 lines
3.2 KiB
115 lines
3.2 KiB
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
#ifndef AP_Compass_AK8963_H |
|
#define AP_Compass_AK8963_H |
|
|
|
#include <AP_HAL.h> |
|
#include "../AP_Common/AP_Common.h" |
|
#include "../AP_Math/AP_Math.h" |
|
|
|
#include "Compass.h" |
|
|
|
class AK8963_Backend |
|
{ |
|
public: |
|
virtual void read(uint8_t address, uint8_t *buf, uint32_t count) = 0; |
|
virtual void write(uint8_t address, const uint8_t *buf, uint32_t count) = 0; |
|
virtual bool sem_take_nonblocking() = 0; |
|
virtual bool sem_take_blocking() = 0; |
|
virtual bool sem_give() = 0; |
|
virtual uint8_t read(uint8_t address) |
|
{ |
|
uint8_t value; |
|
read(address, &value, 1); |
|
return value; |
|
} |
|
|
|
virtual void write(uint8_t address, uint8_t value) |
|
{ |
|
write(address, &value, 1); |
|
} |
|
}; |
|
|
|
class AP_Compass_AK8963 : public Compass |
|
{ |
|
private: |
|
typedef enum |
|
{ |
|
CONVERSION, |
|
SAMPLE, |
|
ERROR |
|
} state_t; |
|
|
|
virtual bool _backend_init() = 0; |
|
virtual void _register_read(uint8_t address, uint8_t count, uint8_t *value) = 0; |
|
virtual void _register_write(uint8_t address, uint8_t value) = 0; |
|
virtual void _backend_reset() = 0; |
|
virtual bool _read_raw() = 0; |
|
virtual uint8_t _read_id() = 0; |
|
virtual void _dump_registers() {} |
|
|
|
bool _register_read(uint8_t address, uint8_t *value); |
|
bool _calibrate(); |
|
bool _self_test(); |
|
void _update(); |
|
void _start_conversion(); |
|
void _collect_samples(); |
|
|
|
float _mag_x_accum; |
|
float _mag_y_accum; |
|
float _mag_z_accum; |
|
uint32_t _accum_count; |
|
|
|
bool _initialised; |
|
state_t _state; |
|
uint8_t _magnetometer_adc_resolution; |
|
uint32_t _last_update_timestamp; |
|
uint32_t _last_accum_time; |
|
|
|
protected: |
|
float magnetometer_ASA[3]; |
|
float _mag_x; |
|
float _mag_y; |
|
float _mag_z; |
|
|
|
AK8963_Backend *_backend; |
|
|
|
public: |
|
AP_Compass_AK8963(); |
|
|
|
virtual bool init(void); |
|
virtual bool read(void); |
|
virtual void accumulate(void); |
|
|
|
}; |
|
|
|
class AK8963_MPU9250_SPI_Backend: public AK8963_Backend |
|
{ |
|
public: |
|
AK8963_MPU9250_SPI_Backend(); |
|
void read(uint8_t address, uint8_t *buf, uint32_t count); |
|
void write(uint8_t address, const uint8_t *buf, uint32_t count); |
|
bool sem_take_nonblocking(); |
|
bool sem_take_blocking(); |
|
bool sem_give(); |
|
|
|
private: |
|
AP_HAL::SPIDeviceDriver *_spi; |
|
AP_HAL::Semaphore *_spi_sem; |
|
}; |
|
|
|
class AP_Compass_AK8963_MPU9250: public AP_Compass_AK8963 |
|
{ |
|
public: |
|
AP_Compass_AK8963_MPU9250(); |
|
bool init(); |
|
private: |
|
bool _backend_init(); |
|
void _backend_reset(); |
|
void _register_read(uint8_t address, uint8_t count, uint8_t *value); |
|
void _register_write(uint8_t address, uint8_t value); |
|
void _dump_registers(); |
|
bool _read_raw(); |
|
uint8_t _read_id(); |
|
}; |
|
|
|
#endif
|
|
|