ArduCopter: changes to use CONFIG_HIL_BOARD

This commit is contained in:
Pat Hickey 2012-12-13 15:27:42 -08:00 committed by Andrew Tridgell
parent f060df9747
commit 2aa4657315
7 changed files with 42 additions and 49 deletions

View File

@ -1,14 +1,11 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// Example config file. Take a look at config.h. Any term define there can be overridden by defining it here.
// Example config file. Take a look at config.h. Any term define there can be
// overridden by defining it here.
//#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
// Ordinary users should please ignore the following define.
// APM2_BETA_HARDWARE is used to support early (September-October 2011) APM2
// hardware which had the BMP085 barometer onboard. Only a handful of
// developers have these boards.
//#define APM2_BETA_HARDWARE
// If you used to define your CONFIG_APM_HARDWARE setting here, it is no longer
// valid! You should switch to using a HAL_BOARD flag in your local config.mk.
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
//#define HIL_MODE HIL_MODE_ATTITUDE

View File

@ -109,9 +109,20 @@
#include "Parameters.h"
#include "GCS.h"
////////////////////////////////////////////////////////////////////////////////
// cliSerial
////////////////////////////////////////////////////////////////////////////////
// cliSerial isn't strictly necessary - it is an alias for hal.console. It may
// be deprecated in favor of hal.console in later releases.
AP_HAL::BetterStream* cliSerial;
// N.B. we need to keep a static declaration which isn't guarded by macros
// at the top to cooperate with the prototype mangler.
////////////////////////////////////////////////////////////////////////////////
// AP_HAL instance
////////////////////////////////////////////////////////////////////////////////
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1
@ -122,7 +133,6 @@ const AP_HAL::HAL& hal = AP_HAL_AVR_SITL;
SITL sitl;
#endif
AP_HAL::BetterStream* cliSerial;
////////////////////////////////////////////////////////////////////////////////
// Parameters

View File

@ -18,8 +18,11 @@ hilnocli:
heli:
make -f Makefile EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME"
apm1:
make -f Makefile HAL_BOARD=HAL_BOARD_APM1
apm2:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2"
make -f Makefile HAL_BOARD=HAL_BOARD_APM2
dmp:
make -f Makefile EXTRAFLAGS="-DDMP_ENABLED=ENABLED"

View File

@ -41,19 +41,16 @@
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// APM HARDWARE
//
#ifndef CONFIG_APM_HARDWARE
# define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
#ifdef CONFIG_APM_HARDWARE
#error CONFIG_APM_HARDWARE option is deprecated! use CONFIG_HAL_BOARD instead
#endif
//////////////////////////////////////////////////////////////////////////////
// APM2 HARDWARE DEFAULTS
//
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
# define CONFIG_IMU_TYPE CONFIG_IMU_MPU6000
# define CONFIG_PUSHBUTTON DISABLED
# define CONFIG_RELAY DISABLED
@ -134,7 +131,7 @@
////////////////////////////////////////////////////////
// LED and IO Pins
//
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
# define A_LED_PIN 37
# define B_LED_PIN 36
# define C_LED_PIN 35
@ -147,7 +144,7 @@
# define OPTFLOW_CS_PIN 34
# define BATTERY_VOLT_PIN 0 // Battery voltage on A0
# define BATTERY_CURR_PIN 1 // Battery current on A1
#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
# define A_LED_PIN 27
# define B_LED_PIN 26
# define C_LED_PIN 25
@ -157,7 +154,7 @@
# define PUSHBUTTON_PIN (-1)
# define CLI_SLIDER_ENABLED DISABLED
# define USB_MUX_PIN 23
# define OPTFLOW_CS_PIN A3
# define OPTFLOW_CS_PIN 57 // Optflow CS on A3
# define BATTERY_VOLT_PIN 1 // Battery voltage on A1
# define BATTERY_CURR_PIN 2 // Battery current on A2
#endif
@ -173,7 +170,7 @@
#define COPTER_LED_ON HIGH
#define COPTER_LED_OFF LOW
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
#define COPTER_LED_1 AN4 // Motor or Aux LED
#define COPTER_LED_2 AN5 // Motor LED or Beeper
#define COPTER_LED_3 AN6 // Motor or GPS LED
@ -182,7 +179,7 @@
#define COPTER_LED_6 AN9 // Motor LED
#define COPTER_LED_7 AN10 // Motor LED
#define COPTER_LED_8 AN11 // Motor LED
#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1
#define COPTER_LED_1 AN8 // Motor or Aux LED
#define COPTER_LED_2 AN9 // Motor LED
#define COPTER_LED_3 AN10 // Motor or GPS LED
@ -235,7 +232,7 @@
# endif
#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
# ifndef CONFIG_SONAR_SOURCE_ANALOG_PIN
# define CONFIG_SONAR_SOURCE_ANALOG_PIN A0
# define CONFIG_SONAR_SOURCE_ANALOG_PIN 0
# endif
#else
# warning Invalid value for CONFIG_SONAR_SOURCE, disabling sonar

View File

@ -411,10 +411,6 @@ enum gcs_severity {
#define CONFIG_IMU_OILPAN 1
#define CONFIG_IMU_MPU6000 2
// APM Hardware selection
#define APM_HARDWARE_APM1 1
#define APM_HARDWARE_APM2 2
#define AP_BARO_BMP085 1
#define AP_BARO_MS5611 2

View File

@ -207,7 +207,7 @@ static void copter_leds_on(void) {
if ( !bitRead(g.copter_leds_mode, 2) ) {
digitalWriteFast(COPTER_LED_1, COPTER_LED_ON);
}
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
if ( !bitRead(g.copter_leds_mode, 3) ) {
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
}
@ -228,7 +228,7 @@ static void copter_leds_off(void) {
if ( !bitRead(g.copter_leds_mode, 2) ) {
digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF);
}
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
if ( !bitRead(g.copter_leds_mode, 3) ) {
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
}
@ -275,7 +275,7 @@ static void copter_leds_oscillate(void) {
if ( !bitRead(g.copter_leds_mode, 2) ) {
digitalWriteFast(COPTER_LED_1, COPTER_LED_ON);
}
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
if ( !bitRead(g.copter_leds_mode, 3) ) {
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
}
@ -294,7 +294,7 @@ static void copter_leds_oscillate(void) {
if ( !bitRead(g.copter_leds_mode, 2) ) {
digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF);
}
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
if ( !bitRead(g.copter_leds_mode, 3) ) {
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
}

View File

@ -149,11 +149,16 @@ static void init_ardupilot()
// init the GCS
gcs0.init(hal.uartA);
// Register the mavlink service callback. This will run
// anytime there are more than 5ms remaining in a call to
// hal.scheduler->delay.
hal.scheduler->register_delay_callback(mavlink_delay_cb, 5);
#if USB_MUX_PIN > 0
if (!ap_system.usb_connected) {
// we are not connected via USB, re-init UART0 with right
// baud rate
cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
}
#else
// we have a 2nd serial port for telemetry
@ -603,9 +608,9 @@ static void check_usb_mux(void)
// the user has switched to/from the telemetry port
ap_system.usb_connected = usb_check;
if (ap_system.usb_connected) {
cliSerial->begin(SERIAL0_BAUD);
hal.uartA->begin(SERIAL0_BAUD);
} else {
cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
}
}
#endif
@ -633,23 +638,8 @@ uint16_t board_voltage(void)
/*
force a software reset of the APM
*/
static void reboot_apm(void)
{
cliSerial->printf_P(PSTR("REBOOTING\n"));
delay(100); // let serial flush
// see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1250663814/
// for the method
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
// this relies on the bootloader resetting the watchdog, which
// APM1 doesn't do
cli();
wdt_enable(WDTO_15MS);
#else
// this works on APM1
void (*fn)(void) = NULL;
fn();
#endif
while (1);
static void reboot_apm(void) {
hal.scheduler->reboot();
}
//