|
|
|
@ -38,6 +38,8 @@
@@ -38,6 +38,8 @@
|
|
|
|
|
#include <AP_HAL_Empty.h> |
|
|
|
|
#include <AP_HAL_PX4.h> |
|
|
|
|
#include <AP_BattMonitor.h> |
|
|
|
|
#include <AP_SerialManager.h> |
|
|
|
|
#include <RC_Channel.h> |
|
|
|
|
|
|
|
|
|
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; |
|
|
|
|
|
|
|
|
@ -52,6 +54,7 @@ AP_Compass_HMC5843 compass;
@@ -52,6 +54,7 @@ AP_Compass_HMC5843 compass;
|
|
|
|
|
|
|
|
|
|
AP_GPS gps; |
|
|
|
|
AP_Baro baro; |
|
|
|
|
AP_SerialManager serial_manager; |
|
|
|
|
|
|
|
|
|
// choose which AHRS system to use |
|
|
|
|
AP_AHRS_DCM ahrs(ins, baro, gps); |
|
|
|
@ -75,6 +78,7 @@ void setup(void)
@@ -75,6 +78,7 @@ void setup(void)
|
|
|
|
|
ins.init_accel(); |
|
|
|
|
|
|
|
|
|
ahrs.init(); |
|
|
|
|
serial_manager.init(); |
|
|
|
|
|
|
|
|
|
if( compass.init() ) { |
|
|
|
|
hal.console->printf("Enabling compass\n"); |
|
|
|
@ -82,7 +86,7 @@ void setup(void)
@@ -82,7 +86,7 @@ void setup(void)
|
|
|
|
|
} else { |
|
|
|
|
hal.console->printf("No compass detected\n"); |
|
|
|
|
} |
|
|
|
|
gps.init(NULL); |
|
|
|
|
gps.init(NULL, serial_manager); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void loop(void) |
|
|
|
|