Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Moved APO quad/ rover projects.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1956 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2011-05-01 17:14:54 +00:00
parent 9b3510bd15
commit 80f941e8e0
8 changed files with 0 additions and 816 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/APO/APO_Config.h
View File

@ -2,7 +2,6 @@
#define APO_Config_H
#include "AP_HardwareAbstractionLayer.h"
#include "mikrokopter.h"
#include "constants.h"
// Serial 0: debug /dev/ttyUSB0

1
libraries/APO/AP_Autopilot.h
View File

@ -40,7 +40,6 @@
#include "AP_Navigator.h"
#include "AP_Guide.h"
#include "AP_CommLink.h"
#include "controllers.h"
/**
* ArduPilotOne namespace to protect varibles

278
libraries/APO/controllers.h
View File

@ -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 <avr/pgmspace.h>
#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

238
libraries/APO/examples/AutopilotCar/AutopilotCar.pde
View File

@ -1,238 +0,0 @@
/*
* AutopilotCar.pde
*
* Created on: Apr 30, 2011
* Author: jgoppert
*/
// Libraries
#include <APO.h>
#include <AP_Common.h>
#include <FastSerial.h>
#include <APM_RC.h>
#include <AP_RangeFinder.h>
#include <GCS_MAVLink.h>
#include <AP_ADC.h>
#include <AP_DCM.h>
#include <AP_Compass.h>
#include <Wire.h>
#include <AP_GPS.h>
#include <AP_IMU.h>
#include <APM_BMP085.h>
// 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();
}

2
libraries/APO/examples/AutopilotCar/Makefile
View File

@ -1,2 +0,0 @@
BOARD = mega
include ../../../AP_Common/Arduino.mk

238
libraries/APO/examples/AutopilotQuad/AutopilotQuad.pde
View File

@ -1,238 +0,0 @@
/*
* AutopilotQuad.pde
*
* Created on: Apr 30, 2011
* Author: jgoppert
*/
// Libraries
#include <APO.h>
#include <AP_Common.h>
#include <FastSerial.h>
#include <APM_RC.h>
#include <AP_RangeFinder.h>
#include <GCS_MAVLink.h>
#include <AP_ADC.h>
#include <AP_DCM.h>
#include <AP_Compass.h>
#include <Wire.h>
#include <AP_GPS.h>
#include <AP_IMU.h>
#include <APM_BMP085.h>
// 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();
}

2
libraries/APO/examples/AutopilotQuad/Makefile
View File

@ -1,2 +0,0 @@
BOARD = mega
include ../../../AP_Common/Arduino.mk

56
libraries/APO/mikrokopter.h
View File

@ -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_ */