Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-28 02:33:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Fixed along track bug.

Browse Source
This commit is contained in:
Jason Kemmerling 2011-12-07 20:55:06 -05:00
parent c6de814a23
commit d6c904f398
4 changed files with 43 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
ArduBoat/ControllerSailboat.h
View File

@ -32,60 +32,51 @@ public:
new AP_RcChannel(k_chStr, PSTR("STR_"), board->radio, 3, 1100, 1500,
1900, RC_MODE_INOUT, false));
_board->rc.push_back(
new AP_RcChannel(k_chsail, PSTR("SAIL_"), board->radio, 2, 1100, 1500,
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();
_hal->debug->printf_P(PSTR("sail: %f, steering: %f\n"),_sailCmd,_strCmd);
_strCmd = -_board->rc[ch_str]->getRadioPosition();
_sailCmd = _board->rc[ch_sail]->getRadioPosition();
_board->debug->printf_P(PSTR("sail: %f, steering: %f\n"),_sailCmd,_strCmd);
}
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 = -.339373*analogRead(1)+175.999;
// neglects heading command derivative
float steering = pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt);
float steering = -pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt);
float sail = 0.00587302*fabs(windDir) - 0.05;
if (sail < 0.0) sail = 0.0;
_hal->debug->printf_P(PSTR("wind direction: %f, sail: %f, steering: %f\n"),windDir,sail,steering);
/* float calibrate = 0.34; //Calibration Factor from analog reading
* float relwinddir = windDir*calibrate; //Wind Direction Relative to boat
float pathideal; //Path from boat to waypoint
float psi = relwinddir-pathideal; //Angle between relative wind direction and path from boat to waypoint
float alpha = relwinddir-heading; //Angle between relatvive wind direction and the heading
_hal->debug->printf_P(PSTR("heading: %f\n"),heading); //Print Heading
//_board->debug->printf_P(PSTR("heading: %f\n"),heading); //Print Heading
if(fabs(psi)<45) //Tacking Logic
{
if(psi<-10)
alpha = -45;
else if(psi>10)
alpha = 45;
else
{
if(psi==10)
alpha = 45;
else if(psi==-10)
alpha = -45;
else
alpha = alpha;
}
}*/
//if(fabs(psi)<45) //Tacking Logic
//{
//if(psi<-10)
//alpha = -45;
//else if(psi>10)
//alpha = 45;
//else
//{
//if(psi==10)
//alpha = 45;
//else if(psi==-10)
//alpha = -45;
//else
//alpha = alpha;
//}
//}*/
_strCmd = steering;
_sailCmd = sail;
}
void setMotors() {
<<<<<<< HEAD
_hal->rc[ch_str]->setPosition(_strCmd);
_hal->rc[ch_sail]->setPosition(_sailCmd);
=======
_board->rc[ch_str]->setPosition(_strCmd);
_board->rc[ch_sail]->setPosition(fabs(_sailCmd) < 0.1 ? 0 : _sailCmd);
>>>>>>> 8265597d37c8ae9fb88f59dfe89b6077dc14c0d8
_board->rc[ch_sail]->setPosition(_sailCmd);
}
void handleFailsafe() {
// turn off

12
ArduBoat/SailboatLaser.h
View File

@ -14,8 +14,8 @@ 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::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 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
@ -24,9 +24,9 @@ static const uint8_t heartBeatTimeout = 0;
#define NAVIGATOR_CLASS Navigator_Dcm
// hardware selection
//#define BOARD_TYPE Board_APM1
#define BOARD_TYPE Board_APM1
//#define BOARD_TYPE Board_APM1_2560
#define BOARD_TYPE Board_APM2
//#define BOARD_TYPE Board_APM2
// loop rates
static const float loopRate = 150; // attitude nav
@ -45,13 +45,13 @@ static const float steeringDFCut = 25.0;
static const float throttleP = 0.1;
static const float throttleI = 0.0;
static const float throttleD = 0.0;
static const float throttleD = 0.2;
static const float throttleIMax = 0.0;
static const float throttleYMax = 1;
static const float throttleDFCut = 0.0;
// guidance
static const float velCmd = 5;
static const float velCmd = 2;
static const float xt = 10;
static const float xtLim = 90;

6
libraries/APO/AP_Guide.cpp
View File

@ -193,9 +193,9 @@ void MavlinkGuide::handleCommand() {
float bearing = _previousCommand.bearingTo(_command);
_headingCommand = bearing - temp;
_yawCommand = _command.getYawCommand();
//_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);
_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);
// for these modes just head to current command
} else if (

14
libraries/APO/AP_MavlinkCommand.cpp
View File

@ -196,13 +196,13 @@ float AP_MavlinkCommand::crossTrack(const AP_MavlinkCommand & previous,
// calculates along track distance of a current location
float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous,
int32_t lat_degInt, int32_t lon_degInt) const {
float dXt = crossTrack(previous,lat_degInt,lon_degInt);
float d = previous.distanceTo(getLat_degInt(),getLon_degInt());
float bCurrent = previous.bearingTo(lat_degInt, lon_degInt);
float bNext = previous.bearingTo(*this);
float y = acos(cos(d/rEarth)/cos(dXt/rEarth))*rEarth;
if (cos(bCurrent-bNext) < 0) y = -y;
return y;
float t1N = previous.getPN(lat_degInt, lon_degInt);
float t1E = previous.getPE(lat_degInt, lon_degInt);
float t2N = previous.getPN(getLat_degInt(), getLon_degInt());
float t2E = previous.getPE(getLat_degInt(), getLon_degInt());
float segmentLength = previous.distanceTo(*this);
if (segmentLength == 0) return 0;
return (t1N*t2N + t1E*t2E)/segmentLength;
}