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

Got APO quad stabilized flight to hover.

Browse Source 
Needs more gain tuning.
This commit is contained in:
James Goppert 2011-10-12 20:05:01 -04:00
parent 086412ecc3
commit 76a3625024
6 changed files with 76 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
apo/ControllerQuad.h
View File

@ -21,26 +21,27 @@ public:
*/ */
enum { enum {
CH_MODE = 0, // note scicoslab channels set mode, left, right, front, back order CH_MODE = 0, // note scicoslab channels set mode, left, right, front, back order
CH_LEFT, // this enum must match this
CH_RIGHT, CH_RIGHT,
CH_LEFT,
CH_FRONT, CH_FRONT,
CH_BACK, CH_BACK,
CH_ROLL, CH_ROLL,
CH_PITCH, CH_PITCH,
CH_YAW, CH_THRUST,
CH_THRUST CH_YAW
}; };
// must match channel enum
enum { enum {
k_chMode = k_radioChannelsStart, k_chMode = k_radioChannelsStart,
k_chLeft,
k_chRight, k_chRight,
k_chLeft,
k_chFront, k_chFront,
k_chBack, k_chBack,
k_chRoll, k_chRoll,
k_chPitch, k_chPitch,
k_chYaw, k_chThr,
k_chThr k_chYaw
}; };
enum { enum {
@ -84,11 +85,12 @@ public:
new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100, new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100,
1500, 1900, RC_MODE_IN, false)); 1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back( _hal->rc.push_back(
new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100, new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 0, 1100,
1100, 1900, RC_MODE_OUT, false)); 1100, 1900, RC_MODE_OUT, false));
_hal->rc.push_back( _hal->rc.push_back(
new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100, new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 1, 1100,
1100, 1900, RC_MODE_OUT, false)); 1100, 1900, RC_MODE_OUT, false));
_hal->rc.push_back( _hal->rc.push_back(
new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100, new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100,
1100, 1900, RC_MODE_OUT, false)); 1100, 1900, RC_MODE_OUT, false));
@ -102,11 +104,11 @@ public:
new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100, new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100,
1500, 1900, RC_MODE_IN, false)); 1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back( _hal->rc.push_back(
new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 2, 1100, 1500, new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 2, 1100,
1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 3, 1100,
1100, 1900, RC_MODE_IN, false)); 1100, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 3, 1100, 1500,
1900, RC_MODE_IN, false));
} }
virtual void update(const float & dt) { virtual void update(const float & dt) {
@ -133,9 +135,7 @@ public:
} }
// manual mode // manual mode
float mixRemoteWeight = 0;
if (_hal->rc[CH_MODE]->getRadioPosition() > 0) { if (_hal->rc[CH_MODE]->getRadioPosition() > 0) {
mixRemoteWeight = 1;
_mode = MAV_MODE_MANUAL; _mode = MAV_MODE_MANUAL;
} else { } else {
_mode = MAV_MODE_AUTO; _mode = MAV_MODE_AUTO;
@ -152,13 +152,10 @@ public:
case MAV_MODE_MANUAL: { case MAV_MODE_MANUAL: {
setAllRadioChannelsManually(); setAllRadioChannelsManually();
// "mix manual" // "mix manual"
cmdRoll = 0.5 * _hal->rc[CH_ROLL]->getPosition() cmdRoll = -1 * _hal->rc[CH_ROLL]->getPosition();
* mixRemoteWeight; cmdPitch = -1 * _hal->rc[CH_PITCH]->getPosition();
cmdPitch = 0.5 * _hal->rc[CH_PITCH]->getPosition() cmdYawRate = -1 * _hal->rc[CH_YAW]->getPosition();
* mixRemoteWeight; thrustMix = _hal->rc[CH_THRUST]->getPosition();
cmdYawRate = 0.5 * _hal->rc[CH_YAW]->getPosition()
* mixRemoteWeight;
thrustMix = _hal->rc[CH_THRUST]->getPosition() * mixRemoteWeight;
break; break;
} }
@ -166,6 +163,7 @@ public:
// XXX kills all commands, // XXX kills all commands,
// auto not currently implemented // auto not currently implemented
setAllRadioChannelsToNeutral();
// position loop // position loop
/* /*
@ -203,36 +201,39 @@ public:
} }
// attitude loop // attitude loop
// XXX negative sign added to nav roll, not sure why this is necessary float rollMix = pidRoll.update(cmdRoll - _nav->getRoll(),
// XXX negative sign added to nav roll rate, not sure why this is necessary _nav->getRollRate(), dt);
float rollMix = pidRoll.update(cmdRoll + _nav->getRoll(), float pitchMix = pidPitch.update(cmdPitch - _nav->getPitch(),
-_nav->getRollRate(), dt);
// XXX negative sign added to cmdPitch, not sure why this is necessary
float pitchMix = pidPitch.update(-cmdPitch - _nav->getPitch(),
_nav->getPitchRate(), dt); _nav->getPitchRate(), dt);
// XXX negative sign added to cmdYawRate, not sure why this is necessary float yawMix = pidYawRate.update(cmdYawRate - _nav->getYawRate(), 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_RIGHT]->setPosition(thrustMix - rollMix + yawMix);
_hal->rc[CH_LEFT]->setPosition(thrustMix + rollMix + yawMix);
_hal->rc[CH_FRONT]->setPosition(thrustMix + pitchMix - yawMix); _hal->rc[CH_FRONT]->setPosition(thrustMix + pitchMix - yawMix);
_hal->rc[CH_BACK]->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->debug->printf("R: %f\t L: %f\t F: %f\t B: %f\n",
// _hal->rc[CH_LEFT]->getPosition(), //_hal->rc[CH_RIGHT]->getPosition(),
// _hal->rc[CH_RIGHT]->getPosition(), //_hal->rc[CH_LEFT]->getPosition(),
// _hal->rc[CH_FRONT]->getPosition(), //_hal->rc[CH_FRONT]->getPosition(),
// _hal->rc[CH_BACK]->getPosition()); //_hal->rc[CH_BACK]->getPosition());
_hal->debug->printf( //_hal->debug->printf(
"rollMix: %f\t pitchMix: %f\t yawMix: %f\t thrustMix: %f\n", // "rollMix: %f\t pitchMix: %f\t yawMix: %f\t thrustMix: %f\n",
rollMix, pitchMix, yawMix, thrustMix); // rollMix, pitchMix, yawMix, thrustMix);
//_hal->debug->printf("cmdRoll: %f\t roll: %f\t rollMix: %f\n",
// cmdRoll, _nav->getRoll(), rollMix);
//_hal->debug->printf("cmdPitch: %f\t pitch: %f\t pitchMix: %f\n",
// cmdPitch, _nav->getPitch(), pitchMix);
//_hal->debug->printf("cmdYawRate: %f\t yawRate: %f\t yawMix: %f\n",
// cmdYawRate, _nav->getYawRate(), yawMix);
// _hal->debug->printf("roll pwm: %d\t pitch pwm: %d\t yaw pwm: %d\t thrust pwm: %d\n", //_hal->debug->printf("roll pwm: %d\t pitch pwm: %d\t yaw pwm: %d\t thrust pwm: %d\n",
// _hal->rc[CH_ROLL]->readRadio(), //_hal->rc[CH_ROLL]->getRadioPwm(),
// _hal->rc[CH_PITCH]->readRadio(), //_hal->rc[CH_PITCH]->getRadioPwm(),
// _hal->rc[CH_YAW]->readRadio(), //_hal->rc[CH_YAW]->getRadioPwm(),
// _hal->rc[CH_THRUST]->readRadio()); //_hal->rc[CH_THRUST]->getRadioPwm());
} }
virtual MAV_MODE getMode() { virtual MAV_MODE getMode() {
return (MAV_MODE) _mode.get(); return (MAV_MODE) _mode.get();

32
apo/QuadArducopter.h
View File

@ -11,7 +11,7 @@
// vehicle options // vehicle options
static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD; static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD;
static const apo::halMode_t halMode = apo::MODE_LIVE; static const apo::halMode_t halMode = apo::MODE_LIVE;
static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560; static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
static const uint8_t heartBeatTimeout = 3; static const uint8_t heartBeatTimeout = 3;
// algorithm selection // algorithm selection
@ -25,22 +25,26 @@ static const uint8_t heartBeatTimeout = 3;
#define COMPASS_CLASS AP_Compass_HMC5843 #define COMPASS_CLASS AP_Compass_HMC5843
#define BARO_CLASS APM_BMP085_Class #define BARO_CLASS APM_BMP085_Class
#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL #define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL
#define DEBUG_BAUD 57600
#define TELEM_BAUD 57600 // baud rates
#define GPS_BAUD 38400 static uint32_t debugBaud = 57600;
#define HIL_BAUD 57600 static uint32_t telemBaud = 57600;
static uint32_t gpsBaud = 38400;
static uint32_t hilBaud = 57600;
// optional sensors // optional sensors
static bool gpsEnabled = false; static const bool gpsEnabled = false;
static bool baroEnabled = true; static const bool baroEnabled = false;
static bool compassEnabled = true; static const bool compassEnabled = true;
static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD;
// compass orientation: See AP_Compass_HMC5843.h for possible values
static bool rangeFinderFrontEnabled = true; static const bool rangeFinderFrontEnabled = false;
static bool rangeFinderBackEnabled = true; static const bool rangeFinderBackEnabled = false;
static bool rangeFinderLeftEnabled = true; static const bool rangeFinderLeftEnabled = false;
static bool rangeFinderRightEnabled = true; static const bool rangeFinderRightEnabled = false;
static bool rangeFinderUpEnabled = true; static const bool rangeFinderUpEnabled = false;
static bool rangeFinderDownEnabled = true; static const bool rangeFinderDownEnabled = false;
// loop rates // loop rates
static const float loop0Rate = 150; static const float loop0Rate = 150;

2
apo/apo.pde
View File

@ -17,7 +17,7 @@
#include <WProgram.h> #include <WProgram.h>
// Vehicle Configuration // Vehicle Configuration
#include "PlaneEasystar.h" #include "QuadArducopter.h"
// ArduPilotOne Default Setup // ArduPilotOne Default Setup
#include "APO_DefaultSetup.h" #include "APO_DefaultSetup.h"

17
libraries/APO/APO_DefaultSetup.h
View File

@ -27,7 +27,7 @@ void setup() {
/* /*
* Communications * Communications
*/ */
Serial.begin(DEBUG_BAUD, 128, 128); // debug Serial.begin(debugBaud, 128, 128); // debug
// hardware abstraction layer // hardware abstraction layer
hal = new AP_HardwareAbstractionLayer( hal = new AP_HardwareAbstractionLayer(
@ -47,7 +47,7 @@ void setup() {
hal->adc->Init(); hal->adc->Init();
if (gpsEnabled) { if (gpsEnabled) {
Serial1.begin(GPS_BAUD, 128, 16); // gps Serial1.begin(gpsBaud, 128, 16); // gps
hal->debug->println_P(PSTR("initializing gps")); hal->debug->println_P(PSTR("initializing gps"));
AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps)); AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps));
hal->gps = &gpsDriver; hal->gps = &gpsDriver;
@ -62,9 +62,12 @@ void setup() {
} }
if (compassEnabled) { if (compassEnabled) {
Wire.begin();
hal->debug->println_P(PSTR("initializing compass")); hal->debug->println_P(PSTR("initializing compass"));
hal->compass = new COMPASS_CLASS; hal->compass = new COMPASS_CLASS;
hal->compass->set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD); hal->compass->set_orientation(compassOrientation);
hal->compass->set_offsets(0,0,0);
hal->compass->set_declination(0.0);
hal->compass->init(); hal->compass->init();
} }
@ -139,12 +142,12 @@ void setup() {
*/ */
if (board==BOARD_ARDUPILOTMEGA_2) if (board==BOARD_ARDUPILOTMEGA_2)
{ {
Serial2.begin(TELEM_BAUD, 128, 128); // gcs Serial2.begin(telemBaud, 128, 128); // gcs
hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal); hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal);
} }
else else
{ {
Serial3.begin(TELEM_BAUD, 128, 128); // gcs Serial3.begin(telemBaud, 128, 128); // gcs
hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal); hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal);
} }
@ -153,14 +156,14 @@ void setup() {
*/ */
if (hal->getMode() == MODE_HIL_CNTL) { if (hal->getMode() == MODE_HIL_CNTL) {
Serial.println("HIL line setting up"); Serial.println("HIL line setting up");
Serial1.begin(HIL_BAUD, 128, 128); Serial1.begin(hilBaud, 128, 128);
hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal); hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal);
} }
/* /*
* Start the autopilot * Start the autopilot
*/ */
hal->debug->printf_P(PSTR("initializing arduplane\n")); hal->debug->printf_P(PSTR("initializing autopilot\n"));
hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory());
autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal, autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal,

3
libraries/APO/AP_Navigator.h
View File

@ -348,8 +348,9 @@ public:
if (_hal->compass) { if (_hal->compass) {
_hal->compass->read(); _hal->compass->read();
_hal->compass->calculate(getRoll(), getPitch()); _hal->compass->calculate(_dcm->get_dcm_matrix());
_hal->compass->null_offsets(_dcm->get_dcm_matrix()); _hal->compass->null_offsets(_dcm->get_dcm_matrix());
//_hal->debug->printf_P(PSTR("heading: %f"), _hal->compass->heading);
} }
} }
void updateGpsLight(void) { void updateGpsLight(void) {

2
libraries/APO/AP_RcChannel.h
View File

@ -56,7 +56,7 @@ public:
// set // set
void setUsingRadio() { void setUsingRadio() {
setPwm(getRadioPwm()); if (_rcMode != RC_MODE_OUT) setPwm(getRadioPwm());
} }
void setPwm(uint16_t pwm); void setPwm(uint16_t pwm);
void setPosition(float position) { void setPosition(float position) {