Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: support 3 mags on PX4

This commit is contained in:
Andrew Tridgell 2014-07-04 12:07:47 +10:00
parent d462d91533
commit c138244155
3 changed files with 16 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -43,18 +43,20 @@ extern const AP_HAL::HAL& hal;
bool AP_Compass_PX4::init(void) bool AP_Compass_PX4::init(void)
{ {
_mag_fd[0] = open(MAG_DEVICE_PATH, O_RDONLY); _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<COMPASS_MAX_INSTANCES; i++) {
if (_mag_fd[i] >= 0) {
_num_instances = i+1;
}
}
if (_num_instances == 0) {
hal.console->printf("Unable to open " MAG_DEVICE_PATH "\n"); hal.console->printf("Unable to open " MAG_DEVICE_PATH "\n");
return false; 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++) { for (uint8_t i=0; i<_num_instances; i++) {
// average over up to 20 samples // average over up to 20 samples
if (ioctl(_mag_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) { if (ioctl(_mag_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) {

5
libraries/AP_Compass/Compass.cpp
View File

@ -110,6 +110,11 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
AP_GROUPINFO("PRIMARY", 12, Compass, _primary, 0), AP_GROUPINFO("PRIMARY", 12, Compass, _primary, 0),
#endif #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 AP_GROUPEND
}; };

2
libraries/AP_Compass/Compass.h
View File

@ -45,7 +45,7 @@
than 1 then redundent sensors may be available than 1 then redundent sensors may be available
*/ */
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#define COMPASS_MAX_INSTANCES 2 #define COMPASS_MAX_INSTANCES 3
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#define COMPASS_MAX_INSTANCES 2 #define COMPASS_MAX_INSTANCES 2
#else #else