From 59fc8e7a63f53a571c22916a9ddd986366611add Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 28 Nov 2011 00:53:30 -0500 Subject: [PATCH] Updated apo for merge with apm 2.0 changes. Still need to handle switches for 2.0 board in hal. Should probably move into hal ctor. --- ArduBoat/ArduBoat.pde | 2 ++ ArduBoat/ControllerBoat.h | 6 ++--- ArduRover/ArduRover.pde | 3 ++- ArduRover/ControllerCar.h | 8 +++---- CMakeLists.txt | 2 +- apo/ControllerQuad.h | 18 +++++++-------- apo/apo.pde | 2 ++ libraries/APO/APO_DefaultSetup.h | 25 +++++++++++---------- libraries/APO/AP_Autopilot.cpp | 1 - libraries/APO/AP_HardwareAbstractionLayer.h | 9 +++++++- libraries/APO/AP_Navigator.cpp | 5 +---- libraries/APO/AP_RcChannel.cpp | 8 +++---- libraries/APO/AP_RcChannel.h | 4 ++-- 13 files changed, 51 insertions(+), 42 deletions(-) diff --git a/ArduBoat/ArduBoat.pde b/ArduBoat/ArduBoat.pde index 2ac3b8afee..9886411b47 100644 --- a/ArduBoat/ArduBoat.pde +++ b/ArduBoat/ArduBoat.pde @@ -13,6 +13,8 @@ #include #include #include +#include +#include // Vehicle Configuration #include "BoatGeneric.h" diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h index 9db7354b0f..00b089eaa9 100644 --- a/ArduBoat/ControllerBoat.h +++ b/ArduBoat/ControllerBoat.h @@ -26,13 +26,13 @@ public: _hal->debug->println_P(PSTR("initializing boat controller")); _hal->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100, + new AP_RcChannel(k_chMode, PSTR("MODE_"), hal->radio, 5, 1100, 1500, 1900, RC_MODE_IN, false)); _hal->rc.push_back( - new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 3, 1100, 1500, + new AP_RcChannel(k_chStr, PSTR("STR_"), hal->radio, 3, 1100, 1500, 1900, RC_MODE_INOUT, false)); _hal->rc.push_back( - new AP_RcChannel(k_chThrust, PSTR("THR_"), APM_RC, 2, 1100, 1500, + new AP_RcChannel(k_chThrust, PSTR("THR_"), hal->radio, 2, 1100, 1500, 1900, RC_MODE_INOUT, false)); } diff --git a/ArduRover/ArduRover.pde b/ArduRover/ArduRover.pde index baa0d19654..3b3b2c80dc 100644 --- a/ArduRover/ArduRover.pde +++ b/ArduRover/ArduRover.pde @@ -14,7 +14,8 @@ #include #include #include -#include +#include +#include // Vehicle Configuration #include "CarStampede.h" diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h index 1989f8de8e..75b6a97e17 100644 --- a/ArduRover/ControllerCar.h +++ b/ArduRover/ControllerCar.h @@ -27,16 +27,16 @@ public: _hal->debug->println_P(PSTR("initializing car controller")); _hal->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100, + new AP_RcChannel(k_chMode, PSTR("MODE_"), hal->radio, 5, 1100, 1500, 1900, RC_MODE_IN, false)); _hal->rc.push_back( - new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 3, 1100, 1500, + new AP_RcChannel(k_chStr, PSTR("STR_"), hal->radio, 3, 1100, 1500, 1900, RC_MODE_INOUT, false)); _hal->rc.push_back( - new AP_RcChannel(k_chThrust, PSTR("THR_"), APM_RC, 2, 1100, 1500, + new AP_RcChannel(k_chThrust, PSTR("THR_"), hal->radio, 2, 1100, 1500, 1900, RC_MODE_INOUT, false)); _hal->rc.push_back( - new AP_RcChannel(k_chFwdRev, PSTR("FWDREV_"), APM_RC, 4, 1100, 1500, + new AP_RcChannel(k_chFwdRev, PSTR("FWDREV_"), hal->radio, 4, 1100, 1500, 1900, RC_MODE_IN, false)); for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) { diff --git a/CMakeLists.txt b/CMakeLists.txt index b4a1392048..5fee4eb925 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -158,5 +158,5 @@ endmacro() add_sketch(apo ${BOARD} ${PORT}) add_sketch(ArduRover ${BOARD} ${PORT}) add_sketch(ArduBoat ${BOARD} ${PORT}) -add_sketch(ArduPlane ${BOARD} ${PORT}) +#add_sketch(ArduPlane ${BOARD} ${PORT}) #add_sketch(ArduCopter ${BOARD} ${PORT}) diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 3a920464a3..45a7308d3c 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -44,32 +44,32 @@ public: * the order of the channels has to match the enumeration above */ _hal->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100, + new AP_RcChannel(k_chMode, PSTR("MODE_"), hal->radio, 5, 1100, 1500, 1900, RC_MODE_IN, false)); _hal->rc.push_back( - new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 0, 1100, + new AP_RcChannel(k_chRight, PSTR("RIGHT_"), hal->radio, 0, 1100, 1100, 1900, RC_MODE_OUT, false)); _hal->rc.push_back( - new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 1, 1100, + new AP_RcChannel(k_chLeft, PSTR("LEFT_"), hal->radio, 1, 1100, 1100, 1900, RC_MODE_OUT, false)); _hal->rc.push_back( - new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100, + new AP_RcChannel(k_chFront, PSTR("FRONT_"), hal->radio, 2, 1100, 1100, 1900, RC_MODE_OUT, false)); _hal->rc.push_back( - new AP_RcChannel(k_chBack, PSTR("BACK_"), APM_RC, 3, 1100, + new AP_RcChannel(k_chBack, PSTR("BACK_"), hal->radio, 3, 1100, 1100, 1900, RC_MODE_OUT, false)); _hal->rc.push_back( - new AP_RcChannel(k_chRoll, PSTR("ROLL_"), APM_RC, 0, 1100, + new AP_RcChannel(k_chRoll, PSTR("ROLL_"), hal->radio, 0, 1100, 1500, 1900, RC_MODE_IN, false)); _hal->rc.push_back( - new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100, + new AP_RcChannel(k_chPitch, PSTR("PITCH_"), hal->radio, 1, 1100, 1500, 1900, RC_MODE_IN, false)); _hal->rc.push_back( - new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 2, 1100, + new AP_RcChannel(k_chThr, PSTR("THRUST_"), hal->radio, 2, 1100, 1100, 1900, RC_MODE_IN, false)); _hal->rc.push_back( - new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 3, 1100, 1500, + new AP_RcChannel(k_chYaw, PSTR("YAW_"), hal->radio, 3, 1100, 1500, 1900, RC_MODE_IN, false)); } diff --git a/apo/apo.pde b/apo/apo.pde index 4abd88c96e..2307566265 100644 --- a/apo/apo.pde +++ b/apo/apo.pde @@ -13,6 +13,8 @@ #include #include #include +#include +#include // Vehicle Configuration #include "QuadArducopter.h" diff --git a/libraries/APO/APO_DefaultSetup.h b/libraries/APO/APO_DefaultSetup.h index b6e842f746..2f612368cb 100644 --- a/libraries/APO/APO_DefaultSetup.h +++ b/libraries/APO/APO_DefaultSetup.h @@ -42,6 +42,8 @@ void setup() { */ if (hal->getMode() == MODE_LIVE) { + hal->radio = new APM_RC_APM1; + hal->debug->println_P(PSTR("initializing adc")); hal->adc = new ADC_CLASS; hal->adc->Init(); @@ -86,52 +88,51 @@ void setup() { if (rangeFinderFrontEnabled) { hal->debug->println_P(PSTR("initializing front range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(NULL,new ModeFilter); - rangeFinder->set_analog_port(1); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(new AP_AnalogSource_Arduino(1),new ModeFilter); 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(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(2); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(new AP_AnalogSource_Arduino(2),new ModeFilter); 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(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(3); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(new AP_AnalogSource_Arduino(3),new ModeFilter); 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(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(4); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(new AP_AnalogSource_Arduino(4),new ModeFilter); 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(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(5); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(new AP_AnalogSource_Arduino(5),new ModeFilter); 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(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(6); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(new AP_AnalogSource_Arduino(6),new ModeFilter); rangeFinder->set_orientation(0, 0, 1); hal->rangeFinders.push_back(rangeFinder); } + /* + * navigation sensors + */ + hal->imu = new AP_IMU_INS(new AP_InertialSensor_Oilpan(hal->adc), k_sensorCalib); + //hal->imu = AP_IMU_INS(new AP_InertialSensor_MPU6000(mpu6000SelectPin), k_sensorCalib); } /* diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp index 4d4847e09a..af773db570 100644 --- a/libraries/APO/AP_Autopilot.cpp +++ b/libraries/APO/AP_Autopilot.cpp @@ -35,7 +35,6 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, * Radio setup */ hal->debug->println_P(PSTR("initializing radio")); - APM_RC.Init(); // APM Radio initialization, /* * Calibration diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h index acea7e6529..de49dc3b09 100644 --- a/libraries/APO/AP_HardwareAbstractionLayer.h +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -19,6 +19,8 @@ class Compass; class BetterStream; class RangeFinder; class FastSerial; +class AP_IMU_INS; +class APM_RC_Class; namespace apo { @@ -105,8 +107,13 @@ public: APM_BMP085_Class * baro; Compass * compass; Vector rangeFinders; - IMU * imu; AP_BatteryMonitor * batteryMonitor; + AP_IMU_INS * imu; + + /** + * Actuators + */ + APM_RC_Class * radio; /** * Radio Channels diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp index 8cd442e337..7c800232de 100644 --- a/libraries/APO/AP_Navigator.cpp +++ b/libraries/APO/AP_Navigator.cpp @@ -15,6 +15,7 @@ #include "AP_Var_keys.h" #include "../AP_RangeFinder/AP_RangeFinder.h" #include "../AP_IMU/AP_IMU.h" +#include "../AP_InertialSensor/AP_InertialSensor.h" #include "../APM_BMP085/APM_BMP085_hil.h" #include "../APM_BMP085/APM_BMP085.h" @@ -72,10 +73,6 @@ DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal) : if (_hal->getMode() == MODE_LIVE) { - if (_hal->adc) { - _hal->imu = new AP_IMU_Oilpan(_hal->adc, k_sensorCalib); - } - if (_hal->imu) { _dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass); diff --git a/libraries/APO/AP_RcChannel.cpp b/libraries/APO/AP_RcChannel.cpp index 1a88f887ce..96603b594c 100644 --- a/libraries/APO/AP_RcChannel.cpp +++ b/libraries/APO/AP_RcChannel.cpp @@ -18,7 +18,7 @@ namespace apo { AP_RcChannel::AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, - APM_RC_Class & rc, const uint8_t & ch, const uint16_t & pwmMin, + APM_RC_Class * rc, const uint8_t & ch, const uint16_t & pwmMin, const uint16_t & pwmNeutral, const uint16_t & pwmMax, const rcMode_t & rcMode, const bool & reverse, const float & scale) : AP_Var_group(keyValue, name), _ch(this, 1, ch, PSTR("ch")), @@ -32,7 +32,7 @@ AP_RcChannel::AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, if (rcMode == RC_MODE_IN) return; //Serial.print("pwm set for ch: "); Serial.println(int(ch)); - rc.OutputCh(ch, pwmNeutral); + rc->OutputCh(ch, pwmNeutral); } @@ -42,7 +42,7 @@ uint16_t AP_RcChannel::getRadioPwm() { Serial.println(int(_ch)); return _pwmNeutral; // if this happens give a safe value of neutral } - return _rc.InputCh(_ch); + return _rc->InputCh(_ch); } void AP_RcChannel::setPwm(uint16_t pwm) { @@ -64,7 +64,7 @@ void AP_RcChannel::setPwm(uint16_t pwm) { //Serial.print("ch: "); Serial.print(ch); Serial.print(" pwm: "); Serial.println(pwm); if (_rcMode == RC_MODE_IN) return; - _rc.OutputCh(_ch, _pwm); + _rc->OutputCh(_ch, _pwm); } uint16_t AP_RcChannel::_positionToPwm(const float & position) { diff --git a/libraries/APO/AP_RcChannel.h b/libraries/APO/AP_RcChannel.h index 38cc05df9a..f9c8138310 100644 --- a/libraries/APO/AP_RcChannel.h +++ b/libraries/APO/AP_RcChannel.h @@ -24,7 +24,7 @@ class AP_RcChannel: public AP_Var_group { public: /// Constructor - AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, APM_RC_Class & rc, + AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, APM_RC_Class * rc, const uint8_t & ch, const uint16_t & pwmMin, const uint16_t & pwmNeutral, const uint16_t & pwmMax, const rcMode_t & rcMode, @@ -69,7 +69,7 @@ public: protected: // configuration - APM_RC_Class & _rc; + APM_RC_Class * _rc; // internal states uint16_t _pwm; // this is the internal state, position is just created when needed