mirror of https://github.com/ArduPilot/ardupilot
libraries: fixed examples for no flash_leds() callback
This commit is contained in:
parent
f8e9d48a76
commit
e1aa6e3ff1
|
@ -35,15 +35,11 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||||
// INS and Baro declaration
|
// INS and Baro declaration
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||||
|
|
||||||
#define A_LED_PIN 27
|
|
||||||
#define C_LED_PIN 25
|
|
||||||
AP_InertialSensor_MPU6000 ins;
|
AP_InertialSensor_MPU6000 ins;
|
||||||
AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);
|
AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);
|
||||||
|
|
||||||
#else
|
#else
|
||||||
|
|
||||||
#define A_LED_PIN 37
|
|
||||||
#define C_LED_PIN 35
|
|
||||||
AP_ADC_ADS7844 adc;
|
AP_ADC_ADS7844 adc;
|
||||||
AP_InertialSensor_Oilpan ins(&adc);
|
AP_InertialSensor_Oilpan ins(&adc);
|
||||||
AP_Baro_BMP085 baro;
|
AP_Baro_BMP085 baro;
|
||||||
|
|
|
@ -33,15 +33,11 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||||
// INS and Baro declaration
|
// INS and Baro declaration
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||||
|
|
||||||
#define A_LED_PIN 27
|
|
||||||
#define C_LED_PIN 25
|
|
||||||
AP_InertialSensor_MPU6000 ins;
|
AP_InertialSensor_MPU6000 ins;
|
||||||
AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);
|
AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);
|
||||||
|
|
||||||
#else
|
#else
|
||||||
|
|
||||||
#define A_LED_PIN 37
|
|
||||||
#define C_LED_PIN 35
|
|
||||||
AP_ADC_ADS7844 adc;
|
AP_ADC_ADS7844 adc;
|
||||||
AP_InertialSensor_Oilpan ins(&adc);
|
AP_InertialSensor_Oilpan ins(&adc);
|
||||||
AP_Baro_BMP085 baro;
|
AP_Baro_BMP085 baro;
|
||||||
|
|
|
@ -32,15 +32,11 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||||
// INS and Baro declaration
|
// INS and Baro declaration
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||||
|
|
||||||
#define A_LED_PIN 27
|
|
||||||
#define C_LED_PIN 25
|
|
||||||
AP_InertialSensor_MPU6000 ins;
|
AP_InertialSensor_MPU6000 ins;
|
||||||
AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);
|
AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);
|
||||||
|
|
||||||
#else
|
#else
|
||||||
|
|
||||||
#define A_LED_PIN 37
|
|
||||||
#define C_LED_PIN 35
|
|
||||||
AP_ADC_ADS7844 adc;
|
AP_ADC_ADS7844 adc;
|
||||||
AP_InertialSensor_Oilpan ins(&adc);
|
AP_InertialSensor_Oilpan ins(&adc);
|
||||||
AP_Baro_BMP085 baro;
|
AP_Baro_BMP085 baro;
|
||||||
|
|
|
@ -57,25 +57,6 @@ AP_Baro_HIL barometer;
|
||||||
#define HIGH 1
|
#define HIGH 1
|
||||||
#define LOW 0
|
#define LOW 0
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
|
||||||
# define A_LED_PIN 27
|
|
||||||
# define C_LED_PIN 25
|
|
||||||
# define LED_ON LOW
|
|
||||||
# define LED_OFF HIGH
|
|
||||||
#else
|
|
||||||
# define A_LED_PIN 37
|
|
||||||
# define C_LED_PIN 35
|
|
||||||
# define LED_ON HIGH
|
|
||||||
# define LED_OFF LOW
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
static void flash_leds(bool on)
|
|
||||||
{
|
|
||||||
hal.gpio->write(A_LED_PIN, on ? LED_OFF : LED_ON);
|
|
||||||
hal.gpio->write(C_LED_PIN, on ? LED_ON : LED_OFF);
|
|
||||||
}
|
|
||||||
|
|
||||||
void setup(void)
|
void setup(void)
|
||||||
{
|
{
|
||||||
|
|
||||||
|
@ -86,9 +67,8 @@ void setup(void)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
ins.init(AP_InertialSensor::COLD_START,
|
ins.init(AP_InertialSensor::COLD_START,
|
||||||
AP_InertialSensor::RATE_100HZ,
|
AP_InertialSensor::RATE_100HZ);
|
||||||
flash_leds);
|
ins.init_accel();
|
||||||
ins.init_accel(flash_leds);
|
|
||||||
|
|
||||||
ahrs.init();
|
ahrs.init();
|
||||||
|
|
||||||
|
|
|
@ -30,15 +30,11 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||||
|
|
||||||
#define A_LED_PIN 27
|
|
||||||
#define C_LED_PIN 25
|
|
||||||
AP_InertialSensor_MPU6000 ins;
|
AP_InertialSensor_MPU6000 ins;
|
||||||
AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);
|
AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);
|
||||||
|
|
||||||
#else
|
#else
|
||||||
|
|
||||||
#define A_LED_PIN 37
|
|
||||||
#define C_LED_PIN 35
|
|
||||||
AP_ADC_ADS7844 adc;
|
AP_ADC_ADS7844 adc;
|
||||||
AP_InertialSensor_Oilpan ins(&adc);
|
AP_InertialSensor_Oilpan ins(&adc);
|
||||||
AP_Baro_BMP085 baro;
|
AP_Baro_BMP085 baro;
|
||||||
|
@ -54,23 +50,15 @@ AP_InertialNav inertialnav(&ahrs, &ins, &baro, &gps);
|
||||||
|
|
||||||
uint32_t last_update;
|
uint32_t last_update;
|
||||||
|
|
||||||
static void flash_leds(bool on) {
|
|
||||||
hal.gpio->write(A_LED_PIN, on);
|
|
||||||
hal.gpio->write(C_LED_PIN, ~on);
|
|
||||||
}
|
|
||||||
|
|
||||||
void setup(void)
|
void setup(void)
|
||||||
{
|
{
|
||||||
hal.console->println_P(PSTR("AP_InertialNav test startup..."));
|
hal.console->println_P(PSTR("AP_InertialNav test startup..."));
|
||||||
hal.gpio->pinMode(A_LED_PIN, GPIO_OUTPUT);
|
|
||||||
hal.gpio->pinMode(C_LED_PIN, GPIO_OUTPUT);
|
|
||||||
|
|
||||||
gps = &auto_gps;
|
gps = &auto_gps;
|
||||||
gps->init(hal.uartB, GPS::GPS_ENGINE_AIRBORNE_2G);
|
gps->init(hal.uartB, GPS::GPS_ENGINE_AIRBORNE_2G);
|
||||||
|
|
||||||
ins.init(AP_InertialSensor::COLD_START,
|
ins.init(AP_InertialSensor::COLD_START,
|
||||||
AP_InertialSensor::RATE_100HZ,
|
AP_InertialSensor::RATE_100HZ);
|
||||||
flash_leds);
|
|
||||||
|
|
||||||
ahrs.set_compass(&compass);
|
ahrs.set_compass(&compass);
|
||||||
|
|
||||||
|
|
|
@ -32,8 +32,7 @@ void setup(void)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
ins.init(AP_InertialSensor::COLD_START,
|
ins.init(AP_InertialSensor::COLD_START,
|
||||||
AP_InertialSensor::RATE_100HZ,
|
AP_InertialSensor::RATE_100HZ);
|
||||||
NULL);
|
|
||||||
|
|
||||||
// display initial values
|
// display initial values
|
||||||
display_offsets_and_scaling();
|
display_offsets_and_scaling();
|
||||||
|
@ -97,7 +96,7 @@ void run_calibration()
|
||||||
|
|
||||||
#if !defined( __AVR_ATmega1280__ )
|
#if !defined( __AVR_ATmega1280__ )
|
||||||
AP_InertialSensor_UserInteractStream interact(hal.console);
|
AP_InertialSensor_UserInteractStream interact(hal.console);
|
||||||
ins.calibrate_accel(NULL, &interact, roll_trim, pitch_trim);
|
ins.calibrate_accel(&interact, roll_trim, pitch_trim);
|
||||||
#else
|
#else
|
||||||
hal.console->println_P(PSTR("calibrate_accel not available on 1280"));
|
hal.console->println_P(PSTR("calibrate_accel not available on 1280"));
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue