ardupilot/libraries/APO/APO_DefaultSetup.h

193 lines
6.5 KiB
C
Raw Normal View History

2011-09-29 15:16:36 -03:00
#ifndef _APO_COMMON_H
#define _APO_COMMON_H
FastSerialPort0(Serial);
FastSerialPort1(Serial1);
FastSerialPort2(Serial2);
FastSerialPort3(Serial3);
/*
* Required Global Declarations
*/
static apo::AP_Autopilot * autoPilot;
void setup() {
2011-10-26 13:31:11 -03:00
using namespace apo;
2011-12-06 19:56:16 -04:00
Wire.begin();
2011-10-26 13:31:11 -03:00
// Declare all parts of the system
AP_Navigator * navigator = NULL;
AP_Guide * guide = NULL;
AP_Controller * controller = NULL;
AP_HardwareAbstractionLayer * hal = NULL;
/*/
2011-10-26 13:31:11 -03:00
* Communications
*/
Serial.begin(debugBaud, 128, 128); // debug
//// hardware abstraction layer
2011-11-29 14:59:44 -04:00
hal = new AP_HardwareAbstractionLayer(halMode, board, vehicle);
2011-10-26 13:31:11 -03:00
//// debug serial
2011-10-26 13:31:11 -03:00
hal->debug = &Serial;
hal->debug->println_P(PSTR("initializing debug line"));
/*
* Sensor initialization
*/
if (hal->getMode() == MODE_LIVE) {
if (batteryMonitorEnabled) {
hal->batteryMonitor = new AP_BatteryMonitor(batteryPin,batteryVoltageDivRatio,batteryMinVolt,batteryMaxVolt);
}
if (gpsEnabled) {
Serial1.begin(gpsBaud, 128, 16); // gps
hal->debug->println_P(PSTR("initializing gps"));
AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps));
hal->gps = &gpsDriver;
hal->gps->callback = delay;
hal->gps->init();
}
if (baroEnabled) {
hal->debug->println_P(PSTR("initializing baro"));
hal->baro = new BARO_CLASS;
2011-12-06 19:56:16 -04:00
if (board==BOARD_ARDUPILOTMEGA_2) {
hal->baro->Init(0,true);
} else {
hal->baro->Init(0,false);
}
2011-10-26 13:31:11 -03:00
}
if (compassEnabled) {
hal->debug->println_P(PSTR("initializing compass"));
hal->compass = new COMPASS_CLASS;
hal->compass->set_orientation(compassOrientation);
hal->compass->set_offsets(0,0,0);
hal->compass->set_declination(0.0);
hal->compass->init();
}
/**
* Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not
* initialize them and NULL will be assigned to those corresponding pointers.
* On detecting NU/LL assigned to any ultrasonic sensor, its corresponding block of code
2011-10-26 13:31:11 -03:00
* will not be executed by the navigator.
* The coordinate system is assigned by the right hand rule with the thumb pointing down.
* In set_orientation, it is defined as (front/back,left/right,down,up)
*/
if (rangeFinderFrontEnabled) {
hal->debug->println_P(PSTR("initializing front range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(new AP_AnalogSource_Arduino(1),new ModeFilter);
2011-10-26 13:31:11 -03:00
rangeFinder->set_orientation(1, 0, 0);
hal->rangeFinders.push_back(rangeFinder);
}
if (rangeFinderBackEnabled) {
hal->debug->println_P(PSTR("initializing back range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(new AP_AnalogSource_Arduino(2),new ModeFilter);
2011-10-26 13:31:11 -03:00
rangeFinder->set_orientation(-1, 0, 0);
hal->rangeFinders.push_back(rangeFinder);
}
if (rangeFinderLeftEnabled) {
hal->debug->println_P(PSTR("initializing left range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(new AP_AnalogSource_Arduino(3),new ModeFilter);
2011-10-26 13:31:11 -03:00
rangeFinder->set_orientation(0, -1, 0);
hal->rangeFinders.push_back(rangeFinder);
}
if (rangeFinderRightEnabled) {
hal->debug->println_P(PSTR("initializing right range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(new AP_AnalogSource_Arduino(4),new ModeFilter);
2011-10-26 13:31:11 -03:00
rangeFinder->set_orientation(0, 1, 0);
hal->rangeFinders.push_back(rangeFinder);
}
if (rangeFinderUpEnabled) {
hal->debug->println_P(PSTR("initializing up range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(new AP_AnalogSource_Arduino(5),new ModeFilter);
2011-10-26 13:31:11 -03:00
rangeFinder->set_orientation(0, 0, -1);
hal->rangeFinders.push_back(rangeFinder);
}
if (rangeFinderDownEnabled) {
hal->debug->println_P(PSTR("initializing down range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(new AP_AnalogSource_Arduino(6),new ModeFilter);
2011-10-26 13:31:11 -03:00
rangeFinder->set_orientation(0, 0, 1);
hal->rangeFinders.push_back(rangeFinder);
}
/*
* navigation sensors
*/
hal->debug->println_P(PSTR("initializing imu"));
hal->ins = new AP_InertialSensor_Oilpan(hal->adc);
hal->ins->init(hal->scheduler);
//hal->ins = new AP_InertialSensor_MPU6000(mpu6000SelectPin)
hal->debug->println_P(PSTR("initializing ins"));
delay(2000);
hal->imu = new AP_IMU_INS(hal->ins, k_sensorCalib);
hal->imu->init(IMU::WARM_START,delay,hal->scheduler);
hal->debug->println_P(PSTR("setup completed"));
2011-10-26 13:31:11 -03:00
}
/*
* Select guidance, navigation, control algorithms
*/
2011-12-06 19:56:16 -04:00
if (hal->getMode() == MODE_LIVE) {
navigator = new NAVIGATOR_CLASS(hal,k_nav);
} else {
navigator = new AP_Navigator(hal);
}
2011-10-26 13:31:11 -03:00
guide = new GUIDE_CLASS(navigator, hal, velCmd, xt, xtLim);
controller = new CONTROLLER_CLASS(navigator,guide,hal);
/*
* CommLinks
*/
if (board==BOARD_ARDUPILOTMEGA_2)
{
Serial2.begin(telemBaud, 128, 128); // gcs
2011-11-29 14:59:44 -04:00
hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal, heartBeatTimeout);
2011-10-26 13:31:11 -03:00
}
else
{
Serial3.begin(telemBaud, 128, 128); // gcs
2011-11-29 14:59:44 -04:00
hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal, heartBeatTimeout);
2011-10-26 13:31:11 -03:00
}
/*
* Hardware in the Loop
*/
if (hal->getMode() == MODE_HIL_CNTL) {
Serial.println("HIL line setting up");
Serial1.begin(hilBaud, 128, 128);
delay(1000);
Serial1.println("starting hil");
2011-11-29 14:59:44 -04:00
hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal, heartBeatTimeout);
2011-10-26 13:31:11 -03:00
}
/*
* Start the autopil/ot
2011-10-26 13:31:11 -03:00
*/
hal->debug->printf_P(PSTR("initializing autopilot\n"));
hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory());
autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal,
loopRate, loop0Rate, loop1Rate, loop2Rate, loop3Rate);
2011-09-29 15:16:36 -03:00
}
void loop() {
2011-10-26 13:31:11 -03:00
autoPilot->update();
2011-09-29 15:16:36 -03:00
}
2011-09-30 17:55:27 -03:00
#endif //_APO_COMMON_H
2011-10-26 15:59:40 -03:00
// vim:ts=4:sw=4:expandtab