2e464a53c2
The switching between different AP_HAL was happening by giving different definitions of AP_HAL_BOARD_DRIVER, and the programs would use it to instantiate. A program or library code would have to explicitly include (and depend) on the concrete implementation of the HAL, even when using it only via interface. The proposed change move this dependency to be link time. There is a AP_HAL::get_HAL() function that is used by the client code. Each implementation of HAL provides its own definition of this function, returning the appropriate concrete instance. Since this replaces the job of AP_HAL_BOARD_DRIVER, the definition was removed. The static variables for PX4 and VRBRAIN were named differently to avoid shadowing the extern symbol 'hal'.
136 lines
3.5 KiB
C++
136 lines
3.5 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
//
|
|
// Simple test for the AP_AHRS interface
|
|
//
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Progmem/AP_Progmem.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_Param/AP_Param.h>
|
|
#include <AP_InertialSensor/AP_InertialSensor.h>
|
|
#include <AP_ADC/AP_ADC.h>
|
|
#include <AP_ADC_AnalogSource/AP_ADC_AnalogSource.h>
|
|
#include <AP_Baro/AP_Baro.h> // ArduPilot Mega Barometer Library
|
|
#include <AP_GPS/AP_GPS.h>
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#include <AP_Compass/AP_Compass.h>
|
|
#include <AP_Declination/AP_Declination.h>
|
|
#include <AP_Airspeed/AP_Airspeed.h>
|
|
#include <AP_Baro/AP_Baro.h>
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
|
#include <AP_Mission/AP_Mission.h>
|
|
#include <StorageManager/StorageManager.h>
|
|
#include <AP_Terrain/AP_Terrain.h>
|
|
#include <Filter/Filter.h>
|
|
#include <SITL/SITL.h>
|
|
#include <AP_Buffer/AP_Buffer.h>
|
|
#include <AP_Notify/AP_Notify.h>
|
|
#include <AP_Vehicle/AP_Vehicle.h>
|
|
#include <DataFlash/DataFlash.h>
|
|
#include <AP_NavEKF/AP_NavEKF.h>
|
|
#include <AP_Rally/AP_Rally.h>
|
|
#include <AP_Scheduler/AP_Scheduler.h>
|
|
|
|
#include <AP_HAL_AVR/AP_HAL_AVR.h>
|
|
#include <AP_HAL_SITL/AP_HAL_SITL.h>
|
|
#include <AP_HAL_Empty/AP_HAL_Empty.h>
|
|
#include <AP_HAL_Linux/AP_HAL_Linux.h>
|
|
#include <AP_HAL_PX4/AP_HAL_PX4.h>
|
|
#include <AP_BattMonitor/AP_BattMonitor.h>
|
|
#include <AP_SerialManager/AP_SerialManager.h>
|
|
#include <RC_Channel/RC_Channel.h>
|
|
#include <AP_RangeFinder/AP_RangeFinder.h>
|
|
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
|
|
|
// INS and Baro declaration
|
|
AP_InertialSensor ins;
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
|
|
AP_ADC_ADS7844 apm1_adc;
|
|
#endif
|
|
|
|
Compass compass;
|
|
|
|
AP_GPS gps;
|
|
AP_Baro baro;
|
|
AP_SerialManager serial_manager;
|
|
|
|
// choose which AHRS system to use
|
|
AP_AHRS_DCM ahrs(ins, baro, gps);
|
|
|
|
|
|
|
|
#define HIGH 1
|
|
#define LOW 0
|
|
|
|
void setup(void)
|
|
{
|
|
|
|
#ifdef APM2_HARDWARE
|
|
// we need to stop the barometer from holding the SPI bus
|
|
hal.gpio->pinMode(40, HAL_HAL_GPIO_OUTPUT);
|
|
hal.gpio->write(40, HIGH);
|
|
#endif
|
|
|
|
ins.init(AP_InertialSensor::RATE_100HZ);
|
|
ahrs.init();
|
|
serial_manager.init();
|
|
|
|
if( compass.init() ) {
|
|
hal.console->printf("Enabling compass\n");
|
|
ahrs.set_compass(&compass);
|
|
} else {
|
|
hal.console->printf("No compass detected\n");
|
|
}
|
|
gps.init(NULL, serial_manager);
|
|
}
|
|
|
|
void loop(void)
|
|
{
|
|
static uint16_t counter;
|
|
static uint32_t last_t, last_print, last_compass;
|
|
uint32_t now = hal.scheduler->micros();
|
|
float heading = 0;
|
|
|
|
if (last_t == 0) {
|
|
last_t = now;
|
|
return;
|
|
}
|
|
last_t = now;
|
|
|
|
if (now - last_compass > 100*1000UL &&
|
|
compass.read()) {
|
|
heading = compass.calculate_heading(ahrs.get_dcm_matrix());
|
|
// read compass at 10Hz
|
|
last_compass = now;
|
|
#if WITH_GPS
|
|
g_gps->update();
|
|
#endif
|
|
}
|
|
|
|
ahrs.update();
|
|
counter++;
|
|
|
|
if (now - last_print >= 100000 /* 100ms : 10hz */) {
|
|
Vector3f drift = ahrs.get_gyro_drift();
|
|
hal.console->printf_P(
|
|
PSTR("r:%4.1f p:%4.1f y:%4.1f "
|
|
"drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n"),
|
|
ToDeg(ahrs.roll),
|
|
ToDeg(ahrs.pitch),
|
|
ToDeg(ahrs.yaw),
|
|
ToDeg(drift.x),
|
|
ToDeg(drift.y),
|
|
ToDeg(drift.z),
|
|
compass.use_for_yaw() ? ToDeg(heading) : 0.0f,
|
|
(1.0e6f*counter)/(now-last_print));
|
|
last_print = now;
|
|
counter = 0;
|
|
}
|
|
}
|
|
|
|
AP_HAL_MAIN();
|