From 80f941e8e06cbe7ac98d94e034d8e85d6f7cc258 Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Sun, 1 May 2011 17:14:54 +0000 Subject: [PATCH] Moved APO quad/ rover projects. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1956 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/APO/APO_Config.h | 1 - libraries/APO/AP_Autopilot.h | 1 - libraries/APO/controllers.h | 278 ------------------ .../examples/AutopilotCar/AutopilotCar.pde | 238 --------------- libraries/APO/examples/AutopilotCar/Makefile | 2 - .../examples/AutopilotQuad/AutopilotQuad.pde | 238 --------------- libraries/APO/examples/AutopilotQuad/Makefile | 2 - libraries/APO/mikrokopter.h | 56 ---- 8 files changed, 816 deletions(-) delete mode 100644 libraries/APO/controllers.h delete mode 100644 libraries/APO/examples/AutopilotCar/AutopilotCar.pde delete mode 100644 libraries/APO/examples/AutopilotCar/Makefile delete mode 100644 libraries/APO/examples/AutopilotQuad/AutopilotQuad.pde delete mode 100644 libraries/APO/examples/AutopilotQuad/Makefile delete mode 100644 libraries/APO/mikrokopter.h diff --git a/libraries/APO/APO_Config.h b/libraries/APO/APO_Config.h index 1f01178b25..5b00a701b6 100644 --- a/libraries/APO/APO_Config.h +++ b/libraries/APO/APO_Config.h @@ -2,7 +2,6 @@ #define APO_Config_H #include "AP_HardwareAbstractionLayer.h" -#include "mikrokopter.h" #include "constants.h" // Serial 0: debug /dev/ttyUSB0 diff --git a/libraries/APO/AP_Autopilot.h b/libraries/APO/AP_Autopilot.h index c7bde70eb0..d0ce425593 100644 --- a/libraries/APO/AP_Autopilot.h +++ b/libraries/APO/AP_Autopilot.h @@ -40,7 +40,6 @@ #include "AP_Navigator.h" #include "AP_Guide.h" #include "AP_CommLink.h" -#include "controllers.h" /** * ArduPilotOne namespace to protect varibles diff --git a/libraries/APO/controllers.h b/libraries/APO/controllers.h deleted file mode 100644 index 0cf1a8475a..0000000000 --- a/libraries/APO/controllers.h +++ /dev/null @@ -1,278 +0,0 @@ -#ifndef defaultControllers_H -#define defaultControllers_H - -#include "AP_Controller.h" -#include "AP_HardwareAbstractionLayer.h" -#include "../AP_Common/AP_Var.h" -#include -#include "AP_Navigator.h" - -namespace apo { - -class CarController: public AP_Controller { -private: - // control mode - AP_Var_group _group;AP_Uint8 _mode; - enum { - CH_MODE = 0, CH_STR, CH_THR - }; - PidDFB2 pidStr; - Pid2 pidThr; -public: - CarController(AP_Var::Key cntrlKey, AP_Var::Key pidStrKey, - AP_Var::Key pidThrKey, AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal), _group(cntrlKey, PSTR("CNTRL_")), - _mode(&_group, 1, 0, PSTR("MODE")), - pidStr(pidStrKey, PSTR("STR_"), 1.0, 0, 0, 0, 3), - pidThr(pidThrKey, PSTR("THR_"), 0.6, 0.5, 0, 1, 3) { - _hal->debug->println_P(PSTR("initializing car controller")); - - _hal->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7,1100,1500,1900)); - _hal->rc.push_back( - new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0,1100,1540,1900)); - _hal->rc.push_back( - new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1,1100,1500,1900)); - } - virtual void update(const float & dt) { - // read mode switch - _hal->rc[CH_MODE]->setPwm(_hal->rc[CH_MODE]->readRadio()); - // manual - if (_hal->rc[CH_MODE]->getPosition() > 0) { - _hal->rc[CH_STR]->setPwm(_hal->rc[CH_STR]->readRadio()); - _hal->rc[CH_THR]->setPwm(_hal->rc[CH_THR]->readRadio()); - //_hal->debug->println("manual"); - - } else { // auto - float headingError = _guide->headingCommand - _nav->getHeading(); - if (headingError > 180 * deg2Rad) - headingError -= 360 * deg2Rad; - if (headingError < -180 * deg2Rad) - headingError += 360 * deg2Rad; - _hal->rc[CH_STR]->setPosition(pidStr.update(headingError,_nav->getYawRate(),dt)); - _hal->rc[CH_THR]->setPosition(pidThr.update(_guide->groundSpeedCommand - _nav->getGroundSpeed(),dt)); - //_hal->debug->println("automode"); - } - } -}; - -#if CUSTOM_INCLUDES == CUSTOM_MIKROKOPTER -#include "mikrokopter.h" -#endif - -class QuadController: public AP_Controller { -public: - - /** - * note that these are not the controller radio channel numbers, they are just - * unique keys so they can be reaccessed from the hal rc vector - */ - enum autoChannel_t { - CH_MODE = 0, // note scicoslab channels set mode, left, right, front, back order - CH_LEFT, // this enum must match this - CH_RIGHT, - CH_FRONT, - CH_BACK, - CH_ROLL, - CH_PITCH, - CH_YAW, - CH_THRUST - }; - - QuadController(AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal), - pidRoll(k_pidRoll, PSTR("ROLL_"), PID_ATT_P, PID_ATT_I, - PID_ATT_D, PID_ATT_AWU, PID_ATT_LIM), - pidPitch(k_pidPitch, PSTR("PITCH_"), PID_ATT_P, PID_ATT_I, - PID_ATT_D, PID_ATT_AWU, PID_ATT_LIM), - pidYaw(k_pidYaw, PSTR("YAW_"), PID_YAWPOS_P, PID_YAWPOS_I, - PID_YAWPOS_D, PID_YAWPOS_AWU, PID_YAWPOS_LIM), - pidYawRate(k_pidYawRate, PSTR("YAWRATE_"), PID_YAWSPEED_P, - PID_YAWSPEED_I, PID_YAWSPEED_D, PID_YAWSPEED_AWU, - PID_YAWSPEED_LIM), - pidPN(k_pidPN, PSTR("NORTH_"), PID_POS_P, - PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM), - pidPE(k_pidPE, PSTR("EAST_"), PID_POS_P, PID_POS_I, - PID_POS_D, PID_POS_AWU, PID_POS_LIM), - pidPD(k_pidPD, PSTR("DOWN_"), PID_POS_Z_P, PID_POS_Z_I, - PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM) - { - /* - * allocate radio channels - * the order of the channels has to match the enumeration above - */ - _hal->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7, 1100, 1500, 1900, RC_MODE_IN)); - _hal->rc.push_back( - new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100, 1100, 1900, RC_MODE_OUT)); - _hal->rc.push_back( - new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100, 1100, 1900, RC_MODE_OUT)); - _hal->rc.push_back( - new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100, 1100, 1900, RC_MODE_OUT)); - _hal->rc.push_back( - new AP_RcChannel(k_chBack, PSTR("BACK_"), APM_RC, 3, 1100, 1100, 1900, RC_MODE_OUT)); - _hal->rc.push_back( - new AP_RcChannel(k_chRoll, PSTR("ROLL_"), APM_RC, 0, 1100, 1500, 1900, RC_MODE_IN)); - _hal->rc.push_back( - new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100, 1500, 1900, RC_MODE_IN)); - _hal->rc.push_back( - new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 2, 1100, 1500, 1900, RC_MODE_IN)); - _hal->rc.push_back( // -1 -> 0 maps to 1200, linear 0-1 -> 1200-1800 - new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 3, 1100, 1100, 1900, RC_MODE_IN)); - } - - virtual void update(const float & dt) { - - if (commLost()) { - _mode = MAV_MODE_FAILSAFE; - _hal->rc[CH_LEFT]->setPosition(0); - _hal->rc[CH_RIGHT]->setPosition(0); - _hal->rc[CH_FRONT]->setPosition(0); - _hal->rc[CH_BACK]->setPosition(0); - return; - } - - // read and set pwm so they can be read as positions later - _hal->rc[CH_MODE]->setPwm(_hal->rc[CH_MODE]->readRadio()); - _hal->rc[CH_ROLL]->setPwm(_hal->rc[CH_ROLL]->readRadio()); - _hal->rc[CH_PITCH]->setPwm(_hal->rc[CH_PITCH]->readRadio()); - _hal->rc[CH_YAW]->setPwm(_hal->rc[CH_YAW]->readRadio()); - _hal->rc[CH_THRUST]->setPwm(_hal->rc[CH_THRUST]->readRadio()); - - // manual mode - float mixRemoteWeight = 0; - if (_hal->rc[CH_MODE]->getPwm() > 1350) mixRemoteWeight = 1; - - // "mix manual" - float cmdRoll = _hal->rc[CH_ROLL]->getPosition() * mixRemoteWeight; - float cmdPitch = _hal->rc[CH_PITCH]->getPosition() * mixRemoteWeight; - float cmdYawRate = _hal->rc[CH_YAW]->getPosition() * mixRemoteWeight; - float thrustMix = _hal->rc[CH_THRUST]->getPosition() * mixRemoteWeight; - - // position loop - /* - float cmdNorthTilt = pidPN.update(_nav->getPN(),_nav->getVN(),dt); - float cmdEastTilt = pidPE.update(_nav->getPE(),_nav->getVE(),dt); - float cmdDown = pidPD.update(_nav->getPD(),_nav->getVD(),dt); - - // "transform-to-body" - { - float trigSin = sin(-yaw); - float trigCos = cos(-yaw); - _cmdPitch = _cmdEastTilt * trigCos - - _cmdNorthTilt * trigSin; - _cmdRoll = -_cmdEastTilt * trigSin - + _cmdNorthTilt * trigCos; - // note that the north tilt is negative of the pitch - } - - //thrustMix += THRUST_HOVER_OFFSET; - - // "thrust-trim-adjust" - if (fabs(_cmdRoll) > 0.5) { - _thrustMix *= 1.13949393; - } else { - _thrustMix /= cos(_cmdRoll); - } - if (fabs(_cmdPitch) > 0.5) { - _thrustMix *= 1.13949393; - } else { - _thrustMix /= cos(_cmdPitch); - } - */ - - // attitude loop - float rollMix = pidRoll.update(cmdRoll - _nav->getRoll(),_nav->getRollRate(),dt); - float pitchMix = pidPitch.update(cmdPitch - _nav->getPitch(),_nav->getPitchRate(),dt); - float yawMix = pidYawRate.update(cmdYawRate - _nav->getYawRate(),dt); - - _hal->rc[CH_LEFT]->setPosition(thrustMix + rollMix + yawMix); - _hal->rc[CH_RIGHT]->setPosition(thrustMix - rollMix + yawMix); - _hal->rc[CH_FRONT]->setPosition(thrustMix + pitchMix - yawMix); - _hal->rc[CH_BACK]->setPosition(thrustMix - pitchMix - yawMix); - - _hal->debug->printf("L: %f\t R: %f\t F: %f\t B: %f\n", - _hal->rc[CH_LEFT]->getPosition(), - _hal->rc[CH_RIGHT]->getPosition(), - _hal->rc[CH_FRONT]->getPosition(), - _hal->rc[CH_BACK]->getPosition()); - - _hal->debug->printf("rollMix: %f\t pitchMix: %f\t yawMix: %f\t thrustMix: %f\n", - rollMix, - pitchMix, - yawMix, - thrustMix); - -// _hal->debug->printf("thrust pwm: %d\n",_hal->rc[CH_THRUST]->readRadio()); - } - -private: - PidDFB2 pidRoll, pidPitch, pidYaw, pidPN, pidPE, pidPD; - Pid2 pidYawRate; -}; - -/* - class PlaneController : public AP_Controller - { - private: - // state - AP_Float roll; - AP_Float airspeed; - AP_Float velocity; - AP_Float heading; - - // servo positions - AP_Float steering; - AP_Float throttle; - - // control variables - AP_Float headingCommand; - AP_Float airspeedCommand; - AP_Float rollCommand; - - // channels - static const uint8_t chRoll = 0; - static const uint8_t chPitch = 1; - static const uint8_t chYaw = 2; - - public: - PlaneController(AP_Var::Key chRollKey, AP_Var::Key chPitchKey, AP_Var::Key chYawKey, - AP_Var::Key pidRollKey, AP_Var::Key pidPitchKey, AP_Var::Key pidYawKey) : { - // rc channels - addCh(new AP_RcChannelSimple(chRollKey,PSTR("ROLL"),APM_RC,chRoll,45)); - addCh(new AP_RcChannelSimple(chPitchKey,PSTR("PTCH"),APM_RC,chPitch,45)); - addCh(new AP_RcChannelSimple(chYawKey,PSTR("YAW"),APM_RC,chYaw,45)); - - // pitch control loop - #if AIRSPEED_SENSOR == ENABLED - // pitch control loop w/ airspeed - addBlock(new SumGain(airspeedCommand,AP_Float_unity,airspeed,AP_Float_negative_unity)); - #else - // cross feed variables - addBlock(new SumGain(roll,kffPitchCompk,throttleServo,kffT2P)); - #endif - addBlock(new Pid(pidPitchKey,PSTR("PTCH"),0.1,0,0,1,20)); - addBlock(new ToServo(getRc(chPitch))); - - // roll control loop - addBlock(new SumGain(headingCommand,one,heading,negOne)); - addBlock(new Pid(headingkP,headingKI,headingKD)); - addBlock(new Sink(rollCommand)); - addBlock(new SumGain(rollCommand,one,roll,negOne)); - addBlock(new Pid(rollKP,rollKI,rollKD)); - addBlock(new ToServo(getRc(chRoll))); - - // throttle control loop - addBlock(new SumGain(airspeedCommand,one,airspeed,negOne)); - addBlock(new Pid(throttleKP,throttleKI,throttleKD)); - addBlock(new ToServo(getRc(chThr))); - } - }; - */ - -} // namespace apo - -#endif // defaultControllers_H -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/examples/AutopilotCar/AutopilotCar.pde b/libraries/APO/examples/AutopilotCar/AutopilotCar.pde deleted file mode 100644 index 84e5f4cb22..0000000000 --- a/libraries/APO/examples/AutopilotCar/AutopilotCar.pde +++ /dev/null @@ -1,238 +0,0 @@ -/* - * AutopilotCar.pde - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -// Libraries -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Serial 0: debug /dev/ttyUSB0 -// Serial 1: gps/hil /dev/ttyUSB1 -// Serial 2: gcs /dev/ttyUSB2 - -// select hardware absraction mode from -// MODE_LIVE, actual flight -// TODO: IMPLEMENT --> MODE_HIL_NAV, hardware in the loop with sensors running, tests navigation system and control -// MODE_HIL_CNTL, hardware in the loop with only controller running, just tests controller -apo::halMode_t halMode = apo::MODE_LIVE; - -// select from, BOARD_ARDUPILOTMEGA -apo::board_t board = apo::BOARD_ARDUPILOTMEGA; - -// select from, VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE -apo::vehicle_t vehicle = apo::VEHICLE_CAR; - -// optional sensors -static bool gpsEnabled = true; -static bool baroEnabled = true; -static bool compassEnabled = true; - -static bool rangeFinderFrontEnabled = true; -static bool rangeFinderBackEnabled = true; -static bool rangeFinderLeftEnabled = true; -static bool rangeFinderRightEnabled = true; -static bool rangeFinderUpEnabled = true; -static bool rangeFinderDownEnabled = true; - - -//---------ADVANCED SECTION ----------------// - -// loop rates -const float loop0Rate = 150; -const float loop1Rate = 50; -const float loop2Rate = 10; -const float loop3Rate = 1; -const float loop4Rate = 0.1; - -// max time in seconds to allow flight without ground station comms -// zero will ignore timeout -const uint8_t heartbeatTimeout = 3; - -//---------HARDWARE CONFIG ----------------// - -//Hardware Parameters -#define SLIDE_SWITCH_PIN 40 -#define PUSHBUTTON_PIN 41 -#define A_LED_PIN 37 //36 = B,3637 = A,363735 = C -#define B_LED_PIN 36 -#define C_LED_PIN 35 -#define EEPROM_MAX_ADDR 2048 -#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarLV - - -//---------MAIN ----------------// - - -/* - * Required Global Declarations - */ -FastSerialPort0(Serial); -FastSerialPort1(Serial1); -FastSerialPort2(Serial2); -FastSerialPort3(Serial3); -apo::AP_Autopilot * autoPilot; - -void setup() { - - using namespace apo; - - AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer(halMode,board,vehicle); - - /* - * Communications - */ - Serial.begin(57600, 128, 128); // debug - Serial3.begin(57600, 128, 128); // gcs - - hal->debug = &Serial; - hal->debug->println_P(PSTR("initializing debug line")); - hal->debug->println_P(PSTR("initializing radio")); - APM_RC.Init(); // APM Radio initialization - - /* - * Pins - */ - hal->debug->println_P(PSTR("settings pin modes")); - pinMode(A_LED_PIN, OUTPUT); // extra led - pinMode(B_LED_PIN, OUTPUT); // imu ledclass AP_CommLink; - pinMode(C_LED_PIN, OUTPUT); // gps led - pinMode(SLIDE_SWITCH_PIN, INPUT); - pinMode(PUSHBUTTON_PIN, INPUT); - DDRL |= B00000100; // set port L, pint 2 to output for the relay - - /* - * Initialize Comm Channels - */ - hal->debug->println_P(PSTR("initializing comm channels")); - if (hal->mode()==MODE_LIVE) { - Serial1.begin(38400, 128, 16); // gps - } else { // hil - Serial1.begin(57600, 128, 128); - } - - /* - * Sensor initialization - */ - if (hal->mode()==MODE_LIVE) - { - hal->debug->println_P(PSTR("initializing adc")); - hal->adc = new AP_ADC_ADS7844; - hal->adc->Init(); - - if (gpsEnabled) { - hal->debug->println_P(PSTR("initializing gps")); - AP_GPS_Auto gpsDriver(&Serial1,&(hal->gps)); - hal->gps = &gpsDriver; - hal->gps->init(); - } - - if (baroEnabled) { - hal->debug->println_P(PSTR("initializing baro")); - hal->baro = new APM_BMP085_Class; - hal->baro->Init(); - } - - if (compassEnabled) { - hal->debug->println_P(PSTR("initializing compass")); - hal->compass = new AP_Compass_HMC5843; - hal->compass->set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD); - 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 NULL assigned to any ultrasonic sensor, its corresponding block of code - * 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 defind as (front/back,left/right,down,up) - */ - - if (rangeFinderFrontEnabled) { - hal->debug->println_P(PSTR("initializing front range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS; - rangeFinder->init(1); - 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; - rangeFinder->init(2); - 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; - rangeFinder->init(3); - 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; - rangeFinder->init(4); - 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; - rangeFinder->init(5); - 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; - rangeFinder->init(6); - rangeFinder->set_orientation(0,0,1); - hal->rangeFinders.push_back(rangeFinder); - } - - /* - * Select guidance, navigation, control algorithms - */ - AP_Navigator * navigator = new DcmNavigator(hal); - AP_Guide * guide = new MavlinkGuide(k_guide,navigator,hal); - AP_Controller * controller = new CarController(k_cntrl,k_pidStr,k_pidThr,navigator,guide,hal); - - /* - * CommLinks - */ - hal->gcs = new MavlinkComm(&Serial3,navigator,guide,controller,hal); - hal->hil = new MavlinkComm(&Serial1,navigator,guide,controller,hal); - - /* - * Start the autopilot - */ - hal->debug->printf_P(PSTR("initializing ArduPilotOne\n")); - hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); - autoPilot = new apo::AP_Autopilot(navigator,guide,controller,hal); -} - -void loop() { - autoPilot->update(); -} diff --git a/libraries/APO/examples/AutopilotCar/Makefile b/libraries/APO/examples/AutopilotCar/Makefile deleted file mode 100644 index 85b4d8856b..0000000000 --- a/libraries/APO/examples/AutopilotCar/Makefile +++ /dev/null @@ -1,2 +0,0 @@ -BOARD = mega -include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/examples/AutopilotQuad/AutopilotQuad.pde b/libraries/APO/examples/AutopilotQuad/AutopilotQuad.pde deleted file mode 100644 index 1332b9049b..0000000000 --- a/libraries/APO/examples/AutopilotQuad/AutopilotQuad.pde +++ /dev/null @@ -1,238 +0,0 @@ -/* - * AutopilotQuad.pde - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -// Libraries -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Serial 0: debug /dev/ttyUSB0 -// Serial 1: gps/hil /dev/ttyUSB1 -// Serial 2: gcs /dev/ttyUSB2 - -// select hardware absraction mode from -// MODE_LIVE, actual flight -// TODO: IMPLEMENT --> MODE_HIL_NAV, hardware in the loop with sensors running, tests navigation system and control -// MODE_HIL_CNTL, hardware in the loop with only controller running, just tests controller -apo::halMode_t halMode = apo::MODE_LIVE; - -// select from, BOARD_ARDUPILOTMEGA -apo::board_t board = apo::BOARD_ARDUPILOTMEGA; - -// select from, VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE -apo::vehicle_t vehicle = apo::VEHICLE_QUAD; - -// optional sensors -static bool gpsEnabled = true; -static bool baroEnabled = true; -static bool compassEnabled = true; - -static bool rangeFinderFrontEnabled = true; -static bool rangeFinderBackEnabled = true; -static bool rangeFinderLeftEnabled = true; -static bool rangeFinderRightEnabled = true; -static bool rangeFinderUpEnabled = true; -static bool rangeFinderDownEnabled = true; - - -//---------ADVANCED SECTION ----------------// - -// loop rates -const float loop0Rate = 150; -const float loop1Rate = 50; -const float loop2Rate = 10; -const float loop3Rate = 1; -const float loop4Rate = 0.1; - -// max time in seconds to allow flight without ground station comms -// zero will ignore timeout -const uint8_t heartbeatTimeout = 3; - -//---------HARDWARE CONFIG ----------------// - -//Hardware Parameters -#define SLIDE_SWITCH_PIN 40 -#define PUSHBUTTON_PIN 41 -#define A_LED_PIN 37 //36 = B,3637 = A,363735 = C -#define B_LED_PIN 36 -#define C_LED_PIN 35 -#define EEPROM_MAX_ADDR 2048 -#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarLV - - -//---------MAIN ----------------// - - -/* - * Required Global Declarations - */ -FastSerialPort0(Serial); -FastSerialPort1(Serial1); -FastSerialPort2(Serial2); -FastSerialPort3(Serial3); -apo::AP_Autopilot * autoPilot; - -void setup() { - - using namespace apo; - - AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer(halMode,board,vehicle); - - /* - * Communications - */ - Serial.begin(57600, 128, 128); // debug - Serial3.begin(57600, 128, 128); // gcs - - hal->debug = &Serial; - hal->debug->println_P(PSTR("initializing debug line")); - hal->debug->println_P(PSTR("initializing radio")); - APM_RC.Init(); // APM Radio initialization - - /* - * Pins - */ - hal->debug->println_P(PSTR("settings pin modes")); - pinMode(A_LED_PIN, OUTPUT); // extra led - pinMode(B_LED_PIN, OUTPUT); // imu ledclass AP_CommLink; - pinMode(C_LED_PIN, OUTPUT); // gps led - pinMode(SLIDE_SWITCH_PIN, INPUT); - pinMode(PUSHBUTTON_PIN, INPUT); - DDRL |= B00000100; // set port L, pint 2 to output for the relay - - /* - * Initialize Comm Channels - */ - hal->debug->println_P(PSTR("initializing comm channels")); - if (hal->mode()==MODE_LIVE) { - Serial1.begin(38400, 128, 16); // gps - } else { // hil - Serial1.begin(57600, 128, 128); - } - - /* - * Sensor initialization - */ - if (hal->mode()==MODE_LIVE) - { - hal->debug->println_P(PSTR("initializing adc")); - hal->adc = new AP_ADC_ADS7844; - hal->adc->Init(); - - if (gpsEnabled) { - hal->debug->println_P(PSTR("initializing gps")); - AP_GPS_Auto gpsDriver(&Serial1,&(hal->gps)); - hal->gps = &gpsDriver; - hal->gps->init(); - } - - if (baroEnabled) { - hal->debug->println_P(PSTR("initializing baro")); - hal->baro = new APM_BMP085_Class; - hal->baro->Init(); - } - - if (compassEnabled) { - hal->debug->println_P(PSTR("initializing compass")); - hal->compass = new AP_Compass_HMC5843; - hal->compass->set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD); - 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 NULL assigned to any ultrasonic sensor, its corresponding block of code - * 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 defind as (front/back,left/right,down,up) - */ - - if (rangeFinderFrontEnabled) { - hal->debug->println_P(PSTR("initializing front range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS; - rangeFinder->init(1); - 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; - rangeFinder->init(2); - 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; - rangeFinder->init(3); - 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; - rangeFinder->init(4); - 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; - rangeFinder->init(5); - 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; - rangeFinder->init(6); - rangeFinder->set_orientation(0,0,1); - hal->rangeFinders.push_back(rangeFinder); - } - - /* - * Select guidance, navigation, control algorithms - */ - AP_Navigator * navigator = new DcmNavigator(hal); - AP_Guide * guide = new MavlinkGuide(k_guide,navigator,hal); - AP_Controller * controller = new QuadController(navigator,guide,hal); - - /* - * CommLinks - */ - hal->gcs = new MavlinkComm(&Serial3,navigator,guide,controller,hal); - hal->hil = new MavlinkComm(&Serial1,navigator,guide,controller,hal); - - /* - * Start the autopilot - */ - hal->debug->printf_P(PSTR("initializing ArduPilotOne\n")); - hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); - autoPilot = new apo::AP_Autopilot(navigator,guide,controller,hal); -} - -void loop() { - autoPilot->update(); -} diff --git a/libraries/APO/examples/AutopilotQuad/Makefile b/libraries/APO/examples/AutopilotQuad/Makefile deleted file mode 100644 index 85b4d8856b..0000000000 --- a/libraries/APO/examples/AutopilotQuad/Makefile +++ /dev/null @@ -1,2 +0,0 @@ -BOARD = mega -include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/mikrokopter.h b/libraries/APO/mikrokopter.h deleted file mode 100644 index c3d8b50def..0000000000 --- a/libraries/APO/mikrokopter.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * mikrokopter.h - * - * Created on: Apr 6, 2011 - * Author: nkgentry - */ - -#ifndef MIKROKOPTER_H_ -#define MIKROKOPTER_H_ - -#define VEHICLE_MIKROKOPTER - -//motor parameters -#define MOTOR_MAX 1 -#define MOTOR_MIN 0.1 - -// position control loop -#define PID_POS_INTERVAL 1/5 // 5 hz -#define PID_POS_P 0.02 -#define PID_POS_I 0 -#define PID_POS_D 0.1 -#define PID_POS_LIM 0.1 // about 5 deg -#define PID_POS_AWU 0.0 // about 5 deg -#define PID_POS_Z_P 0.5 -#define PID_POS_Z_I 0.2 -#define PID_POS_Z_D 0.5 -#define PID_POS_Z_LIM 0.5 -#define PID_POS_Z_AWU 0.1 - -// attitude control loop -#define PID_ATT_INTERVAL 1/20 // 20 hz -#define PID_ATT_P 0.03 // 0.1 -#define PID_ATT_I 0.03 // 0.0 -#define PID_ATT_D 0.03 // 0.1 -#define PID_ATT_LIM 0.1 // 0.01 // 10 % #define MOTORs -#define PID_ATT_AWU 0.1 // 0.0 -#define PID_YAWPOS_P 1 -#define PID_YAWPOS_I 0.1 -#define PID_YAWPOS_D 0 -#define PID_YAWPOS_LIM 1 // 1 rad/s -#define PID_YAWPOS_AWU 1 // 1 rad/s -#define PID_YAWSPEED_P 1 -#define PID_YAWSPEED_I 0 -#define PID_YAWSPEED_D 0.5 -#define PID_YAWSPEED_LIM 0.1 // 0.01 // 10 % #define MOTORs -#define PID_YAWSPEED_AWU 0.0 - -// mixing -#define MIX_REMOTE_WEIGHT 1 -#define MIX_POSITION_WEIGHT 1 -#define MIX_POSITION_Z_WEIGHT 1 -#define MIX_POSITION_YAW_WEIGHT 1 - -#define THRUST_HOVER_OFFSET 0.475 - -#endif /* MIKROKOPTER_H_ */