Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added AP_Board to APO

This commit is contained in:
James Goppert 2011-12-07 16:31:56 -05:00
parent c4203631dc
commit a25353da79
36 changed files with 936 additions and 821 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
ArduBoat/BoatGeneric.h
View File

@ -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;

38
ArduBoat/ControllerBoat.h
View File

@ -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

30
ArduBoat/ControllerSailboat.h
View File

@ -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

46
ArduBoat/SailboatLaser.h
View File

@ -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

44
ArduRover/CarStampede.h
View File

@ -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

38
ArduRover/ControllerCar.h
View File

@ -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

44
ArduRover/TankGeneric.h
View File

@ -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

44
apo/ControllerPlane.h
View File

@ -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() {

64
apo/ControllerQuad.h
View File

@ -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

46
apo/PlaneEasystar.h
View File

@ -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

31
apo/QuadArducopter.h
View File

@ -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

10
libraries/APO/APO.h
View File

@ -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

164
libraries/APO/APO_DefaultSetup.h
View File

@ -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);
}

20
libraries/APO/AP_ArmingMechanism.cpp
View File

@ -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"));
}
}

8
libraries/APO/AP_ArmingMechanism.h
View File

@ -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

130
libraries/APO/AP_Autopilot.cpp
View File

@ -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

10
libraries/APO/AP_Autopilot.h
View File

@ -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

14
libraries/APO/AP_Board.cpp Normal file
View File

@ -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

55
libraries/APO/AP_HardwareAbstractionLayer.h → libraries/APO/AP_Board.h
View File

@ -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;
};

78
libraries/APO/AP_CommLink.cpp
View File

@ -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);

8
libraries/APO/AP_CommLink.h
View File

@ -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);

38
libraries/APO/AP_Controller.cpp
View File

@ -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();
}
}

8
libraries/APO/AP_Controller.h
View File

@ -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

32
libraries/APO/AP_Guide.cpp
View File

@ -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());
}

8
libraries/APO/AP_Guide.h
View File

@ -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();

99
libraries/APO/AP_HardwareAbstractionLayer.cpp
View File

@ -1,99 +0,0 @@
/*
* AP_HardwareAbstractionLayer.cpp
*
* Created on: Apr 30, 2011
* Author: jgoppert
*/
// Libraries
#include <FastSerial.h>
#include <Arduino_Mega_ISR_Registry.h>
#include "AP_HardwareAbstractionLayer.h"
#include <AP_ADC.h>
#include <APM_RC.h>
#include <AP_AnalogSource.h>
#include <AP_PeriodicProcess.h>
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

4
libraries/APO/AP_Navigator.cpp
View File

@ -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),

6
libraries/APO/AP_Navigator.h
View File

@ -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

184
libraries/APO/Board_APM1.cpp Normal file
View File

@ -0,0 +1,184 @@
/*
* Board_APM1.cpp
*
* Created on: Dec 7, 2011
*
*/
#include <Wire.h>
#include <FastSerial.h>
#include <AP_Common.h>
#include <APM_RC.h>
#include <AP_RangeFinder.h>
#include <GCS_MAVLink.h>
#include <AP_ADC.h>
#include <AP_DCM.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_IMU.h>
#include <APM_BMP085.h>
#include <ModeFilter.h>
#include <APO.h>
#include <AP_AnalogSource.h>
#include <AP_InertialSensor.h>
#include <DataFlash.h>
#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

24
libraries/APO/Board_APM1.h Normal file
View File

@ -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

14
libraries/APO/Board_APM1_2560.cpp Normal file
View File

@ -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

26
libraries/APO/Board_APM1_2560.h Normal file
View File

@ -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

184
libraries/APO/Board_APM2.cpp Normal file
View File

@ -0,0 +1,184 @@
/*
* Board_APM2.cpp
*
* Created on: Dec 7, 2011
*
*/
#include <Wire.h>
#include <FastSerial.h>
#include <AP_Common.h>
#include <APM_RC.h>
#include <AP_RangeFinder.h>
#include <GCS_MAVLink.h>
#include <AP_ADC.h>
#include <AP_DCM.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_IMU.h>
#include <APM_BMP085.h>
#include <ModeFilter.h>
#include <APO.h>
#include <AP_AnalogSource.h>
#include <AP_InertialSensor.h>
#include <DataFlash.h>
#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

24
libraries/APO/Board_APM2.h Normal file
View File

@ -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

122
libraries/APO/DcmNavigator.cpp → libraries/APO/Navigator_Dcm.cpp
View File

@ -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<direction> 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;
}
}

12
libraries/APO/DcmNavigator.h → libraries/APO/Navigator_Dcm.h
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#ifndef DcmNavigator_H
#define DcmNavigator_H
#ifndef Navigator_Dcm_H
#define Navigator_Dcm_H
#include "AP_Navigator.h"
#include <FastSerial.h>
@ -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