diff --git a/libraries/AP_Compass/AP_Compass_PX4.cpp b/libraries/AP_Compass/AP_Compass_PX4.cpp index baaac55188..9696cd57fe 100644 --- a/libraries/AP_Compass/AP_Compass_PX4.cpp +++ b/libraries/AP_Compass/AP_Compass_PX4.cpp @@ -43,18 +43,20 @@ extern const AP_HAL::HAL& hal; bool AP_Compass_PX4::init(void) { _mag_fd[0] = open(MAG_DEVICE_PATH, O_RDONLY); - if (_mag_fd[0] < 0) { + _mag_fd[1] = open(MAG_DEVICE_PATH "1", O_RDONLY); + _mag_fd[2] = open(MAG_DEVICE_PATH "2", O_RDONLY); + + _num_instances = 0; + for (uint8_t i=0; i= 0) { + _num_instances = i+1; + } + } + if (_num_instances == 0) { hal.console->printf("Unable to open " MAG_DEVICE_PATH "\n"); return false; } - _mag_fd[1] = open(MAG_DEVICE_PATH "1", O_RDONLY); - if (_mag_fd[1] >= 0) { - _num_instances = 2; - } else { - _num_instances = 1; - } - for (uint8_t i=0; i<_num_instances; i++) { // average over up to 20 samples if (ioctl(_mag_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) { diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index a7120384cf..79f3fc48c0 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -110,6 +110,11 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { AP_GROUPINFO("PRIMARY", 12, Compass, _primary, 0), #endif +#if COMPASS_MAX_INSTANCES > 2 + AP_GROUPINFO("OFS3", 13, Compass, _offset[2], 0), + AP_GROUPINFO("MOT3", 14, Compass, _motor_compensation[2], 0), +#endif + AP_GROUPEND }; diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 8a0d040cf1..971db7883b 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -45,7 +45,7 @@ than 1 then redundent sensors may be available */ #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 -#define COMPASS_MAX_INSTANCES 2 +#define COMPASS_MAX_INSTANCES 3 #elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #define COMPASS_MAX_INSTANCES 2 #else