diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 9e12bbe0b0..19e763585a 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -21,26 +21,27 @@ public: */ enum { CH_MODE = 0, // note scicoslab channels set mode, left, right, front, back order - CH_LEFT, // this enum must match this CH_RIGHT, + CH_LEFT, CH_FRONT, CH_BACK, CH_ROLL, CH_PITCH, - CH_YAW, - CH_THRUST + CH_THRUST, + CH_YAW }; + // must match channel enum enum { k_chMode = k_radioChannelsStart, - k_chLeft, k_chRight, + k_chLeft, k_chFront, k_chBack, k_chRoll, k_chPitch, - k_chYaw, - k_chThr + k_chThr, + k_chYaw }; enum { @@ -84,11 +85,12 @@ public: new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100, 1500, 1900, RC_MODE_IN, false)); _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)); _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)); + _hal->rc.push_back( new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100, 1100, 1900, RC_MODE_OUT, false)); @@ -102,11 +104,11 @@ public: new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100, 1500, 1900, RC_MODE_IN, false)); _hal->rc.push_back( - new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 2, 1100, 1500, - 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 3, 1100, + new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 2, 1100, 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) { @@ -133,9 +135,7 @@ public: } // manual mode - float mixRemoteWeight = 0; if (_hal->rc[CH_MODE]->getRadioPosition() > 0) { - mixRemoteWeight = 1; _mode = MAV_MODE_MANUAL; } else { _mode = MAV_MODE_AUTO; @@ -152,13 +152,10 @@ public: case MAV_MODE_MANUAL: { setAllRadioChannelsManually(); // "mix manual" - cmdRoll = 0.5 * _hal->rc[CH_ROLL]->getPosition() - * mixRemoteWeight; - cmdPitch = 0.5 * _hal->rc[CH_PITCH]->getPosition() - * mixRemoteWeight; - cmdYawRate = 0.5 * _hal->rc[CH_YAW]->getPosition() - * mixRemoteWeight; - thrustMix = _hal->rc[CH_THRUST]->getPosition() * mixRemoteWeight; + cmdRoll = -1 * _hal->rc[CH_ROLL]->getPosition(); + cmdPitch = -1 * _hal->rc[CH_PITCH]->getPosition(); + cmdYawRate = -1 * _hal->rc[CH_YAW]->getPosition(); + thrustMix = _hal->rc[CH_THRUST]->getPosition(); break; } @@ -166,6 +163,7 @@ public: // XXX kills all commands, // auto not currently implemented + setAllRadioChannelsToNeutral(); // position loop /* @@ -203,36 +201,39 @@ public: } // attitude loop - // XXX negative sign added to nav roll, not sure why this is necessary - // XXX negative sign added to nav roll rate, not sure why this is necessary - float rollMix = pidRoll.update(cmdRoll + _nav->getRoll(), - -_nav->getRollRate(), dt); - // XXX negative sign added to cmdPitch, not sure why this is necessary - float pitchMix = pidPitch.update(-cmdPitch - _nav->getPitch(), + float rollMix = pidRoll.update(cmdRoll - _nav->getRoll(), + _nav->getRollRate(), dt); + float pitchMix = pidPitch.update(cmdPitch - _nav->getPitch(), _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_LEFT]->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("R: %f\t L: %f\t F: %f\t B: %f\n", + //_hal->rc[CH_RIGHT]->getPosition(), + //_hal->rc[CH_LEFT]->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( + // "rollMix: %f\t pitchMix: %f\t yawMix: %f\t thrustMix: %f\n", + // 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->rc[CH_ROLL]->readRadio(), - // _hal->rc[CH_PITCH]->readRadio(), - // _hal->rc[CH_YAW]->readRadio(), - // _hal->rc[CH_THRUST]->readRadio()); + //_hal->debug->printf("roll pwm: %d\t pitch pwm: %d\t yaw pwm: %d\t thrust pwm: %d\n", + //_hal->rc[CH_ROLL]->getRadioPwm(), + //_hal->rc[CH_PITCH]->getRadioPwm(), + //_hal->rc[CH_YAW]->getRadioPwm(), + //_hal->rc[CH_THRUST]->getRadioPwm()); } virtual MAV_MODE getMode() { return (MAV_MODE) _mode.get(); diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 5151b224fe..4d1941b1f6 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -11,7 +11,7 @@ // vehicle options static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD; 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; // algorithm selection @@ -25,22 +25,26 @@ static const uint8_t heartBeatTimeout = 3; #define COMPASS_CLASS AP_Compass_HMC5843 #define BARO_CLASS APM_BMP085_Class #define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL -#define DEBUG_BAUD 57600 -#define TELEM_BAUD 57600 -#define GPS_BAUD 38400 -#define HIL_BAUD 57600 + +// baud rates +static uint32_t debugBaud = 57600; +static uint32_t telemBaud = 57600; +static uint32_t gpsBaud = 38400; +static uint32_t hilBaud = 57600; // optional sensors -static bool gpsEnabled = false; -static bool baroEnabled = true; -static bool compassEnabled = true; +static const bool gpsEnabled = false; +static const bool baroEnabled = false; +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 bool rangeFinderBackEnabled = true; -static bool rangeFinderLeftEnabled = true; -static bool rangeFinderRightEnabled = true; -static bool rangeFinderUpEnabled = true; -static bool rangeFinderDownEnabled = true; +static const bool rangeFinderFrontEnabled = false; +static const bool rangeFinderBackEnabled = false; +static const bool rangeFinderLeftEnabled = false; +static const bool rangeFinderRightEnabled = false; +static const bool rangeFinderUpEnabled = false; +static const bool rangeFinderDownEnabled = false; // loop rates static const float loop0Rate = 150; diff --git a/apo/apo.pde b/apo/apo.pde index 5148713899..dfca3d80ef 100644 --- a/apo/apo.pde +++ b/apo/apo.pde @@ -17,7 +17,7 @@ #include // Vehicle Configuration -#include "PlaneEasystar.h" +#include "QuadArducopter.h" // ArduPilotOne Default Setup #include "APO_DefaultSetup.h" diff --git a/libraries/APO/APO_DefaultSetup.h b/libraries/APO/APO_DefaultSetup.h index b378d5ded2..36eaefc552 100644 --- a/libraries/APO/APO_DefaultSetup.h +++ b/libraries/APO/APO_DefaultSetup.h @@ -27,7 +27,7 @@ void setup() { /* * Communications */ - Serial.begin(DEBUG_BAUD, 128, 128); // debug + Serial.begin(debugBaud, 128, 128); // debug // hardware abstraction layer hal = new AP_HardwareAbstractionLayer( @@ -47,7 +47,7 @@ void setup() { hal->adc->Init(); if (gpsEnabled) { - Serial1.begin(GPS_BAUD, 128, 16); // gps + Serial1.begin(gpsBaud, 128, 16); // gps hal->debug->println_P(PSTR("initializing gps")); AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps)); hal->gps = &gpsDriver; @@ -62,9 +62,12 @@ void setup() { } if (compassEnabled) { + Wire.begin(); hal->debug->println_P(PSTR("initializing compass")); 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(); } @@ -139,12 +142,12 @@ void setup() { */ 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); } else { - Serial3.begin(TELEM_BAUD, 128, 128); // gcs + Serial3.begin(telemBaud, 128, 128); // gcs hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal); } @@ -153,14 +156,14 @@ void setup() { */ if (hal->getMode() == MODE_HIL_CNTL) { 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); } /* * 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()); autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal, diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h index 8170aa826a..aabbb0eaf6 100644 --- a/libraries/APO/AP_Navigator.h +++ b/libraries/APO/AP_Navigator.h @@ -348,8 +348,9 @@ public: if (_hal->compass) { _hal->compass->read(); - _hal->compass->calculate(getRoll(), getPitch()); + _hal->compass->calculate(_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) { diff --git a/libraries/APO/AP_RcChannel.h b/libraries/APO/AP_RcChannel.h index 5a9076e92f..1a93132863 100644 --- a/libraries/APO/AP_RcChannel.h +++ b/libraries/APO/AP_RcChannel.h @@ -56,7 +56,7 @@ public: // set void setUsingRadio() { - setPwm(getRadioPwm()); + if (_rcMode != RC_MODE_OUT) setPwm(getRadioPwm()); } void setPwm(uint16_t pwm); void setPosition(float position) {