diff --git a/ArduBoat/BoatGeneric.h b/ArduBoat/BoatGeneric.h index ac11ad7a08..8292bae789 100644 --- a/ArduBoat/BoatGeneric.h +++ b/ArduBoat/BoatGeneric.h @@ -9,52 +9,25 @@ #ifndef BOATGENERIC_H_ #define BOATGENERIC_H_ +using namespace apo; + // vehicle options +static const AP_Board::options_t options = AP_Board::opt_gps | AP_Board::opt_baro | AP_Board::opt_compass; static const MAV_TYPE vehicle = UGV_SURFACE_SHIP; static const apo::halMode_t halMode = apo::MODE_LIVE; -static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; -static const uint8_t heartBeatTimeout = 3; +//static const apo::AP_Board::mode_e = apo::AP_Board::MODE_HIL_CNTL; +static const apo::AP_Board::mode_e = apo::AP_Board::MODE_LIVE; +static const uint8_t heartBeatTimeout = 0; // algorithm selection #define CONTROLLER_CLASS ControllerBoat #define GUIDE_CLASS MavlinkGuide -#define NAVIGATOR_CLASS DcmNavigator -#define COMMLINK_CLASS MavlinkComm +#define NAVIGATOR_CLASS Navigator_Dcm // hardware selection -#define COMPASS_CLASS AP_Compass_HMC5843 -#define BARO_CLASS APM_BMP085_Class -#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL - -// baud rates -static const uint32_t debugBaud = 57600; -static const uint32_t telemBaud = 57600; -static const uint32_t gpsBaud = 38400; -static const uint32_t hilBaud = 115200; - -// optional sensors -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 - -// battery monitoring -static const bool batteryMonitorEnabled = false; -static const uint8_t batteryPin = 0; -static const float batteryVoltageDivRatio = 6; -static const float batteryMinVolt = 10.0; -static const float batteryMaxVolt = 12.4; - - -static const bool useForwardReverseSwitch = 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; +//#define BOARD_TYPE Board_APM1 +//#define BOARD_TYPE Board_APM1_2560 +#define BOARD_TYPE Board_APM2 // loop rates static const float loopRate = 150; // attitude nav @@ -63,6 +36,9 @@ static const float loop1Rate = 5; // pos nav/ gcs fast static const float loop2Rate = 1; // gcs slow static const float loop3Rate = 0.1; +// controller +static const bool useForwardReverseSwitch = true; + // gains static const float steeringP = 0.1; static const float steeringI = 0.0; diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h index 633609743e..ee45be824c 100644 --- a/ArduBoat/ControllerBoat.h +++ b/ArduBoat/ControllerBoat.h @@ -15,8 +15,8 @@ namespace apo { class ControllerBoat: public AP_Controller { public: ControllerBoat(AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl), + AP_Board * board) : + AP_Controller(nav, guide, board,new AP_ArmingMechanism(board,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl), pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, steeringI, steeringD, steeringIMax, steeringYMax), pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP, @@ -24,23 +24,23 @@ public: throttleDFCut), _strCmd(0), _thrustCmd(0), _rangeFinderFront() { - _hal->debug->println_P(PSTR("initializing boat controller")); + _board->debug->println_P(PSTR("initializing boat controller")); - _hal->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("MODE_"), hal->radio, 5, 1100, + _board->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), board->radio, 5, 1100, 1500, 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chStr, PSTR("STR_"), hal->radio, 3, 1100, 1500, + _board->rc.push_back( + new AP_RcChannel(k_chStr, PSTR("STR_"), board->radio, 3, 1100, 1500, 1900, RC_MODE_INOUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chThrust, PSTR("THR_"), hal->radio, 2, 1100, 1500, + _board->rc.push_back( + new AP_RcChannel(k_chThrust, PSTR("THR_"), board->radio, 2, 1100, 1500, 1900, RC_MODE_INOUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chFwdRev, PSTR("FWDREV_"), hal->radio, 4, 1100, 1500, + _board->rc.push_back( + new AP_RcChannel(k_chFwdRev, PSTR("FWDREV_"), board->radio, 4, 1100, 1500, 1900, RC_MODE_IN, false)); - for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) { - RangeFinder * rF = _hal->rangeFinders[i]; + for (uint8_t i = 0; i < _board->rangeFinders.getSize(); i++) { + RangeFinder * rF = _board->rangeFinders[i]; if (rF == NULL) continue; if (rF->orientation_x == 1 && rF->orientation_y == 0 @@ -52,13 +52,13 @@ public: private: // methods void manualLoop(const float dt) { - _strCmd = _hal->rc[ch_str]->getRadioPosition(); - _thrustCmd = _hal->rc[ch_thrust]->getRadioPosition(); - if (useForwardReverseSwitch && _hal->rc[ch_FwdRev]->getRadioPosition() < 0.0) + _strCmd = _board->rc[ch_str]->getRadioPosition(); + _thrustCmd = _board->rc[ch_thrust]->getRadioPosition(); + if (useForwardReverseSwitch && _board->rc[ch_FwdRev]->getRadioPosition() < 0.0) _thrustCmd = -_thrustCmd; } void autoLoop(const float dt) { - //_hal->debug->printf_P(PSTR("cont: ch1: %f\tch2: %f\n"),_hal->rc[ch_thrust]->getRadioPosition(), _hal->rc[ch_str]->getRadioPosition()); + //_board->debug->printf_P(PSTR("cont: ch1: %f\tch2: %f\n"),_board->rc[ch_thrust]->getRadioPosition(), _board->rc[ch_str]->getRadioPosition()); // neglects heading command derivative float steering = pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt); float thrust = pidThrust.update( @@ -86,8 +86,8 @@ private: _thrustCmd = thrust; } void setMotors() { - _hal->rc[ch_str]->setPosition(_strCmd); - _hal->rc[ch_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd); + _board->rc[ch_str]->setPosition(_strCmd); + _board->rc[ch_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd); } void handleFailsafe() { // turn off diff --git a/ArduBoat/ControllerSailboat.h b/ArduBoat/ControllerSailboat.h index f7431b07ca..3600b8b7d5 100644 --- a/ArduBoat/ControllerSailboat.h +++ b/ArduBoat/ControllerSailboat.h @@ -15,37 +15,37 @@ namespace apo { class ControllerSailboat: public AP_Controller { public: ControllerSailboat(AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_sail,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl), + AP_Board * board) : + AP_Controller(nav, guide, board,new AP_ArmingMechanism(board,this,ch_sail,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl), pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, steeringI, steeringD, steeringIMax, steeringYMax), pidsail(new AP_Var_group(k_pidsail, PSTR("SAIL_")), 1, throttleP, throttleI, throttleD, throttleIMax, throttleYMax, throttleDFCut), _strCmd(0), _sailCmd(0) { - _hal->debug->println_P(PSTR("initializing sailboat controller")); + _board->debug->println_P(PSTR("initializing sailboat controller")); - _hal->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("MODE_"), hal->radio, 5, 1100, + _board->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), board->radio, 5, 1100, 1500, 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chStr, PSTR("STR_"), hal->radio, 3, 1100, 1500, + _board->rc.push_back( + new AP_RcChannel(k_chStr, PSTR("STR_"), board->radio, 3, 1100, 1500, 1900, RC_MODE_INOUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chsail, PSTR("SAIL_"), hal->radio, 2, 1100, 1500, + _board->rc.push_back( + new AP_RcChannel(k_chsail, PSTR("SAIL_"), board->radio, 2, 1100, 1500, 1900, RC_MODE_INOUT, false)); } private: // methods void manualLoop(const float dt) { - _strCmd = _hal->rc[ch_str]->getRadioPosition(); - _sailCmd = _hal->rc[ch_sail]->getRadioPosition(); + _strCmd = _board->rc[ch_str]->getRadioPosition(); + _sailCmd = _board->rc[ch_sail]->getRadioPosition(); } void autoLoop(const float dt) { - //_hal->debug->printf_P(PSTR("cont: ch1: %f\tch2: %f\n"),_hal->rc[ch_sail]->getRadioPosition(), _hal->rc[ch_str]->getRadioPosition()); + //_board->debug->printf_P(PSTR("cont: ch1: %f\tch2: %f\n"),_board->rc[ch_sail]->getRadioPosition(), _board->rc[ch_str]->getRadioPosition()); float windDir = analogRead(1); - _hal->debug->printf_P(PSTR("wind directiono: %f\n"),windDir); + _board->debug->printf_P(PSTR("wind directiono: %f\n"),windDir); // neglects heading command derivative float steering = pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt); float sail = 0; @@ -53,8 +53,8 @@ private: _sailCmd = sail; } void setMotors() { - _hal->rc[ch_str]->setPosition(_strCmd); - _hal->rc[ch_sail]->setPosition(fabs(_sailCmd) < 0.1 ? 0 : _sailCmd); + _board->rc[ch_str]->setPosition(_strCmd); + _board->rc[ch_sail]->setPosition(fabs(_sailCmd) < 0.1 ? 0 : _sailCmd); } void handleFailsafe() { // turn off diff --git a/ArduBoat/SailboatLaser.h b/ArduBoat/SailboatLaser.h index 4bf037496d..a24ad87819 100644 --- a/ArduBoat/SailboatLaser.h +++ b/ArduBoat/SailboatLaser.h @@ -9,50 +9,24 @@ #ifndef SAILBOATLASER_H_ #define SAILBOATLASER_H_ +using namespace apo; + // vehicle options +static const AP_Board::options_t options = AP_Board::opt_gps | AP_Board::opt_baro | AP_Board::opt_compass; static const MAV_TYPE vehicle = UGV_SURFACE_SHIP; -static const apo::halMode_t halMode = apo::MODE_LIVE; -static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; -static const uint8_t heartBeatTimeout = 3; +//static const apo::AP_Board::mode_e boardMode = apo::AP_Board::MODE_HIL_CNTL; +static const apo::AP_Board::mode_e boardMode = apo::AP_Board::MODE_LIVE; +static const uint8_t heartBeatTimeout = 0; // algorithm selection #define CONTROLLER_CLASS ControllerSailboat #define GUIDE_CLASS MavlinkGuide -#define NAVIGATOR_CLASS DcmNavigator -#define COMMLINK_CLASS MavlinkComm +#define NAVIGATOR_CLASS Navigator_Dcm // hardware selection -#define ADC_CLASS AP_ADC_ADS7844 -#define COMPASS_CLASS AP_Compass_HMC5843 -#define BARO_CLASS APM_BMP085_Class -#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL - -// baud rates -static const uint32_t debugBaud = 57600; -static const uint32_t telemBaud = 57600; -static const uint32_t gpsBaud = 38400; -static const uint32_t hilBaud = 115200; - -// optional sensors -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 - -// battery monitoring -static const bool batteryMonitorEnabled = false; -static const uint8_t batteryPin = 0; -static const float batteryVoltageDivRatio = 6; -static const float batteryMinVolt = 10.0; -static const float batteryMaxVolt = 12.4; - -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; +//#define BOARD_TYPE Board_APM1 +//#define BOARD_TYPE Board_APM1_2560 +#define BOARD_TYPE Board_APM2 // loop rates static const float loopRate = 150; // attitude nav diff --git a/ArduRover/CarStampede.h b/ArduRover/CarStampede.h index 3d8cbd7a28..5659243c4a 100644 --- a/ArduRover/CarStampede.h +++ b/ArduRover/CarStampede.h @@ -10,53 +10,25 @@ #define CARSTAMPEDE_H_ // vehicle options +using namespace apo; +static const AP_Board::options_t options = AP_Board::opt_gps | AP_Board::opt_baro | AP_Board::opt_compass; static const MAV_TYPE vehicle = UGV_GROUND_ROVER; -//static const apo::halMode_t halMode = apo::MODE_HIL_CNTL; -static const apo::halMode_t halMode = apo::MODE_LIVE; -static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; +//static const apo::AP_Board::mode_e boardMode = apo::AP_Board::MODE_HIL_CNTL; +static const apo::AP_Board::mode_e boardMode = apo::AP_Board::MODE_LIVE; static const uint8_t heartBeatTimeout = 3; // algorithm selection #define CONTROLLER_CLASS ControllerCar #define GUIDE_CLASS MavlinkGuide -#define NAVIGATOR_CLASS DcmNavigator -#define COMMLINK_CLASS MavlinkComm +#define NAVIGATOR_CLASS Navigator_Dcm // hardware selection -#define ADC_CLASS AP_ADC_ADS7844 -#define COMPASS_CLASS AP_Compass_HMC5843 -#define BARO_CLASS APM_BMP085_Class -#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL - -// baud rates -static const uint32_t debugBaud = 57600; -static const uint32_t telemBaud = 57600; -static const uint32_t gpsBaud = 38400; -static const uint32_t hilBaud = 115200; - -// optional sensors -static const bool gpsEnabled = true; -static const bool baroEnabled = 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 - -// battery monitoring -static const bool batteryMonitorEnabled = false; -static const uint8_t batteryPin = 0; -static const float batteryVoltageDivRatio = 6; -static const float batteryMinVolt = 10.0; -static const float batteryMaxVolt = 12.4; +//#define BOARD_TYPE Board_APM1 +//#define BOARD_TYPE Board_APM1_2560 +#define BOARD_TYPE Board_APM2 static const bool useForwardReverseSwitch = false; -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 loopRate = 150; // attitude nav static const float loop0Rate = 50; // controller diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h index 2a80f32ab4..257ffb9082 100644 --- a/ArduRover/ControllerCar.h +++ b/ArduRover/ControllerCar.h @@ -15,8 +15,8 @@ namespace apo { class ControllerCar: public AP_Controller { public: ControllerCar(AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl), + AP_Board * board) : + AP_Controller(nav, guide, board,new AP_ArmingMechanism(board,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl), pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, steeringI, steeringD, steeringIMax, steeringYMax), pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP, @@ -24,23 +24,23 @@ public: throttleDFCut), _strCmd(0), _thrustCmd(0), _rangeFinderFront() { - _hal->debug->println_P(PSTR("initializing car controller")); + _board->debug->println_P(PSTR("initializing car controller")); - _hal->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("MODE_"), hal->radio, 5, 1100, + _board->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), board->radio, 5, 1100, 1500, 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chStr, PSTR("STR_"), hal->radio, 3, 1100, 1500, + _board->rc.push_back( + new AP_RcChannel(k_chStr, PSTR("STR_"), board->radio, 3, 1100, 1500, 1900, RC_MODE_INOUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chThrust, PSTR("THR_"), hal->radio, 2, 1100, 1500, + _board->rc.push_back( + new AP_RcChannel(k_chThrust, PSTR("THR_"), board->radio, 2, 1100, 1500, 1900, RC_MODE_INOUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chFwdRev, PSTR("FWDREV_"), hal->radio, 4, 1100, 1500, + _board->rc.push_back( + new AP_RcChannel(k_chFwdRev, PSTR("FWDREV_"), board->radio, 4, 1100, 1500, 1900, RC_MODE_IN, false)); - for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) { - RangeFinder * rF = _hal->rangeFinders[i]; + for (uint8_t i = 0; i < _board->rangeFinders.getSize(); i++) { + RangeFinder * rF = _board->rangeFinders[i]; if (rF == NULL) continue; if (rF->orientation_x == 1 && rF->orientation_y == 0 @@ -52,13 +52,13 @@ public: private: // methods void manualLoop(const float dt) { - _strCmd = _hal->rc[ch_str]->getRadioPosition(); - _thrustCmd = _hal->rc[ch_thrust]->getRadioPosition(); - if (useForwardReverseSwitch && _hal->rc[ch_FwdRev]->getRadioPosition() < 0.0) + _strCmd = _board->rc[ch_str]->getRadioPosition(); + _thrustCmd = _board->rc[ch_thrust]->getRadioPosition(); + if (useForwardReverseSwitch && _board->rc[ch_FwdRev]->getRadioPosition() < 0.0) _thrustCmd = -_thrustCmd; } void autoLoop(const float dt) { - //_hal->debug->printf_P(PSTR("cont: ch1: %f\tch2: %f\n"),_hal->rc[ch_thrust]->getRadioPosition(), _hal->rc[ch_str]->getRadioPosition()); + //_board->debug->printf_P(PSTR("cont: ch1: %f\tch2: %f\n"),_board->rc[ch_thrust]->getRadioPosition(), _board->rc[ch_str]->getRadioPosition()); // neglects heading command derivative float steering = pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt); float thrust = pidThrust.update( @@ -86,8 +86,8 @@ private: _thrustCmd = thrust; } void setMotors() { - _hal->rc[ch_str]->setPosition(_strCmd); - _hal->rc[ch_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd); + _board->rc[ch_str]->setPosition(_strCmd); + _board->rc[ch_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd); } void handleFailsafe() { // turn off diff --git a/ArduRover/TankGeneric.h b/ArduRover/TankGeneric.h index 53f025ba04..d60e9a1cdf 100644 --- a/ArduRover/TankGeneric.h +++ b/ArduRover/TankGeneric.h @@ -11,49 +11,21 @@ #define TANKGENERIC_H_ // vehicle options +static const AP_Board::options_t options = AP_Board::opt_gps | AP_Board::opt_baro | AP_Board::opt_compass; static const MAV_TYPE vehicle = UGV_GROUND_ROVER; -static const apo::halMode_t halMode = apo::MODE_LIVE; -static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; -static const uint8_t heartBeatTimeout = 3; +//static const apo::AP_Board::mode_e = apo::AP_Board::MODE_HIL_CNTL; +static const apo::AP_Board::mode_e = apo::AP_Board::MODE_LIVE; +static const uint8_t heartBeatTimeout = 0; // algorithm selection #define CONTROLLER_CLASS ControllerTank #define GUIDE_CLASS MavlinkGuide -#define NAVIGATOR_CLASS DcmNavigator -#define COMMLINK_CLASS MavlinkComm +#define NAVIGATOR_CLASS Navigator_Dcm // hardware selection -#define ADC_CLASS AP_ADC_ADS7844 -#define COMPASS_CLASS AP_Compass_HMC5843 -#define BARO_CLASS APM_BMP085_Class -#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL - -// baud rates -static const uint32_t debugBaud = 57600; -static const uint32_t telemBaud = 57600; -static const uint32_t gpsBaud = 38400; -static const uint32_t hilBaud = 115200; - -// optional sensors -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 - -// battery monitoring -static const bool batteryMonitorEnabled = true; -static const uint8_t batteryPin = 0; -static const float batteryVoltageDivRatio = 6; -static const float batteryMinVolt = 10.0; -static const float batteryMaxVolt = 12.4; - -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; +//#define BOARD_TYPE Board_APM1 +//#define BOARD_TYPE Board_APM1_2560 +#define BOARD_TYPE Board_APM2 // loop rates static const float loopRate = 150; // attitude nav diff --git a/apo/ControllerPlane.h b/apo/ControllerPlane.h index 078fae9c67..640e2d232b 100644 --- a/apo/ControllerPlane.h +++ b/apo/ControllerPlane.h @@ -15,8 +15,8 @@ namespace apo { class ControllerPlane: public AP_Controller { public: ControllerPlane(AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,this,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode,k_cntrl), + AP_Board * board) : + AP_Controller(nav, guide, board, new AP_ArmingMechanism(board,this,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode,k_cntrl), _trimGroup(k_trim, PSTR("trim_")), _rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")), _needsTrim(false), @@ -44,22 +44,22 @@ public: pidAltThrLim, pidAltThrDFCut), requireRadio(false), _aileron(0), _elevator(0), _rudder(0), _throttle(0) { - _hal->debug->println_P(PSTR("initializing plane controller")); + _board->debug->println_P(PSTR("initializing plane controller")); - _hal->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("mode_"), hal->radio, 5, 1100, + _board->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("mode_"), board->radio, 5, 1100, 1500, 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chRoll, PSTR("roll_"), hal->radio, 0, 1200, + _board->rc.push_back( + new AP_RcChannel(k_chRoll, PSTR("roll_"), board->radio, 0, 1200, 1500, 1800, RC_MODE_INOUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chPitch, PSTR("pitch_"), hal->radio, 1, 1200, + _board->rc.push_back( + new AP_RcChannel(k_chPitch, PSTR("pitch_"), board->radio, 1, 1200, 1500, 1800, RC_MODE_INOUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chThr, PSTR("thr_"), hal->radio, 2, 1100, 1100, + _board->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("thr_"), board->radio, 2, 1100, 1100, 1900, RC_MODE_INOUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chYaw, PSTR("yaw_"), hal->radio, 3, 1200, 1500, + _board->rc.push_back( + new AP_RcChannel(k_chYaw, PSTR("yaw_"), board->radio, 3, 1200, 1500, 1800, RC_MODE_INOUT, false)); } @@ -87,10 +87,10 @@ private: if (_needsTrim) { // need to subtract current controller deflections so control // surfaces are actually at the same position as manual flight - _ailTrim = _hal->rc[ch_roll]->getRadioPosition() - _aileron; - _elvTrim = _hal->rc[ch_pitch]->getRadioPosition() - _elevator; - _rdrTrim = _hal->rc[ch_yaw]->getRadioPosition() - _rudder; - _thrTrim = _hal->rc[ch_thrust]->getRadioPosition() - _throttle; + _ailTrim = _board->rc[ch_roll]->getRadioPosition() - _aileron; + _elvTrim = _board->rc[ch_pitch]->getRadioPosition() - _elevator; + _rdrTrim = _board->rc[ch_yaw]->getRadioPosition() - _rudder; + _thrTrim = _board->rc[ch_thrust]->getRadioPosition() - _throttle; _needsTrim = false; } @@ -102,13 +102,13 @@ private: } void setMotors() { // turn all motors off if below 0.1 throttle - if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) { + if (fabs(_board->rc[ch_thrust]->getRadioPosition()) < 0.1) { setAllRadioChannelsToNeutral(); } else { - _hal->rc[ch_roll]->setPosition(_aileron); - _hal->rc[ch_yaw]->setPosition(_rudder); - _hal->rc[ch_pitch]->setPosition(_elevator); - _hal->rc[ch_thrust]->setPosition(_throttle); + _board->rc[ch_roll]->setPosition(_aileron); + _board->rc[ch_yaw]->setPosition(_rudder); + _board->rc[ch_pitch]->setPosition(_elevator); + _board->rc[ch_thrust]->setPosition(_throttle); } } void handleFailsafe() { diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index f8bd8ffa77..7246ed6e22 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -17,8 +17,8 @@ namespace apo { class ControllerQuad: public AP_Controller { public: ControllerQuad(AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,this,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode, k_cntrl), + AP_Board * board) : + AP_Controller(nav, guide, board, new AP_ArmingMechanism(board,this,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode, k_cntrl), pidRoll(new AP_Var_group(k_pidRoll, PSTR("ROLL_")), 1, PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU, PID_ATT_LIM), @@ -39,39 +39,39 @@ public: PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM), _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0), _cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) { - _hal->debug->println_P(PSTR("initializing quad controller")); + _board->debug->println_P(PSTR("initializing quad controller")); /* * 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_"), hal->radio, 5, 1100, + _board->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), board->radio, 5, 1100, 1500, 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chRight, PSTR("RIGHT_"), hal->radio, 0, 1100, + _board->rc.push_back( + new AP_RcChannel(k_chRight, PSTR("RIGHT_"), board->radio, 0, 1100, 1100, 1900, RC_MODE_OUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chLeft, PSTR("LEFT_"), hal->radio, 1, 1100, + _board->rc.push_back( + new AP_RcChannel(k_chLeft, PSTR("LEFT_"), board->radio, 1, 1100, 1100, 1900, RC_MODE_OUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chFront, PSTR("FRONT_"), hal->radio, 2, 1100, + _board->rc.push_back( + new AP_RcChannel(k_chFront, PSTR("FRONT_"), board->radio, 2, 1100, 1100, 1900, RC_MODE_OUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chBack, PSTR("BACK_"), hal->radio, 3, 1100, + _board->rc.push_back( + new AP_RcChannel(k_chBack, PSTR("BACK_"), board->radio, 3, 1100, 1100, 1900, RC_MODE_OUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chRoll, PSTR("ROLL_"), hal->radio, 0, 1100, + _board->rc.push_back( + new AP_RcChannel(k_chRoll, PSTR("ROLL_"), board->radio, 0, 1100, 1500, 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chPitch, PSTR("PITCH_"), hal->radio, 1, 1100, + _board->rc.push_back( + new AP_RcChannel(k_chPitch, PSTR("PITCH_"), board->radio, 1, 1100, 1500, 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chThr, PSTR("THRUST_"), hal->radio, 2, 1100, + _board->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("THRUST_"), board->radio, 2, 1100, 1100, 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chYaw, PSTR("YAW_"), hal->radio, 3, 1100, 1500, + _board->rc.push_back( + new AP_RcChannel(k_chYaw, PSTR("YAW_"), board->radio, 3, 1100, 1500, 1900, RC_MODE_IN, false)); } @@ -79,10 +79,10 @@ private: // methods void manualLoop(const float dt) { setAllRadioChannelsManually(); - _cmdRoll = -0.5 * _hal->rc[ch_roll]->getPosition(); - _cmdPitch = -0.5 * _hal->rc[ch_pitch]->getPosition(); - _cmdYawRate = -1 * _hal->rc[ch_yaw]->getPosition(); - _thrustMix = _hal->rc[ch_thrust]->getPosition(); + _cmdRoll = -0.5 * _board->rc[ch_roll]->getPosition(); + _cmdPitch = -0.5 * _board->rc[ch_pitch]->getPosition(); + _cmdYawRate = -1 * _board->rc[ch_yaw]->getPosition(); + _thrustMix = _board->rc[ch_thrust]->getPosition(); autoAttitudeLoop(dt); } void autoLoop(const float dt) { @@ -113,7 +113,7 @@ private: else _thrustMix /= cos(_cmdPitch); // debug for position loop - //_hal->debug->printf_P(PSTR("cmd: tilt(%f), down(%f), pitch(%f), roll(%f)\n"),cmdTilt,cmdDown,_cmdPitch,_cmdRoll); + //_board->debug->printf_P(PSTR("cmd: tilt(%f), down(%f), pitch(%f), roll(%f)\n"),cmdTilt,cmdDown,_cmdPitch,_cmdRoll); } void autoAttitudeLoop(float dt) { _rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(), @@ -124,13 +124,13 @@ private: } void setMotors() { // turn all motors off if below 0.1 throttle - if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) { + if (fabs(_board->rc[ch_thrust]->getRadioPosition()) < 0.1) { setAllRadioChannelsToNeutral(); } else { - _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); + _board->rc[ch_right]->setPosition(_thrustMix - _rollMix + _yawMix); + _board->rc[ch_left]->setPosition(_thrustMix + _rollMix + _yawMix); + _board->rc[ch_front]->setPosition(_thrustMix + _pitchMix - _yawMix); + _board->rc[ch_back]->setPosition(_thrustMix - _pitchMix - _yawMix); } } @@ -142,7 +142,7 @@ private: // attributes /** * 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 + * unique keys so they can be reaccessed from the board rc vector */ enum { ch_mode = 0, // note scicoslab channels set mode, left, right, front, back order diff --git a/apo/PlaneEasystar.h b/apo/PlaneEasystar.h index 6e1d56ff0f..7ee78e75c5 100644 --- a/apo/PlaneEasystar.h +++ b/apo/PlaneEasystar.h @@ -8,50 +8,24 @@ #ifndef PLANEEASYSTAR_H_ #define PLANEEASYSTAR_H_ +using namespace apo; + // vehicle options +static const AP_Board::options_t options = AP_Board::opt_gps | AP_Board::opt_baro | AP_Board::opt_compass; static const MAV_TYPE vehicle = MAV_TYPE_FIXED_WING; -static const apo::halMode_t halMode = apo::MODE_LIVE; -static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; -static const uint8_t heartBeatTimeout = 3; +//static const apo::AP_Board::mode_e = apo::AP_Board::MODE_HIL_CNTL; +static const apo::AP_Board::mode_e = apo::AP_Board::MODE_LIVE; +static const uint8_t heartBeatTimeout = 0; // algorithm selection #define CONTROLLER_CLASS ControllerPlane #define GUIDE_CLASS MavlinkGuide -#define NAVIGATOR_CLASS DcmNavigator -#define COMMLINK_CLASS MavlinkComm +#define NAVIGATOR_CLASS Navigator_Dcm // hardware selection -#define ADC_CLASS AP_ADC_ADS7844 -#define COMPASS_CLASS AP_Compass_HMC5843 -#define BARO_CLASS APM_BMP085_Class -#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL - -// baud rates -static const uint32_t debugBaud = 57600; -static const uint32_t telemBaud = 57600; -static const uint32_t gpsBaud = 38400; -static const uint32_t hilBaud = 115200; - -// optional sensors -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 - -// battery monitoring -static const bool batteryMonitorEnabled = false; -static const uint8_t batteryPin = 0; -static const float batteryVoltageDivRatio = 6; -static const float batteryMinVolt = 10.0; -static const float batteryMaxVolt = 12.4; - -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; +//#define BOARD_TYPE Board_APM1 +//#define BOARD_TYPE Board_APM1_2560 +#define BOARD_TYPE Board_APM2 // loop rates static const float loopRate = 150; // attitude nav diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 29464038c0..7501367ed3 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -8,40 +8,35 @@ #ifndef QUADARDUCOPTER_H_ #define QUADARDUCOPTER_H_ +using namespace apo; + // vehicle options +static const AP_Board::options_t options = AP_Board::opt_gps | AP_Board::opt_baro | AP_Board::opt_compass; static const MAV_TYPE vehicle = MAV_QUADROTOR; -//static const apo::halMode_t halMode = apo::MODE_HIL_CNTL; -static const apo::halMode_t halMode = apo::MODE_LIVE; -static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; +//static const apo::AP_Board::mode_e boardMode = apo::AP_Board::MODE_HIL_CNTL; +static const apo::AP_Board::mode_e boardMode = apo::AP_Board::MODE_LIVE; static const uint8_t heartBeatTimeout = 0; // algorithm selection #define CONTROLLER_CLASS ControllerQuad #define GUIDE_CLASS MavlinkGuide -#define NAVIGATOR_CLASS DcmNavigator -#define COMMLINK_CLASS MavlinkComm +#define NAVIGATOR_CLASS Navigator_Dcm -// hardware selection -#define ADC_CLASS AP_ADC_ADS7844 -#define COMPASS_CLASS AP_Compass_HMC5843 -#define BARO_CLASS APM_BMP085_Class -#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL +//// hardware selection +//#define BOARD_TYPE Board_APM1 +//#define BOARD_TYPE Board_APM1_2560 +#define BOARD_TYPE Board_APM2 // baud rates -static const uint32_t debugBaud = 57600; -static const uint32_t telemBaud = 57600; -static const uint32_t gpsBaud = 38400; -static const uint32_t hilBaud = 115200; - // optional sensors -static const bool gpsEnabled = true; +static const bool gpsEnabled = false; static const bool baroEnabled = 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 // battery monitoring -static const bool batteryMonitorEnabled = true; +static const bool batteryMonitorEnabled = false; static const uint8_t batteryPin = 0; static const float batteryVoltageDivRatio = 6; static const float batteryMinVolt = 10.0; @@ -55,7 +50,7 @@ static const bool rangeFinderUpEnabled = false; static const bool rangeFinderDownEnabled = false; // loop rates -static const float loopRate = 200; // attitude nav +static const float loopRate = 250; // attitude nav static const float loop0Rate = 50; // controller static const float loop1Rate = 10; // pos nav/ gcs fast static const float loop2Rate = 1; // gcs slow diff --git a/libraries/APO/APO.h b/libraries/APO/APO.h index 0d4bc6c181..ec28083f12 100644 --- a/libraries/APO/APO.h +++ b/libraries/APO/APO.h @@ -13,7 +13,7 @@ #include "AP_Guide.h" #include "AP_Controller.h" #include "AP_ControllerBlock.h" -#include "AP_HardwareAbstractionLayer.h" +#include "AP_Board.h" #include "AP_MavlinkCommand.h" #include "AP_Navigator.h" #include "AP_RcChannel.h" @@ -21,8 +21,14 @@ #include "AP_ArmingMechanism.h" #include "AP_CommLink.h" #include "AP_Var_keys.h" -#include "DcmNavigator.h" #include "constants.h" + +#include "Navigator_Dcm.h" + +#include "Board_APM1.h" +#include "Board_APM1_2560.h" +#include "Board_APM2.h" + #endif /* APO_H_ */ // vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/APO_DefaultSetup.h b/libraries/APO/APO_DefaultSetup.h index e183f8bdba..4cd2604d3b 100644 --- a/libraries/APO/APO_DefaultSetup.h +++ b/libraries/APO/APO_DefaultSetup.h @@ -16,171 +16,25 @@ void setup() { using namespace apo; - Wire.begin(); - - // Declare all parts of the system - AP_Navigator * navigator = NULL; - AP_Guide * guide = NULL; - AP_Controller * controller = NULL; - AP_HardwareAbstractionLayer * hal = NULL; - - /*/ - * Communications - */ - Serial.begin(debugBaud, 128, 128); // debug - - //// hardware abstraction layer - hal = new AP_HardwareAbstractionLayer(halMode, board, vehicle); - - //// debug serial - hal->debug = &Serial; - hal->debug->println_P(PSTR("initializing debug line")); - - /* - * Sensor initialization - */ - if (hal->getMode() == MODE_LIVE) { - - if (batteryMonitorEnabled) { - hal->batteryMonitor = new AP_BatteryMonitor(batteryPin,batteryVoltageDivRatio,batteryMinVolt,batteryMaxVolt); - } - - if (gpsEnabled) { - Serial1.begin(gpsBaud, 128, 16); // gps - hal->debug->println_P(PSTR("initializing gps")); - AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps)); - hal->gps = &gpsDriver; - hal->gps->callback = delay; - hal->gps->init(); - } - - if (baroEnabled) { - hal->debug->println_P(PSTR("initializing baro")); - hal->baro = new BARO_CLASS; - if (board==BOARD_ARDUPILOTMEGA_2) { - hal->baro->Init(0,true); - } else { - hal->baro->Init(0,false); - } - } - - if (compassEnabled) { - hal->debug->println_P(PSTR("initializing compass")); - hal->compass = new COMPASS_CLASS; - hal->compass->set_orientation(compassOrientation); - hal->compass->set_offsets(0,0,0); - hal->compass->set_declination(0.0); - 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 NU/LL 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 defined as (front/back,left/right,down,up) - */ - - if (rangeFinderFrontEnabled) { - hal->debug->println_P(PSTR("initializing front range finder")); - 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(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(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(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(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(new AP_AnalogSource_Arduino(6),new ModeFilter); - rangeFinder->set_orientation(0, 0, 1); - hal->rangeFinders.push_back(rangeFinder); - } - - /* - * navigation sensors - */ - hal->debug->println_P(PSTR("initializing imu")); - hal->ins = new AP_InertialSensor_Oilpan(hal->adc); - hal->ins->init(hal->scheduler); - //hal->ins = new AP_InertialSensor_MPU6000(mpu6000SelectPin) - hal->debug->println_P(PSTR("initializing ins")); - delay(2000); - hal->imu = new AP_IMU_INS(hal->ins, k_sensorCalib); - hal->imu->init(IMU::WARM_START,delay,hal->scheduler); - hal->debug->println_P(PSTR("setup completed")); - } + // hardware abstraction layer + AP_Board * board = new BOARD_TYPE(boardMode, vehicle, options); /* * Select guidance, navigation, control algorithms */ - if (hal->getMode() == MODE_LIVE) { - navigator = new NAVIGATOR_CLASS(hal,k_nav); + AP_Navigator * navigator = NULL; + if (board->getMode() == AP_Board::MODE_LIVE) { + navigator = new NAVIGATOR_CLASS(board,k_nav); } else { - navigator = new AP_Navigator(hal); - } - guide = new GUIDE_CLASS(navigator, hal, velCmd, xt, xtLim); - controller = new CONTROLLER_CLASS(navigator,guide,hal); - - /* - * CommLinks - */ - if (board==BOARD_ARDUPILOTMEGA_2) - { - Serial2.begin(telemBaud, 128, 128); // gcs - hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal, heartBeatTimeout); - } - else - { - Serial3.begin(telemBaud, 128, 128); // gcs - hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal, heartBeatTimeout); - } - - /* - * Hardware in the Loop - */ - if (hal->getMode() == MODE_HIL_CNTL) { - Serial.println("HIL line setting up"); - Serial1.begin(hilBaud, 128, 128); - delay(1000); - Serial1.println("starting hil"); - hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal, heartBeatTimeout); + navigator = new AP_Navigator(board); } + AP_Guide * guide = new GUIDE_CLASS(navigator, board, velCmd, xt, xtLim); + AP_Controller * controller = new CONTROLLER_CLASS(navigator,guide,board); /* * Start the autopil/ot */ - 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, + autoPilot = new apo::AP_Autopilot(navigator, guide, controller, board, loopRate, loop0Rate, loop1Rate, loop2Rate, loop3Rate); } diff --git a/libraries/APO/AP_ArmingMechanism.cpp b/libraries/APO/AP_ArmingMechanism.cpp index 6236e590c4..1d6702d489 100644 --- a/libraries/APO/AP_ArmingMechanism.cpp +++ b/libraries/APO/AP_ArmingMechanism.cpp @@ -7,17 +7,17 @@ #include "AP_Controller.h" #include "AP_RcChannel.h" #include "../FastSerial/FastSerial.h" -#include "AP_HardwareAbstractionLayer.h" +#include "AP_Board.h" #include "AP_CommLink.h" namespace apo { void AP_ArmingMechanism::update(const float dt) { - //_hal->debug->printf_P(PSTR("ch1: %f\tch2: %f\n"),_hal->rc[_ch1]->getRadioPosition(), _hal->rc[_ch2]->getRadioPosition()); + //_board->debug->printf_P(PSTR("ch1: %f\tch2: %f\n"),_board->rc[_ch1]->getRadioPosition(), _board->rc[_ch2]->getRadioPosition()); // arming if ( (_controller->getState() != MAV_STATE_ACTIVE) && - (fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) && - (_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) { + (fabs(_board->rc[_ch1]->getRadioPosition()) < _ch1Min) && + (_board->rc[_ch2]->getRadioPosition() < _ch2Min) ) { // always start clock at 0 if (_armingClock<0) _armingClock = 0; @@ -25,13 +25,13 @@ void AP_ArmingMechanism::update(const float dt) { if (_armingClock++ >= 100) { _controller->setMode(MAV_MODE_READY); } else { - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("arming")); + _board->gcs->sendText(SEVERITY_HIGH, PSTR("arming")); } } // disarming else if ( (_controller->getState() == MAV_STATE_ACTIVE) && - (fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) && - (_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) { + (fabs(_board->rc[_ch1]->getRadioPosition()) < _ch1Min) && + (_board->rc[_ch2]->getRadioPosition() > _ch2Max) ) { // always start clock at 0 if (_armingClock>0) _armingClock = 0; @@ -39,14 +39,14 @@ void AP_ArmingMechanism::update(const float dt) { if (_armingClock-- <= -100) { _controller->setMode(MAV_MODE_LOCKED); } else { - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarming")); + _board->gcs->sendText(SEVERITY_HIGH, PSTR("disarming")); } } // reset arming clock and report status else if (_armingClock != 0) { _armingClock = 0; - if (_controller->getState()==MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); - else if (_controller->getState()!=MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed")); + if (_controller->getState()==MAV_STATE_ACTIVE) _board->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); + else if (_controller->getState()!=MAV_STATE_ACTIVE) _board->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed")); } } diff --git a/libraries/APO/AP_ArmingMechanism.h b/libraries/APO/AP_ArmingMechanism.h index 8ff3e56105..b09e94f8e4 100644 --- a/libraries/APO/AP_ArmingMechanism.h +++ b/libraries/APO/AP_ArmingMechanism.h @@ -11,7 +11,7 @@ namespace apo { -class AP_HardwareAbstractionLayer; +class AP_Board; class AP_Controller; class AP_ArmingMechanism { @@ -26,9 +26,9 @@ public: * @param ch2Min: disarms below this * @param ch2Max: arms above this */ - AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal, AP_Controller * controller, + AP_ArmingMechanism(AP_Board * board, AP_Controller * controller, uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min, - float ch2Max) : _armingClock(0), _hal(hal), _controller(controller), + float ch2Max) : _armingClock(0), _board(board), _controller(controller), _ch1(ch1), _ch2(ch2), _ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) { } @@ -52,7 +52,7 @@ public: private: - AP_HardwareAbstractionLayer * _hal; + AP_Board * _board; AP_Controller * _controller; int8_t _armingClock; uint8_t _ch1; /// typically throttle channel diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp index fcd5fef57e..39e6d21048 100644 --- a/libraries/APO/AP_Autopilot.cpp +++ b/libraries/APO/AP_Autopilot.cpp @@ -9,7 +9,7 @@ #include "AP_Autopilot.h" #include "../AP_GPS/AP_GPS.h" #include "../APM_RC/APM_RC.h" -#include "AP_HardwareAbstractionLayer.h" +#include "AP_Board.h" #include "AP_CommLink.h" #include "AP_MavlinkCommand.h" #include "AP_Navigator.h" @@ -19,29 +19,35 @@ namespace apo { -class AP_HardwareAbstractionLayer; +class AP_Board; AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, - AP_Controller * controller, AP_HardwareAbstractionLayer * hal, + AP_Controller * controller, AP_Board * board, float loopRate, float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate) : Loop(loopRate, callback, this), _navigator(navigator), _guide(guide), - _controller(controller), _hal(hal), + _controller(controller), _board(board), callbackCalls(0) { - hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + board->debug->printf_P(PSTR("initializing autopilot\n")); + board->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); /* - * Radio setup + * Comm links */ - hal->debug->println_P(PSTR("initializing radio")); + board->gcs = new MavlinkComm(board->gcsPort, navigator, guide, controller, board, 3); + if (board->getMode() != AP_Board::MODE_LIVE) { + board->hil = new MavlinkComm(board->hilPort, navigator, guide, controller, board, 3); + } + + board->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + board->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); /* * Calibration */ controller->setState(MAV_STATE_CALIBRATING); - hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + board->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + board->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); if (navigator) navigator->calibrate(); @@ -51,36 +57,36 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, while (_navigator) { // letc gcs known we are alive - hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); - if (hal->getMode() == MODE_LIVE) { + board->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + board->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + if (board->getMode() == AP_Board::MODE_LIVE) { _navigator->updateSlow(0); - if (hal->gps) { - if (hal->gps->fix) { + if (board->gps) { + if (board->gps->fix) { break; } else { - hal->gps->update(); - hal->gcs->sendText(SEVERITY_LOW, + board->gps->update(); + board->gcs->sendText(SEVERITY_LOW, PSTR("waiting for gps lock\n")); - hal->debug->printf_P(PSTR("waiting for gps lock\n")); + board->debug->printf_P(PSTR("waiting for gps lock\n")); } } else { // no gps, can skip break; } - } else if (hal->getMode() == MODE_HIL_CNTL) { // hil - hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - hal->hil->receive(); + } else if (board->getMode() == AP_Board::MODE_HIL_CNTL) { // hil + board->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + board->hil->receive(); if (_navigator->getTimeStamp() != 0) { // give hil a chance to send some packets for (int i = 0; i < 5; i++) { - hal->debug->println_P(PSTR("reading initial hil packets")); - hal->gcs->sendText(SEVERITY_LOW, PSTR("reading initial hil packets")); + board->debug->println_P(PSTR("reading initial hil packets")); + board->gcs->sendText(SEVERITY_LOW, PSTR("reading initial hil packets")); delay(1000); } break; } - hal->debug->println_P(PSTR("waiting for hil packet")); - hal->gcs->sendText(SEVERITY_LOW, PSTR("waiting for hil packets")); + board->debug->println_P(PSTR("waiting for hil packet")); + board->gcs->sendText(SEVERITY_LOW, PSTR("waiting for hil packets")); } delay(500); } @@ -90,12 +96,12 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, AP_MavlinkCommand::home.setLon(_navigator->getLon()); AP_MavlinkCommand::home.setCommand(MAV_CMD_NAV_WAYPOINT); AP_MavlinkCommand::home.save(); - _hal->debug->printf_P(PSTR("\nhome before load lat: %f deg, lon: %f deg, cmd: %d\n"), + _board->debug->printf_P(PSTR("\nhome before load lat: %f deg, lon: %f deg, cmd: %d\n"), AP_MavlinkCommand::home.getLat()*rad2Deg, AP_MavlinkCommand::home.getLon()*rad2Deg, AP_MavlinkCommand::home.getCommand()); AP_MavlinkCommand::home.load(); - _hal->debug->printf_P(PSTR("\nhome after load lat: %f deg, lon: %f deg, cmd: %d\n"), + _board->debug->printf_P(PSTR("\nhome after load lat: %f deg, lon: %f deg, cmd: %d\n"), AP_MavlinkCommand::home.getLat()*rad2Deg, AP_MavlinkCommand::home.getLon()*rad2Deg, AP_MavlinkCommand::home.getCommand()); @@ -107,19 +113,19 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, /* * Attach loops, stacking for priority */ - hal->debug->println_P(PSTR("attaching loops")); + board->debug->println_P(PSTR("attaching loops")); subLoops().push_back(new Loop(loop0Rate, callback0, this)); subLoops().push_back(new Loop(loop1Rate, callback1, this)); subLoops().push_back(new Loop(loop2Rate, callback2, this)); subLoops().push_back(new Loop(loop3Rate, callback3, this)); - hal->debug->println_P(PSTR("running")); - hal->gcs->sendText(SEVERITY_LOW, PSTR("running")); + board->debug->println_P(PSTR("running")); + board->gcs->sendText(SEVERITY_LOW, PSTR("running")); } void AP_Autopilot::callback(void * data) { AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getHal()->debug->println_P(PSTR("callback")); + //apo->getBoard()->debug->println_P(PSTR("callback")); /* * ahrs update @@ -131,40 +137,40 @@ void AP_Autopilot::callback(void * data) { void AP_Autopilot::callback0(void * data) { AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getHal()->debug->println_P(PSTR("callback 0")); + //apo->getBoard()->debug->println_P(PSTR("callback 0")); /* * hardware in the loop */ - if (apo->getHal()->hil && apo->getHal()->getMode() != MODE_LIVE) { - apo->getHal()->hil->receive(); - apo->getHal()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); + if (apo->getBoard()->hil && apo->getBoard()->getMode() != AP_Board::MODE_LIVE) { + apo->getBoard()->hil->receive(); + apo->getBoard()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); } /* * update control laws */ if (apo->getController()) { - //apo->getHal()->debug->println_P(PSTR("updating controller")); + //apo->getBoard()->debug->println_P(PSTR("updating controller")); apo->getController()->update(apo->subLoops()[0]->dt()); } /* char msg[50]; sprintf(msg, "c_hdg: %f, c_thr: %f", apo->guide()->headingCommand, apo->guide()->groundSpeedCommand); - apo->hal()->gcs->sendText(AP_CommLink::SEVERITY_LOW, msg); + apo->board()->gcs->sendText(AP_CommLink::SEVERITY_LOW, msg); */ } void AP_Autopilot::callback1(void * data) { AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getHal()->debug->println_P(PSTR("callback 1")); + //apo->getBoard()->debug->println_P(PSTR("callback 1")); /* * update guidance laws */ if (apo->getGuide()) { - //apo->getHal()->debug->println_P(PSTR("updating guide")); + //apo->getBoard()->debug->println_P(PSTR("updating guide")); apo->getGuide()->update(); } @@ -178,21 +184,21 @@ void AP_Autopilot::callback1(void * data) { /* * send telemetry */ - if (apo->getHal()->gcs) { - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE); - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION); + if (apo->getBoard()->gcs) { + apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE); + apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION); } /* * handle ground control station communication */ - if (apo->getHal()->gcs) { + if (apo->getBoard()->gcs) { // send messages - apo->getHal()->gcs->requestCmds(); - apo->getHal()->gcs->sendParameters(); + apo->getBoard()->gcs->requestCmds(); + apo->getBoard()->gcs->sendParameters(); // receive messages - apo->getHal()->gcs->receive(); + apo->getBoard()->gcs->receive(); } /* @@ -200,11 +206,11 @@ void AP_Autopilot::callback1(void * data) { */ /* if (apo->navigator()) { - apo->getHal()->debug->printf_P(PSTR("roll: %f deg\tpitch: %f deg\tyaw: %f deg\n"), + apo->getBoard()->debug->printf_P(PSTR("roll: %f deg\tpitch: %f deg\tyaw: %f deg\n"), apo->navigator()->getRoll()*rad2Deg, apo->navigator()->getPitch()*rad2Deg, apo->navigator()->getYaw()*rad2Deg); - apo->getHal()->debug->printf_P(PSTR("lat: %f deg\tlon: %f deg\talt: %f m\n"), + apo->getBoard()->debug->printf_P(PSTR("lat: %f deg\tlon: %f deg\talt: %f m\n"), apo->navigator()->getLat()*rad2Deg, apo->navigator()->getLon()*rad2Deg, apo->navigator()->getAlt()); @@ -214,39 +220,39 @@ void AP_Autopilot::callback1(void * data) { void AP_Autopilot::callback2(void * data) { AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getHal()->debug->println_P(PSTR("callback 2")); + //apo->getBoard()->debug->println_P(PSTR("callback 2")); /* * send telemetry */ - if (apo->getHal()->gcs) { + if (apo->getBoard()->gcs) { // send messages - //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW_INT); - //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_LOCAL_POSITION); - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW); - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU); + //apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW_INT); + //apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_LOCAL_POSITION); + apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); + apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW); + apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU); } /* * update battery monitor */ - if (apo->getHal()->batteryMonitor) apo->getHal()->batteryMonitor->update(); + if (apo->getBoard()->batteryMonitor) apo->getBoard()->batteryMonitor->update(); /* * send heartbeat */ - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); /* * load/loop rate/ram debug */ - apo->getHal()->load = apo->load(); - apo->getHal()->debug->printf_P(PSTR("callback calls: %d\n"),apo->callbackCalls); + apo->getBoard()->load = apo->load(); + apo->getBoard()->debug->printf_P(PSTR("callback calls: %d\n"),apo->callbackCalls); apo->callbackCalls = 0; - apo->getHal()->debug->printf_P(PSTR("load: %d%%\trate: %f Hz\tfree ram: %d bytes\n"), + apo->getBoard()->debug->printf_P(PSTR("load: %d%%\trate: %f Hz\tfree ram: %d bytes\n"), apo->load(),1.0/apo->dt(),freeMemory()); - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); /* * adc debug @@ -259,7 +265,7 @@ void AP_Autopilot::callback2(void * data) { void AP_Autopilot::callback3(void * data) { //AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getHal()->debug->println_P(PSTR("callback 3")); + //apo->getBoard()->debug->println_P(PSTR("callback 3")); } } // apo diff --git a/libraries/APO/AP_Autopilot.h b/libraries/APO/AP_Autopilot.h index a1a6b82adb..f05d79fe81 100644 --- a/libraries/APO/AP_Autopilot.h +++ b/libraries/APO/AP_Autopilot.h @@ -24,7 +24,7 @@ namespace apo { class AP_Navigator; class AP_Guide; class AP_Controller; -class AP_HardwareAbstractionLayer; +class AP_Board; /** * This class encapsulates the entire autopilot system @@ -43,7 +43,7 @@ public: * Default constructor */ AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, - AP_Controller * controller, AP_HardwareAbstractionLayer * hal, + AP_Controller * controller, AP_Board * board, float loopRate, float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate); /** @@ -58,8 +58,8 @@ public: AP_Controller * getController() { return _controller; } - AP_HardwareAbstractionLayer * getHal() { - return _hal; + AP_Board * getBoard() { + return _board; } /** @@ -114,7 +114,7 @@ private: AP_Navigator * _navigator; AP_Guide * _guide; AP_Controller * _controller; - AP_HardwareAbstractionLayer * _hal; + AP_Board * _board; }; } // namespace apo diff --git a/libraries/APO/AP_Board.cpp b/libraries/APO/AP_Board.cpp new file mode 100644 index 0000000000..ffb430b293 --- /dev/null +++ b/libraries/APO/AP_Board.cpp @@ -0,0 +1,14 @@ +/* + * AP_Board.cpp + * + * Created on: Apr 4, 2011 + * + */ + +#include "AP_Board.h" + +namespace apo { + +} // namespace apo + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_Board.h similarity index 64% rename from libraries/APO/AP_HardwareAbstractionLayer.h rename to libraries/APO/AP_Board.h index 43770431e8..af88062855 100644 --- a/libraries/APO/AP_HardwareAbstractionLayer.h +++ b/libraries/APO/AP_Board.h @@ -1,12 +1,12 @@ /* - * AP_HardwareAbstractionLayer.h + * AP_Board.h * * Created on: Apr 4, 2011 * */ -#ifndef AP_HARDWAREABSTRACTIONLAYER_H_ -#define AP_HARDWAREABSTRACTIONLAYER_H_ +#ifndef AP_BOARD_H_ +#define AP_BOARD_H_ #include "../AP_Common/AP_Vector.h" #include "../GCS_MAVLink/GCS_MAVLink.h" @@ -24,6 +24,7 @@ class AP_InertialSensor; class APM_RC_Class; class AP_TimerProcess; class Arduino_Mega_ISR_Registry; +class DataFlash_Class; namespace apo { @@ -31,23 +32,38 @@ class AP_RcChannel; class AP_CommLink; class AP_BatteryMonitor; -// enumerations -enum halMode_t { - MODE_LIVE, MODE_HIL_CNTL, - /*MODE_HIL_NAV*/ -}; -enum board_t { - BOARD_ARDUPILOTMEGA_1280, BOARD_ARDUPILOTMEGA_2560, BOARD_ARDUPILOTMEGA_2 -}; - -class AP_HardwareAbstractionLayer { +class AP_Board { public: + typedef uint32_t options_t; + options_t _options; + + // enumerations + enum mode_e { + MODE_LIVE, MODE_HIL_CNTL, + /*MODE_HIL_NAV*/ + }; + + + enum options_e { + opt_gps = 0<<1, + opt_baro = 1<<1, + opt_compass = 2<<1, + opt_batteryMonitor = 3<<1, + opt_rangeFinderFront = 4<<1, + opt_rangeFinderBack = 5<<1, + opt_rangeFinderLeft = 6<<1, + opt_rangeFinderRight = 7<<1, + opt_rangeFinderUp = 8<<1, + opt_rangeFinderDown = 9<<1, + }; + // default ctors on pointers called on pointers here, this // allows NULL to be used as a boolean for if the device was // initialized - AP_HardwareAbstractionLayer(halMode_t mode, board_t board, MAV_TYPE vehicle); + AP_Board(mode_e mode, MAV_TYPE vehicle, options_t options): _mode(mode), _vehicle(vehicle), _options(options) { + } /** * Sensors @@ -83,10 +99,13 @@ public: AP_CommLink * gcs; AP_CommLink * hil; FastSerial * debug; + FastSerial * gcsPort; + FastSerial * hilPort; /** * data */ + DataFlash_Class * dataFlash; uint8_t load; /** @@ -100,12 +119,9 @@ public: uint16_t eepromMaxAddr; // accessors - halMode_t getMode() { + mode_e getMode() { return _mode; } - board_t getBoard() { - return _board; - } MAV_TYPE getVehicle() { return _vehicle; } @@ -113,8 +129,7 @@ public: private: // enumerations - halMode_t _mode; - board_t _board; + mode_e _mode; MAV_TYPE _vehicle; }; diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp index d7c3beee55..30d473e9eb 100644 --- a/libraries/APO/AP_CommLink.cpp +++ b/libraries/APO/AP_CommLink.cpp @@ -11,7 +11,7 @@ #include "AP_Guide.h" #include "AP_Controller.h" #include "AP_MavlinkCommand.h" -#include "AP_HardwareAbstractionLayer.h" +#include "AP_Board.h" #include "AP_RcChannel.h" #include "../AP_GPS/AP_GPS.h" #include "../AP_Math/AP_Math.h" @@ -25,16 +25,16 @@ uint8_t MavlinkComm::_nChannels = 0; uint8_t MavlinkComm::_paramNameLengthMax = 13; AP_CommLink::AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide, - AP_Controller * controller, AP_HardwareAbstractionLayer * hal, + AP_Controller * controller, AP_Board * board, const uint16_t heartBeatTimeout) : _link(link), _navigator(navigator), _guide(guide), - _controller(controller), _hal(hal), _heartBeatTimeout(heartBeatTimeout), _lastHeartBeat(0) { + _controller(controller), _board(board), _heartBeatTimeout(heartBeatTimeout), _lastHeartBeat(0) { } MavlinkComm::MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide, - AP_Controller * controller, AP_HardwareAbstractionLayer * hal, + AP_Controller * controller, AP_Board * board, const uint16_t heartBeatTimeout) : - AP_CommLink(link, nav, guide, controller, hal,heartBeatTimeout), + AP_CommLink(link, nav, guide, controller, board,heartBeatTimeout), // options _useRelativeAlt(true), @@ -74,7 +74,7 @@ void MavlinkComm::send() { } void MavlinkComm::sendMessage(uint8_t id, uint32_t param) { - //_hal->debug->printf_P(PSTR("send message\n")); + //_board->debug->printf_P(PSTR("send message\n")); // if number of channels exceeded return if (_channel == MAVLINK_COMM_3) @@ -85,7 +85,7 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) { switch (id) { case MAVLINK_MSG_ID_HEARTBEAT: { - mavlink_msg_heartbeat_send(_channel, _hal->getVehicle(), + mavlink_msg_heartbeat_send(_channel, _board->getVehicle(), MAV_AUTOPILOT_ARDUPILOTMEGA); break; } @@ -115,31 +115,31 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) { } case MAVLINK_MSG_ID_GPS_RAW: { - mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(), - _hal->gps->latitude/1.0e7, - _hal->gps->longitude/1.0e7, _hal->gps->altitude/100.0, 0, 0, - _hal->gps->ground_speed/100.0, - _hal->gps->ground_course/10.0); + mavlink_msg_gps_raw_send(_channel, timeStamp, _board->gps->status(), + _board->gps->latitude/1.0e7, + _board->gps->longitude/1.0e7, _board->gps->altitude/100.0, 0, 0, + _board->gps->ground_speed/100.0, + _board->gps->ground_course/10.0); break; } case MAVLINK_MSG_ID_GPS_RAW_INT: { - mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(), - _hal->gps->latitude, - _hal->gps->longitude, _hal->gps->altitude*10.0, 0, 0, - _hal->gps->ground_speed/100.0, - _hal->gps->ground_course/10.0); + mavlink_msg_gps_raw_send(_channel, timeStamp, _board->gps->status(), + _board->gps->latitude, + _board->gps->longitude, _board->gps->altitude*10.0, 0, 0, + _board->gps->ground_speed/100.0, + _board->gps->ground_course/10.0); break; } case MAVLINK_MSG_ID_SCALED_IMU: { int16_t xmag, ymag, zmag; xmag = ymag = zmag = 0; - if (_hal->compass) { + if (_board->compass) { // XXX THIS IS NOT SCALED - xmag = _hal->compass->mag_x; - ymag = _hal->compass->mag_y; - zmag = _hal->compass->mag_z; + xmag = _board->compass->mag_x; + ymag = _board->compass->mag_y; + zmag = _board->compass->mag_z; } mavlink_msg_scaled_imu_send(_channel, timeStamp, _navigator->getXAccel()*1e3, @@ -156,9 +156,9 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) { int16_t ch[8]; for (int i = 0; i < 8; i++) ch[i] = 0; - for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) { - ch[i] = 10000 * _hal->rc[i]->getPosition(); - //_hal->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]); + for (uint8_t i = 0; i < 8 && i < _board->rc.getSize(); i++) { + ch[i] = 10000 * _board->rc[i]->getPosition(); + //_board->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]); } mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2], ch[3], ch[4], ch[5], ch[6], ch[7], 255); @@ -169,8 +169,8 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) { int16_t ch[8]; for (int i = 0; i < 8; i++) ch[i] = 0; - for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) - ch[i] = _hal->rc[i]->getPwm(); + for (uint8_t i = 0; i < 8 && i < _board->rc.getSize(); i++) + ch[i] = _board->rc[i]->getPwm(); mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2], ch[3], ch[4], ch[5], ch[6], ch[7], 255); break; @@ -180,12 +180,12 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) { uint16_t batteryVoltage = 0; // (milli volts) uint16_t batteryPercentage = 1000; // times 10 - if (_hal->batteryMonitor) { - batteryPercentage = _hal->batteryMonitor->getPercentage()*10; - batteryVoltage = _hal->batteryMonitor->getVoltage()*1000; + if (_board->batteryMonitor) { + batteryPercentage = _board->batteryMonitor->getPercentage()*10; + batteryVoltage = _board->batteryMonitor->getVoltage()*1000; } mavlink_msg_sys_status_send(_channel, _controller->getMode(), - _guide->getMode(), _controller->getState(), _hal->load * 10, + _guide->getMode(), _controller->getState(), _board->load * 10, batteryVoltage, batteryPercentage, _packetDrops); break; } @@ -218,7 +218,7 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) { } // send message void MavlinkComm::receive() { - //_hal->debug->printf_P(PSTR("receive\n")); + //_board->debug->printf_P(PSTR("receive\n")); // if number of channels exceeded return // if (_channel == MAVLINK_COMM_3) @@ -264,7 +264,7 @@ void MavlinkComm::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) { * sends parameters one at a time */ void MavlinkComm::sendParameters() { - //_hal->debug->printf_P(PSTR("send parameters\n")); + //_board->debug->printf_P(PSTR("send parameters\n")); // Check to see if we are sending parameters while (NULL != _queuedParameter) { AP_Var *vp; @@ -295,7 +295,7 @@ void MavlinkComm::sendParameters() { * request commands one at a time */ void MavlinkComm::requestCmds() { - //_hal->debug->printf_P(PSTR("requesting commands\n")); + //_board->debug->printf_P(PSTR("requesting commands\n")); // request cmds one by one if (_receivingCmds && _cmdRequestIndex <= _cmdNumberRequested) { mavlink_msg_waypoint_request_send(_channel, _cmdDestSysId, @@ -308,7 +308,7 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) { uint32_t timeStamp = micros(); switch (msg->msgid) { - _hal->debug->printf_P(PSTR("message received: %d"), msg->msgid); + _board->debug->printf_P(PSTR("message received: %d"), msg->msgid); case MAVLINK_MSG_ID_HEARTBEAT: { mavlink_heartbeat_t packet; @@ -329,9 +329,9 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) { _navigator->setYaw(packet.hdg * deg2Rad); _navigator->setGroundSpeed(packet.v); _navigator->setAirSpeed(packet.v); - //_hal->debug->printf_P(PSTR("received hil gps raw packet\n")); + //_board->debug->printf_P(PSTR("received hil gps raw packet\n")); /* - _hal->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"), + _board->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"), packet.lat, packet.lon, packet.alt); @@ -375,7 +375,7 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) { _navigator->setRollRate(packet.rollspeed); _navigator->setPitchRate(packet.pitchspeed); _navigator->setYawRate(packet.yawspeed); - //_hal->debug->printf_P(PSTR("received hil attitude packet\n")); + //_board->debug->printf_P(PSTR("received hil attitude packet\n")); break; } @@ -502,7 +502,7 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) { if (_checkTarget(packet.target_system, packet.target_component)) break; - _hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq); + _board->debug->printf_P(PSTR("sequence: %d\n"),packet.seq); AP_MavlinkCommand cmd(packet.seq); mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex()); @@ -638,7 +638,7 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) { break; } - _hal->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"), + _board->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"), packet.x, packet.y, packet.z); diff --git a/libraries/APO/AP_CommLink.h b/libraries/APO/AP_CommLink.h index 0718958e60..0cad2ae62b 100644 --- a/libraries/APO/AP_CommLink.h +++ b/libraries/APO/AP_CommLink.h @@ -31,7 +31,7 @@ namespace apo { class AP_Controller; class AP_Navigator; class AP_Guide; -class AP_HardwareAbstractionLayer; +class AP_Board; enum { SEVERITY_LOW, SEVERITY_MED, SEVERITY_HIGH @@ -46,7 +46,7 @@ class AP_CommLink { public: AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide, - AP_Controller * controller, AP_HardwareAbstractionLayer * hal, const uint16_t heartBeatTimeout); + AP_Controller * controller, AP_Board * board, const uint16_t heartBeatTimeout); virtual void send() = 0; virtual void receive() = 0; virtual void sendMessage(uint8_t id, uint32_t param = 0) = 0; @@ -69,7 +69,7 @@ protected: AP_Navigator * _navigator; AP_Guide * _guide; AP_Controller * _controller; - AP_HardwareAbstractionLayer * _hal; + AP_Board * _board; uint16_t _heartBeatTimeout; /// vehicle heartbeat timeout, s uint32_t _lastHeartBeat; /// time of last heartbeat, s }; @@ -77,7 +77,7 @@ protected: class MavlinkComm: public AP_CommLink { public: MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide, - AP_Controller * controller, AP_HardwareAbstractionLayer * hal, uint16_t heartBeatTimeout); + AP_Controller * controller, AP_Board * board, uint16_t heartBeatTimeout); virtual void send(); void sendMessage(uint8_t id, uint32_t param = 0); diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp index c6d12881e1..40219af450 100644 --- a/libraries/APO/AP_Controller.cpp +++ b/libraries/APO/AP_Controller.cpp @@ -8,7 +8,7 @@ #include "../FastSerial/FastSerial.h" #include "AP_ArmingMechanism.h" #include "AP_BatteryMonitor.h" -#include "AP_HardwareAbstractionLayer.h" +#include "AP_Board.h" #include "AP_RcChannel.h" #include "../GCS_MAVLink/include/mavlink_types.h" #include "constants.h" @@ -18,9 +18,9 @@ namespace apo { AP_Controller::AP_Controller(AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal, AP_ArmingMechanism * armingMechanism, + AP_Board * board, AP_ArmingMechanism * armingMechanism, const uint8_t chMode, const uint16_t key, const prog_char_t * name) : - _nav(nav), _guide(guide), _hal(hal), _armingMechanism(armingMechanism), + _nav(nav), _guide(guide), _board(board), _armingMechanism(armingMechanism), _group(key, name ? : PSTR("CNTRL_")), _chMode(chMode), _mode(MAV_MODE_LOCKED), _state(MAV_STATE_BOOT) { @@ -28,16 +28,16 @@ AP_Controller::AP_Controller(AP_Navigator * nav, AP_Guide * guide, } void AP_Controller::setAllRadioChannelsToNeutral() { - for (uint8_t i = 0; i < _hal->rc.getSize(); i++) { - _hal->rc[i]->setPosition(0.0); + for (uint8_t i = 0; i < _board->rc.getSize(); i++) { + _board->rc[i]->setPosition(0.0); } } void AP_Controller::setAllRadioChannelsManually() { - //_hal->debug->printf_P(PSTR("\tsize: %d\n"),_hal->rc.getSize()); - for (uint8_t i = 0; i < _hal->rc.getSize(); i++) { - _hal->rc[i]->setUsingRadio(); - //_hal->debug->printf_P(PSTR("\trc %d: %f\n"),i,_hal->rc[i]->getPosition()); + //_board->debug->printf_P(PSTR("\tsize: %d\n"),_board->rc.getSize()); + for (uint8_t i = 0; i < _board->rc.getSize(); i++) { + _board->rc[i]->setUsingRadio(); + //_board->debug->printf_P(PSTR("\trc %d: %f\n"),i,_board->rc[i]->getPosition()); } } @@ -50,7 +50,7 @@ void AP_Controller::update(const float dt) { // if state is not stanby then set it to standby and alert gcs if (getState()!=MAV_STATE_STANDBY) { setState(MAV_STATE_STANDBY); - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed")); + _board->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed")); } } // if not locked @@ -59,24 +59,24 @@ void AP_Controller::update(const float dt) { // if state is not active, set it to active and alert gcs if (getState()!=MAV_STATE_ACTIVE) { setState(MAV_STATE_ACTIVE); - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); + _board->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); } // handle emergencies // // check for heartbeat from gcs, if not found go to failsafe - if (_hal->gcs->heartBeatLost()) { + if (_board->gcs->heartBeatLost()) { setMode(MAV_MODE_FAILSAFE); - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat")); + _board->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat")); // if battery less than 5%, go to failsafe - } else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) { + } else if (_board->batteryMonitor && _board->batteryMonitor->getPercentage() < 5) { setMode(MAV_MODE_FAILSAFE); - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery")); + _board->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery")); } // if in auto mode and manual switch set, change to manual - if (_hal->rc[_chMode]->getRadioPosition() > 0) setMode(MAV_MODE_MANUAL); + if (_board->rc[_chMode]->getRadioPosition() > 0) setMode(MAV_MODE_MANUAL); else setMode(MAV_MODE_AUTO); // handle all possible modes @@ -87,17 +87,17 @@ void AP_Controller::update(const float dt) { } else if (getMode()==MAV_MODE_FAILSAFE) { handleFailsafe(); } else { - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode")); + _board->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode")); setMode(MAV_MODE_FAILSAFE); } } // this sends commands to motors if(getState()==MAV_STATE_ACTIVE) { - digitalWrite(_hal->aLedPin, HIGH); + digitalWrite(_board->aLedPin, HIGH); setMotors(); } else { - digitalWrite(_hal->aLedPin, LOW); + digitalWrite(_board->aLedPin, LOW); setAllRadioChannelsToNeutral(); } } diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h index cd3c10a81e..e35bf618eb 100644 --- a/libraries/APO/AP_Controller.h +++ b/libraries/APO/AP_Controller.h @@ -29,7 +29,7 @@ namespace apo { // forward declarations within apo -class AP_HardwareAbstractionLayer; +class AP_Board; class AP_Guide; class AP_Navigator; class Menu; @@ -48,13 +48,13 @@ public: // Creates the control system. // @nav the navigation system // @guide the guidance system - // @hal the hardware abstraction layer + // @board the hardware abstraction layer // @armingMechanism the device that controls arming/ disarming // @chMode the channel that the mode switch is on // @key the unique key for the control system saved AP_Var variables // @name the name of the control system AP_Controller(AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal, + AP_Board * board, AP_ArmingMechanism * armingMechanism, const uint8_t _chMode, const uint16_t key, @@ -134,7 +134,7 @@ public: protected: AP_Navigator * _nav; /// navigator AP_Guide * _guide; /// guide - AP_HardwareAbstractionLayer * _hal; /// hardware abstraction layer + AP_Board * _board; /// hardware abstraction layer AP_ArmingMechanism * _armingMechanism; /// controls arming/ disarming uint8_t _chMode; /// the channel the mode switch is on AP_Var_group _group; /// holds controller parameters diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index 86c24338aa..5ea84bdcb4 100644 --- a/libraries/APO/AP_Guide.cpp +++ b/libraries/APO/AP_Guide.cpp @@ -9,13 +9,13 @@ #include "../FastSerial/FastSerial.h" #include "AP_Navigator.h" #include "constants.h" -#include "AP_HardwareAbstractionLayer.h" +#include "AP_Board.h" #include "AP_CommLink.h" namespace apo { -AP_Guide::AP_Guide(AP_Navigator * nav, AP_HardwareAbstractionLayer * hal) : - _nav(nav), _hal(hal), _command(AP_MavlinkCommand::home), +AP_Guide::AP_Guide(AP_Navigator * nav, AP_Board * board) : + _nav(nav), _board(board), _command(AP_MavlinkCommand::home), _previousCommand(AP_MavlinkCommand::home), _headingCommand(0), _airSpeedCommand(0), _groundSpeedCommand(0), _altitudeCommand(0), @@ -28,7 +28,7 @@ void AP_Guide::setCurrentIndex(uint8_t val) { _cmdIndex.set_and_save(val); _command = AP_MavlinkCommand(getCurrentIndex()); _previousCommand = AP_MavlinkCommand(getPreviousIndex()); - _hal->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT); + _board->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT); updateCommand(); } @@ -47,8 +47,8 @@ float AP_Guide::getGroundSpeedError() { } MavlinkGuide::MavlinkGuide(AP_Navigator * nav, - AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) : - AP_Guide(nav, hal), + AP_Board * board, float velCmd, float xt, float xtLim) : + AP_Guide(nav, board), _group(k_guide, PSTR("guide_")), _velocityCommand(&_group, 1, velCmd, PSTR("velCmd")), _crossTrackGain(&_group, 2, xt, PSTR("xt")), @@ -113,8 +113,8 @@ void MavlinkGuide::updateCommand() { } else if (_command.getCommand() == MAV_CMD_NAV_TAKEOFF) { _mode = MAV_NAV_LIFTOFF; } else { - _hal->debug->printf_P(PSTR("unhandled command")); - _hal->gcs->sendText(SEVERITY_HIGH,PSTR("unhandled command")); + _board->debug->printf_P(PSTR("unhandled command")); + _board->gcs->sendText(SEVERITY_HIGH,PSTR("unhandled command")); nextCommand(); return; } @@ -162,8 +162,8 @@ void MavlinkGuide::handleCommand() { // check if we are at waypoint or if command // radius is zero within tolerance if ( (getDistanceToNextWaypoint() < _command.getRadius()) | (getDistanceToNextWaypoint() < 1e-5) ) { - _hal->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (distance)")); - _hal->debug->printf_P(PSTR("waypoint reached (distance)")); + _board->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (distance)")); + _board->debug->printf_P(PSTR("waypoint reached (distance)")); nextCommand(); return; } @@ -174,8 +174,8 @@ void MavlinkGuide::handleCommand() { _nav->getLon_degInt()); float segmentLength = _previousCommand.distanceTo(_command); if (alongTrack > segmentLength) { - _hal->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (along track)")); - _hal->debug->printf_P(PSTR("waypoint reached (along track) segmentLength: %f\t alongTrack: %f\n"),segmentLength,alongTrack); + _board->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (along track)")); + _board->debug->printf_P(PSTR("waypoint reached (along track) segmentLength: %f\t alongTrack: %f\n"),segmentLength,alongTrack); nextCommand(); return; } @@ -193,7 +193,7 @@ void MavlinkGuide::handleCommand() { float bearing = _previousCommand.bearingTo(_command); _headingCommand = bearing - temp; _yawCommand = _command.getYawCommand(); - //_hal->debug->printf_P( + //_board->debug->printf_P( //PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\tyaw command: %f\n"), //bearing * rad2Deg, dXt, _headingCommand * rad2Deg, getDistanceToNextWaypoint(), alongTrack, _yawCommand*rad2Deg); @@ -218,15 +218,15 @@ void MavlinkGuide::handleCommand() { // if in unhandled mode, then return else { - _hal->debug->printf_P(PSTR("unhandled guide mode")); - _hal->gcs->sendText(SEVERITY_HIGH,PSTR("unhandled guide mode")); + _board->debug->printf_P(PSTR("unhandled guide mode")); + _board->gcs->sendText(SEVERITY_HIGH,PSTR("unhandled guide mode")); _mode = MAV_NAV_RETURNING; } _groundSpeedCommand = _velocityCommand; // debug - //_hal->debug->printf_P( + //_board->debug->printf_P( //PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"), //getNumberOfCommands(), getCurrentIndex(), getPreviousIndex()); } diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index 95d65069df..86df9d6c92 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -27,7 +27,7 @@ namespace apo { class AP_Navigator; -class AP_HardwareAbstractionLayer; +class AP_Board; /// Guide class class AP_Guide { @@ -37,7 +37,7 @@ public: * This is the constructor, which requires a link to the navigator. * @param navigator This is the navigator pointer. */ - AP_Guide(AP_Navigator * nav, AP_HardwareAbstractionLayer * hal); + AP_Guide(AP_Navigator * nav, AP_Board * board); virtual void update() = 0; @@ -128,7 +128,7 @@ public: protected: AP_Navigator * _nav; - AP_HardwareAbstractionLayer * _hal; + AP_Board * _board; AP_MavlinkCommand _command, _previousCommand; float _headingCommand; float _yawCommand; @@ -145,7 +145,7 @@ protected: class MavlinkGuide: public AP_Guide { public: MavlinkGuide(AP_Navigator * nav, - AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim); + AP_Board * board, float velCmd, float xt, float xtLim); virtual void update(); void nextCommand(); void handleCommand(); diff --git a/libraries/APO/AP_HardwareAbstractionLayer.cpp b/libraries/APO/AP_HardwareAbstractionLayer.cpp deleted file mode 100644 index ee8804811d..0000000000 --- a/libraries/APO/AP_HardwareAbstractionLayer.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/* - * AP_HardwareAbstractionLayer.cpp - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - - -// Libraries -#include -#include -#include "AP_HardwareAbstractionLayer.h" -#include -#include -#include -#include - -namespace apo { - -// default ctors on pointers called on pointers here, this -// allows NULL to be used as a boolean for if the device was -// initialized -AP_HardwareAbstractionLayer::AP_HardwareAbstractionLayer(halMode_t mode, board_t board, MAV_TYPE vehicle) : - adc(), gps(), baro(), compass(), rangeFinders(), imu(), batteryMonitor(), - radio(), rc(), gcs(), - hil(), debug(), load(), _mode(mode), - _board(board), _vehicle(vehicle) { - - - AP_Var::load_all(); - - /* - * Board specific hardware initialization - */ - if (board == BOARD_ARDUPILOTMEGA_1280) { - slideSwitchPin = 40; - pushButtonPin = 41; - aLedPin = 37; - bLedPin = 36; - cLedPin = 35; - eepromMaxAddr = 1024; - pinMode(aLedPin, OUTPUT); // extra led - pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; - pinMode(cLedPin, OUTPUT); // gps led - pinMode(slideSwitchPin, INPUT); - pinMode(pushButtonPin, INPUT); - DDRL |= B00000100; // set port L, pint 2 to output for the relay - adc = new AP_ADC_ADS7844; - radio = new APM_RC_APM1; - } else if (board == BOARD_ARDUPILOTMEGA_2560) { - slideSwitchPin = 40; - pushButtonPin = 41; - aLedPin = 37; - bLedPin = 36; - cLedPin = 35; - eepromMaxAddr = 2048; - pinMode(aLedPin, OUTPUT); // extra led - pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; - pinMode(cLedPin, OUTPUT); // gps led - pinMode(slideSwitchPin, INPUT); - pinMode(pushButtonPin, INPUT); - DDRL |= B00000100; // set port L, pint 2 to output for the relay - adc = new AP_ADC_ADS7844; - radio = new APM_RC_APM1; - } else if (board == BOARD_ARDUPILOTMEGA_2) { - slideSwitchPin = 40; - pushButtonPin = 41; - aLedPin = 37; - bLedPin = 36; - cLedPin = 35; - eepromMaxAddr = 2048; - pinMode(aLedPin, OUTPUT); // extra led - pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; - pinMode(cLedPin, OUTPUT); // gps led - pinMode(slideSwitchPin, INPUT); - pinMode(pushButtonPin, INPUT); - DDRL |= B00000100; // set port L, pint 2 to output for the relay - /// FIXME adc = new ? - adc = new AP_ADC_ADS7844; - radio = new APM_RC_APM2; - } - - // isr - isr_registry = new Arduino_Mega_ISR_Registry; - - // initialize radio - radio->Init(isr_registry); - - // initialize scheduler - scheduler = new AP_TimerProcess; - scheduler->init(isr_registry); - - // initialize the adc - adc->Init(scheduler); -} - -} // apo - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp index 4ce5791f7b..875b724eb7 100644 --- a/libraries/APO/AP_Navigator.cpp +++ b/libraries/APO/AP_Navigator.cpp @@ -10,8 +10,8 @@ namespace apo { -AP_Navigator::AP_Navigator(AP_HardwareAbstractionLayer * hal) : - _hal(hal), _timeStamp(0), _roll(0), _rollRate(0), _pitch(0), +AP_Navigator::AP_Navigator(AP_Board * board) : + _board(board), _timeStamp(0), _roll(0), _rollRate(0), _pitch(0), _pitchRate(0), _yaw(0), _yawRate(0), _windSpeed(0), _windDirection(0), _vN(0), _vE(0), _vD(0), _lat_degInt(0), diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h index 52569c167e..64761975b1 100644 --- a/libraries/APO/AP_Navigator.h +++ b/libraries/APO/AP_Navigator.h @@ -24,12 +24,12 @@ namespace apo { -class AP_HardwareAbstractionLayer; +class AP_Board; /// Navigator class class AP_Navigator { public: - AP_Navigator(AP_HardwareAbstractionLayer * hal); + AP_Navigator(AP_Board * board); // note, override these with derived navigator functionality virtual void calibrate() {}; @@ -268,7 +268,7 @@ public: } protected: - AP_HardwareAbstractionLayer * _hal; + AP_Board * _board; private: int32_t _timeStamp; /// time stamp for navigation data, micros clock float _roll; /// roll angle, radians diff --git a/libraries/APO/Board_APM1.cpp b/libraries/APO/Board_APM1.cpp new file mode 100644 index 0000000000..d1e52cabd1 --- /dev/null +++ b/libraries/APO/Board_APM1.cpp @@ -0,0 +1,184 @@ +/* + * Board_APM1.cpp + * + * Created on: Dec 7, 2011 + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "Board_APM1.h" + +namespace apo { + +Board_APM1::Board_APM1(mode_e mode, MAV_TYPE vehicle, options_t options) : AP_Board(mode,vehicle,options) { + + const uint32_t debugBaud = 57600; + const uint32_t telemBaud = 57600; + const uint32_t gpsBaud = 38400; + const uint32_t hilBaud = 115200; + const uint8_t batteryPin = 0; + const float batteryVoltageDivRatio = 6; + const float batteryMinVolt = 10.0; + const float batteryMaxVolt = 12.4; + Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; + + AP_Var::load_all(); + + Wire.begin(); + + // debug + Serial.begin(debugBaud, 128, 128); + debug = &Serial; + debug->println_P(PSTR("initializing debug line")); + + // gcs + Serial3.begin(telemBaud, 128, 128); + gcsPort = &Serial3; + + // hil + Serial1.begin(hilBaud, 128, 128); + hilPort = &Serial1; + + slideSwitchPin = 40; + pushButtonPin = 41; + aLedPin = 37; + bLedPin = 36; + cLedPin = 35; + + eepromMaxAddr = 1024; + pinMode(aLedPin, OUTPUT); // extra led + pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; + pinMode(cLedPin, OUTPUT); // gps led + pinMode(slideSwitchPin, INPUT); + pinMode(pushButtonPin, INPUT); + DDRL |= B00000100; // set port L, pint 2 to output for the relay + isr_registry = new Arduino_Mega_ISR_Registry; + radio = new APM_RC_APM1; + radio->Init(isr_registry); + dataFlash = new DataFlash_APM1; + scheduler = new AP_TimerProcess; + scheduler->init(isr_registry); + adc = new AP_ADC_ADS7844; + adc->Init(scheduler); + + /* + * Sensor initialization + */ + if (getMode() == MODE_LIVE) { + + if (_options & opt_batteryMonitor) { + batteryMonitor = new AP_BatteryMonitor(batteryPin,batteryVoltageDivRatio,batteryMinVolt,batteryMaxVolt); + } + + if (_options & opt_gps) { + Serial1.begin(gpsBaud, 128, 16); // gps + debug->println_P(PSTR("initializing gps")); + AP_GPS_Auto gpsDriver(&Serial1, &(gps)); + gps = &gpsDriver; + gps->callback = delay; + gps->init(); + } + + if (_options & opt_baro) { + debug->println_P(PSTR("initializing baro")); + baro = new APM_BMP085_Class; + baro->Init(0,false); + } + + if (_options & opt_compass) { + debug->println_P(PSTR("initializing compass")); + compass = new AP_Compass_HMC5843; + compass->set_orientation(compassOrientation); + compass->set_offsets(0,0,0); + compass->set_declination(0.0); + 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 NU/LL 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 defined as (front/back,left/right,down,up) + */ + + // XXX this isn't really that general, should be a better way + + if (_options & opt_rangeFinderFront) { + debug->println_P(PSTR("initializing front range finder")); + RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(1),new ModeFilter); + rangeFinder->set_orientation(1, 0, 0); + rangeFinders.push_back(rangeFinder); + } + + if (_options & opt_rangeFinderBack) { + debug->println_P(PSTR("initializing back range finder")); + RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(2),new ModeFilter); + rangeFinder->set_orientation(-1, 0, 0); + rangeFinders.push_back(rangeFinder); + } + + if (_options & opt_rangeFinderLeft) { + debug->println_P(PSTR("initializing left range finder")); + RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(3),new ModeFilter); + rangeFinder->set_orientation(0, -1, 0); + rangeFinders.push_back(rangeFinder); + } + + if (_options & opt_rangeFinderRight) { + debug->println_P(PSTR("initializing right range finder")); + RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(4),new ModeFilter); + rangeFinder->set_orientation(0, 1, 0); + rangeFinders.push_back(rangeFinder); + } + + if (_options & opt_rangeFinderUp) { + debug->println_P(PSTR("initializing up range finder")); + RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(5),new ModeFilter); + rangeFinder->set_orientation(0, 0, -1); + rangeFinders.push_back(rangeFinder); + } + + if (_options & opt_rangeFinderDown) { + debug->println_P(PSTR("initializing down range finder")); + RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(6),new ModeFilter); + rangeFinder->set_orientation(0, 0, 1); + rangeFinders.push_back(rangeFinder); + } + + /* + * navigation sensors + */ + debug->println_P(PSTR("initializing imu")); + ins = new AP_InertialSensor_Oilpan(adc); + ins->init(scheduler); + //ins = new AP_InertialSensor_MPU6000(mpu6000SelectPin) + debug->println_P(PSTR("initializing ins")); + imu = new AP_IMU_INS(ins, k_sensorCalib); + imu->init(IMU::WARM_START,delay,scheduler); + debug->println_P(PSTR("setup completed")); +} + +} // namespace apo + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/Board_APM1.h b/libraries/APO/Board_APM1.h new file mode 100644 index 0000000000..506ce310d7 --- /dev/null +++ b/libraries/APO/Board_APM1.h @@ -0,0 +1,24 @@ +/* + * Board_APM1.h + * + * Created on: Dec 7, 2011 + * + */ + +#ifndef Board_APM1_H_ +#define Board_APM1_H_ + +#include "AP_Board.h" + +namespace apo { + +class Board_APM1 : public AP_Board { +public: + Board_APM1(mode_e mode, MAV_TYPE vehicle, options_t options); +private: +}; + +} // namespace apo + +#endif /* AP_BOARD_APM1_H_ */ +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/Board_APM1_2560.cpp b/libraries/APO/Board_APM1_2560.cpp new file mode 100644 index 0000000000..6fa0e9c595 --- /dev/null +++ b/libraries/APO/Board_APM1_2560.cpp @@ -0,0 +1,14 @@ +/* + * Board_APM1_2560.cpp + * + * Created on: Dec 7, 2011 + * + */ + +#include "Board_APM1_2560.h" + +namespace apo { + +} // namespace apo + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/Board_APM1_2560.h b/libraries/APO/Board_APM1_2560.h new file mode 100644 index 0000000000..6465d93b63 --- /dev/null +++ b/libraries/APO/Board_APM1_2560.h @@ -0,0 +1,26 @@ +/* + * Board_APM1_2560.h + * + * Created on: Dec 7, 2011 + * + */ + +#ifndef Board_APM1_2560_H_ +#define Board_APM1_2560_H_ + +#include "Board_APM1.h" + +namespace apo { + +class Board_APM1_2560 : public Board_APM1 { +public: + Board_APM1_2560(AP_Board::mode_e mode, MAV_TYPE vehicle, options_t options) : Board_APM1(mode,vehicle,options) { + eepromMaxAddr = 1024; + } +private: +}; + +} // namespace apo + +#endif /* AP_BOARD_APM1_2560_H_ */ +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/Board_APM2.cpp b/libraries/APO/Board_APM2.cpp new file mode 100644 index 0000000000..9987c41469 --- /dev/null +++ b/libraries/APO/Board_APM2.cpp @@ -0,0 +1,184 @@ +/* + * Board_APM2.cpp + * + * Created on: Dec 7, 2011 + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "Board_APM2.h" + +namespace apo { + +Board_APM2::Board_APM2(mode_e mode, MAV_TYPE vehicle, options_t options) : AP_Board(mode,vehicle,options) { + + const uint32_t debugBaud = 57600; + const uint32_t telemBaud = 57600; + const uint32_t gpsBaud = 38400; + const uint32_t hilBaud = 115200; + const uint8_t batteryPin = 0; + const float batteryVoltageDivRatio = 6; + const float batteryMinVolt = 10.0; + const float batteryMaxVolt = 12.4; + Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; + + AP_Var::load_all(); + + Wire.begin(); + + // debug + Serial.begin(debugBaud, 128, 128); + debug = &Serial; + debug->println_P(PSTR("initializing debug line")); + + // gcs + Serial2.begin(telemBaud, 128, 128); + gcsPort = &Serial2; + + // hil + Serial1.begin(hilBaud, 128, 128); + hilPort = &Serial1; + + slideSwitchPin = 40; + pushButtonPin = 41; + aLedPin = 37; + bLedPin = 36; + cLedPin = 35; + + eepromMaxAddr = 1024; + pinMode(aLedPin, OUTPUT); // extra led + pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; + pinMode(cLedPin, OUTPUT); // gps led + pinMode(slideSwitchPin, INPUT); + pinMode(pushButtonPin, INPUT); + DDRL |= B00000100; // set port L, pint 2 to output for the relay + isr_registry = new Arduino_Mega_ISR_Registry; + radio = new APM_RC_APM2; + radio->Init(isr_registry); + dataFlash = new DataFlash_APM2; + scheduler = new AP_TimerProcess; + scheduler->init(isr_registry); + adc = new AP_ADC_ADS7844; + adc->Init(scheduler); + + /* + * Sensor initialization + */ + if (getMode() == MODE_LIVE) { + + if (_options & opt_batteryMonitor) { + batteryMonitor = new AP_BatteryMonitor(batteryPin,batteryVoltageDivRatio,batteryMinVolt,batteryMaxVolt); + } + + if (_options & opt_gps) { + Serial1.begin(gpsBaud, 128, 16); // gps + debug->println_P(PSTR("initializing gps")); + AP_GPS_Auto gpsDriver(&Serial1, &(gps)); + gps = &gpsDriver; + gps->callback = delay; + gps->init(); + } + + if (_options & opt_baro) { + debug->println_P(PSTR("initializing baro")); + baro = new APM_BMP085_Class; + baro->Init(0,true); + } + + if (_options & opt_compass) { + debug->println_P(PSTR("initializing compass")); + compass = new AP_Compass_HMC5843; + compass->set_orientation(compassOrientation); + compass->set_offsets(0,0,0); + compass->set_declination(0.0); + 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 NU/LL 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 defined as (front/back,left/right,down,up) + */ + + // XXX this isn't really that general, should be a better way + + if (_options & opt_rangeFinderFront) { + debug->println_P(PSTR("initializing front range finder")); + RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(1),new ModeFilter); + rangeFinder->set_orientation(1, 0, 0); + rangeFinders.push_back(rangeFinder); + } + + if (_options & opt_rangeFinderBack) { + debug->println_P(PSTR("initializing back range finder")); + RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(2),new ModeFilter); + rangeFinder->set_orientation(-1, 0, 0); + rangeFinders.push_back(rangeFinder); + } + + if (_options & opt_rangeFinderLeft) { + debug->println_P(PSTR("initializing left range finder")); + RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(3),new ModeFilter); + rangeFinder->set_orientation(0, -1, 0); + rangeFinders.push_back(rangeFinder); + } + + if (_options & opt_rangeFinderRight) { + debug->println_P(PSTR("initializing right range finder")); + RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(4),new ModeFilter); + rangeFinder->set_orientation(0, 1, 0); + rangeFinders.push_back(rangeFinder); + } + + if (_options & opt_rangeFinderUp) { + debug->println_P(PSTR("initializing up range finder")); + RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(5),new ModeFilter); + rangeFinder->set_orientation(0, 0, -1); + rangeFinders.push_back(rangeFinder); + } + + if (_options & opt_rangeFinderDown) { + debug->println_P(PSTR("initializing down range finder")); + RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(6),new ModeFilter); + rangeFinder->set_orientation(0, 0, 1); + rangeFinders.push_back(rangeFinder); + } + + /* + * navigation sensors + */ + debug->println_P(PSTR("initializing imu")); + ins = new AP_InertialSensor_Oilpan(adc); + ins->init(scheduler); + //ins = new AP_InertialSensor_MPU6000(mpu6000SelectPin) + debug->println_P(PSTR("initializing ins")); + imu = new AP_IMU_INS(ins, k_sensorCalib); + imu->init(IMU::WARM_START,delay,scheduler); + debug->println_P(PSTR("setup completed")); +} + +} // namespace apo + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/Board_APM2.h b/libraries/APO/Board_APM2.h new file mode 100644 index 0000000000..b14e6226e7 --- /dev/null +++ b/libraries/APO/Board_APM2.h @@ -0,0 +1,24 @@ +/* + * Board_APM2.h + * + * Created on: Dec 7, 2011 + * + */ + +#ifndef Board_APM2_H_ +#define Board_APM2_H_ + +#include "AP_Board.h" + +namespace apo { + +class Board_APM2 : public AP_Board { +public: + Board_APM2(mode_e mode, MAV_TYPE vehicle, options_t options); +private: +}; + +} // namespace apo + +#endif /* AP_BOARD_APM2_H_ */ +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/DcmNavigator.cpp b/libraries/APO/Navigator_Dcm.cpp similarity index 54% rename from libraries/APO/DcmNavigator.cpp rename to libraries/APO/Navigator_Dcm.cpp index 71f75ac359..c5949d5fe3 100644 --- a/libraries/APO/DcmNavigator.cpp +++ b/libraries/APO/Navigator_Dcm.cpp @@ -1,14 +1,14 @@ /* - * DcmNavigator.cpp + * Navigator_Dcm.cpp * * Created on: Dec 6, 2011 * Author: jgoppert/ wenyaoxie */ #include "../FastSerial/FastSerial.h" -#include "DcmNavigator.h" +#include "Navigator_Dcm.h" #include "AP_CommLink.h" -#include "AP_HardwareAbstractionLayer.h" +#include "AP_Board.h" #include "../AP_DCM/AP_DCM.h" #include "../AP_Math/AP_Math.h" #include "../AP_Compass/AP_Compass.h" @@ -22,9 +22,9 @@ namespace apo { -DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal, const uint16_t key, const prog_char_t * name) : - AP_Navigator(hal), _imuOffsetAddress(0), - _dcm(_hal->imu, _hal->gps, _hal->compass), +Navigator_Dcm::Navigator_Dcm(AP_Board * board, const uint16_t key, const prog_char_t * name) : + AP_Navigator(board), _imuOffsetAddress(0), + _dcm(_board->imu, _board->gps, _board->compass), _rangeFinderDown(), _group(key, name ? : PSTR("NAV_")), _baroLowPass(&_group,1,10,PSTR("BAROLP")), @@ -36,13 +36,13 @@ DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal, const uint16_t key * rangeFinder is assigned values based on orientation which * is specified in ArduPilotOne.pde. */ - for (uint8_t i = 0; i < _hal-> rangeFinders.getSize(); i++) { - if (_hal->rangeFinders[i] == NULL) + for (uint8_t i = 0; i < _board-> rangeFinders.getSize(); i++) { + if (_board->rangeFinders[i] == NULL) continue; - if (_hal->rangeFinders[i]->orientation_x == 0 - && _hal->rangeFinders[i]->orientation_y == 0 - && _hal->rangeFinders[i]->orientation_z == 1) - _rangeFinderDown = _hal->rangeFinders[i]; + if (_board->rangeFinders[i]->orientation_x == 0 + && _board->rangeFinders[i]->orientation_y == 0 + && _board->rangeFinders[i]->orientation_z == 1) + _rangeFinderDown = _board->rangeFinders[i]; } // tune down dcm @@ -53,49 +53,49 @@ DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal, const uint16_t key _dcm.kp_yaw(0.08); _dcm.ki_yaw(0); - if (_hal->compass) { - _dcm.set_compass(_hal->compass); + if (_board->compass) { + _dcm.set_compass(_board->compass); } } -void DcmNavigator::calibrate() { +void Navigator_Dcm::calibrate() { AP_Navigator::calibrate(); // TODO: handle cold/warm restart - if (_hal->imu) { - _hal->imu->init(IMU::COLD_START,delay,_hal->scheduler); + if (_board->imu) { + _board->imu->init(IMU::COLD_START,delay,_board->scheduler); } - if (_hal->baro) { + if (_board->baro) { int flashcount = 0; while(_groundPressure == 0){ - _hal->baro->Read(); // Get initial data from absolute pressure sensor - _groundPressure = _hal->baro->Press; - _groundTemperature = _hal->baro->Temp/10.0; + _board->baro->Read(); // Get initial data from absolute pressure sensor + _groundPressure = _board->baro->Press; + _groundTemperature = _board->baro->Temp/10.0; delay(20); } for(int i = 0; i < 30; i++){ // We take some readings... // set using low pass filters - _groundPressure = _groundPressure * 0.9 + _hal->baro->Press * 0.1; - _groundTemperature = _groundTemperature * 0.9 + (_hal->baro->Temp/10.0) * 0.1; + _groundPressure = _groundPressure * 0.9 + _board->baro->Press * 0.1; + _groundTemperature = _groundTemperature * 0.9 + (_board->baro->Temp/10.0) * 0.1; //mavlink_delay(20); delay(20); if(flashcount == 5) { - digitalWrite(_hal->cLedPin, LOW); - digitalWrite(_hal->aLedPin, HIGH); - digitalWrite(_hal->bLedPin, LOW); + digitalWrite(_board->cLedPin, LOW); + digitalWrite(_board->aLedPin, HIGH); + digitalWrite(_board->bLedPin, LOW); } if(flashcount >= 10) { flashcount = 0; - digitalWrite(_hal->cLedPin, LOW); - digitalWrite(_hal->aLedPin, HIGH); - digitalWrite(_hal->bLedPin, LOW); + digitalWrite(_board->cLedPin, LOW); + digitalWrite(_board->aLedPin, HIGH); + digitalWrite(_board->bLedPin, LOW); } flashcount++; } @@ -103,14 +103,14 @@ void DcmNavigator::calibrate() { _groundPressure.save(); _groundTemperature.save(); - _hal->debug->printf_P(PSTR("ground pressure: %ld ground temperature: %d\n"),_groundPressure.get(), _groundTemperature.get()); - _hal->gcs->sendText(SEVERITY_LOW, PSTR("barometer calibration complete\n")); + _board->debug->printf_P(PSTR("ground pressure: %ld ground temperature: %d\n"),_groundPressure.get(), _groundTemperature.get()); + _board->gcs->sendText(SEVERITY_LOW, PSTR("barometer calibration complete\n")); } } -void DcmNavigator::updateFast(float dt) { +void Navigator_Dcm::updateFast(float dt) { - if (_hal->getMode() != MODE_LIVE) + if (_board->getMode() != AP_Board::MODE_LIVE) return; setTimeStamp(micros()); // if running in live mode, record new time stamp @@ -120,7 +120,7 @@ void DcmNavigator::updateFast(float dt) { setAlt(_rangeFinderDown->distance); // otherwise if you have a baro attached, use it - } else if (_hal->baro) { + } else if (_board->baro) { /** * The altitued is read off the barometer by implementing the following formula: * altitude (in m) = 44330*(1-(p/po)^(1/5.255)), @@ -132,14 +132,14 @@ void DcmNavigator::updateFast(float dt) { * pressure input is in pascals * temp input is in deg C *10 */ - _hal->baro->Read(); // Get new data from absolute pressure sensor + _board->baro->Read(); // Get new data from absolute pressure sensor float reference = 44330 * (1.0 - (pow(_groundPressure.get()/101325.0,0.190295))); - setAlt(_baroLowPass.update((44330 * (1.0 - (pow((_hal->baro->Press/101325.0),0.190295)))) - reference,dt)); - //_hal->debug->printf_P(PSTR("Ground Pressure %f\tAltitude = %f\tGround Temperature = %f\tPress = %ld\tTemp = %d\n"),_groundPressure.get(),getAlt(),_groundTemperature.get(),_hal->baro->Press,_hal->baro->Temp); + setAlt(_baroLowPass.update((44330 * (1.0 - (pow((_board->baro->Press/101325.0),0.190295)))) - reference,dt)); + //_board->debug->printf_P(PSTR("Ground Pressure %f\tAltitude = %f\tGround Temperature = %f\tPress = %ld\tTemp = %d\n"),_groundPressure.get(),getAlt(),_groundTemperature.get(),_board->baro->Press,_board->baro->Temp); // last resort, use gps altitude - } else if (_hal->gps && _hal->gps->fix) { - setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm + } else if (_board->gps && _board->gps->fix) { + setAlt_intM(_board->gps->altitude * 10); // gps in cm, intM in mm } // update dcm calculations and navigator data @@ -159,59 +159,59 @@ void DcmNavigator::updateFast(float dt) { * accel/gyro debug */ /* - Vector3f accel = _hal->imu->get_accel(); - Vector3f gyro = _hal->imu->get_gyro(); + Vector3f accel = _board->imu->get_accel(); + Vector3f gyro = _board->imu->get_gyro(); Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"), accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z); */ } -void DcmNavigator::updateSlow(float dt) { - if (_hal->getMode() != MODE_LIVE) +void Navigator_Dcm::updateSlow(float dt) { + if (_board->getMode() != AP_Board::MODE_LIVE) return; setTimeStamp(micros()); // if running in live mode, record new time stamp - if (_hal->gps) { - _hal->gps->update(); + if (_board->gps) { + _board->gps->update(); updateGpsLight(); - if (_hal->gps->fix && _hal->gps->new_data) { - setLat_degInt(_hal->gps->latitude); - setLon_degInt(_hal->gps->longitude); - setGroundSpeed(_hal->gps->ground_speed / 100.0); // gps is in cm/s + if (_board->gps->fix && _board->gps->new_data) { + setLat_degInt(_board->gps->latitude); + setLon_degInt(_board->gps->longitude); + setGroundSpeed(_board->gps->ground_speed / 100.0); // gps is in cm/s } } - if (_hal->compass) { - _hal->compass->read(); - _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); + if (_board->compass) { + _board->compass->read(); + _board->compass->calculate(_dcm.get_dcm_matrix()); + _board->compass->null_offsets(_dcm.get_dcm_matrix()); + //_board->debug->printf_P(PSTR("heading: %f"), _board->compass->heading); } } -void DcmNavigator::updateGpsLight(void) { +void Navigator_Dcm::updateGpsLight(void) { // GPS LED on if we have a fix or Blink GPS LED if we are receiving data // --------------------------------------------------------------------- static bool GPS_light = false; - switch (_hal->gps->status()) { + switch (_board->gps->status()) { case (2): //digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. break; case (1): - if (_hal->gps->valid_read == true) { + if (_board->gps->valid_read == true) { GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock if (GPS_light) { - digitalWrite(_hal->cLedPin, LOW); + digitalWrite(_board->cLedPin, LOW); } else { - digitalWrite(_hal->cLedPin, HIGH); + digitalWrite(_board->cLedPin, HIGH); } - _hal->gps->valid_read = false; + _board->gps->valid_read = false; } break; default: - digitalWrite(_hal->cLedPin, LOW); + digitalWrite(_board->cLedPin, LOW); break; } } diff --git a/libraries/APO/DcmNavigator.h b/libraries/APO/Navigator_Dcm.h similarity index 84% rename from libraries/APO/DcmNavigator.h rename to libraries/APO/Navigator_Dcm.h index e3a308fdae..015e6624e5 100644 --- a/libraries/APO/DcmNavigator.h +++ b/libraries/APO/Navigator_Dcm.h @@ -1,5 +1,5 @@ /* - * DcmNavigator.h + * Navigator_Dcm.h * Copyright (C) James Goppert/ Wenyao Xie 2011 james.goppert@gmail.com/ wenyaoxie@gmail.com * * This file is free software: you can redistribute it and/or modify it @@ -16,8 +16,8 @@ * with this program. If not, see . */ -#ifndef DcmNavigator_H -#define DcmNavigator_H +#ifndef Navigator_Dcm_H +#define Navigator_Dcm_H #include "AP_Navigator.h" #include @@ -28,9 +28,9 @@ class RangeFinder; namespace apo { -class DcmNavigator: public AP_Navigator { +class Navigator_Dcm: public AP_Navigator { public: - DcmNavigator(AP_HardwareAbstractionLayer * hal, const uint16_t key, const prog_char_t * name = NULL); + Navigator_Dcm(AP_Board * board, const uint16_t key, const prog_char_t * name = NULL); virtual void calibrate(); virtual void updateFast(float dt); virtual void updateSlow(float dt); @@ -47,5 +47,5 @@ private: } // namespace apo -#endif // DcmNavigator_H +#endif // Navigator_Dcm_H // vim:ts=4:sw=4:expandtab