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)
{
_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");
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) {

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),
#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
};

2
libraries/AP_Compass/Compass.h
View File

@ -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