AP_Compass: allow override of IST8310 orientation

this allows for vendor GPS modules and internal compass which use a
different orientation
This commit is contained in:
Andrew Tridgell 2023-08-02 16:39:34 +10:00
parent 5af7fdf330
commit e1fad5ee38
2 changed files with 5 additions and 3 deletions

View File

@ -1176,12 +1176,10 @@ void Compass::_probe_external_i2c_compasses(void)
// IST8310 on external and internal bus // IST8310 on external and internal bus
if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV5 && if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV5 &&
AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV6) { AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV6) {
enum Rotation default_rotation; enum Rotation default_rotation = AP_COMPASS_IST8310_DEFAULT_ROTATION;
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_AEROFC) { if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_AEROFC) {
default_rotation = ROTATION_PITCH_180_YAW_90; default_rotation = ROTATION_PITCH_180_YAW_90;
} else {
default_rotation = ROTATION_PITCH_180;
} }
// probe all 4 possible addresses // probe all 4 possible addresses
const uint8_t ist8310_addr[] = { 0x0C, 0x0D, 0x0E, 0x0F }; const uint8_t ist8310_addr[] = { 0x0C, 0x0D, 0x0E, 0x0F };

View File

@ -31,6 +31,10 @@
#define HAL_COMPASS_IST8310_I2C_ADDR 0x0E #define HAL_COMPASS_IST8310_I2C_ADDR 0x0E
#endif #endif
#ifndef AP_COMPASS_IST8310_DEFAULT_ROTATION
#define AP_COMPASS_IST8310_DEFAULT_ROTATION ROTATION_PITCH_180
#endif
class AP_Compass_IST8310 : public AP_Compass_Backend class AP_Compass_IST8310 : public AP_Compass_Backend
{ {
public: public: