Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-28 10:43: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

61
ArduBoat/ControllerSailboat.h
View File

@ -32,60 +32,51 @@ public:
new AP_RcChannel(k_chStr, PSTR("STR_"), board->radio, 3, 1100, 1500, new AP_RcChannel(k_chStr, PSTR("STR_"), board->radio, 3, 1100, 1500,
1900, RC_MODE_INOUT, false)); 1900, RC_MODE_INOUT, false));
_board->rc.push_back( _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)); 1900, RC_MODE_INOUT, false));
} }
private: private:
// methods // methods
void manualLoop(const float dt) { void manualLoop(const float dt) {
_strCmd = _hal->rc[ch_str]->getRadioPosition(); _strCmd = -_board->rc[ch_str]->getRadioPosition();
_sailCmd = _hal->rc[ch_sail]->getRadioPosition(); _sailCmd = _board->rc[ch_sail]->getRadioPosition();
_hal->debug->printf_P(PSTR("sail: %f, steering: %f\n"),_sailCmd,_strCmd); _board->debug->printf_P(PSTR("sail: %f, steering: %f\n"),_sailCmd,_strCmd);
} }
void autoLoop(const float dt) { 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; float windDir = -.339373*analogRead(1)+175.999;
// neglects heading command derivative // 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; float sail = 0.00587302*fabs(windDir) - 0.05;
if (sail < 0.0) sail = 0.0; 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(fabs(psi)<45) //Tacking Logic
{ //{
if(psi<-10) //if(psi<-10)
alpha = -45; //alpha = -45;
else if(psi>10) //else if(psi>10)
alpha = 45; //alpha = 45;
else //else
{ //{
if(psi==10) //if(psi==10)
alpha = 45; //alpha = 45;
else if(psi==-10) //else if(psi==-10)
alpha = -45; //alpha = -45;
else //else
alpha = alpha; //alpha = alpha;
} //}
}*/ //}*/
_strCmd = steering; _strCmd = steering;
_sailCmd = sail; _sailCmd = sail;
} }
void setMotors() { void setMotors() {
<<<<<<< HEAD
_hal->rc[ch_str]->setPosition(_strCmd);
_hal->rc[ch_sail]->setPosition(_sailCmd);
=======
_board->rc[ch_str]->setPosition(_strCmd); _board->rc[ch_str]->setPosition(_strCmd);
_board->rc[ch_sail]->setPosition(fabs(_sailCmd) < 0.1 ? 0 : _sailCmd); _board->rc[ch_sail]->setPosition(_sailCmd);
>>>>>>> 8265597d37c8ae9fb88f59dfe89b6077dc14c0d8
} }
void handleFailsafe() { void handleFailsafe() {
// turn off // turn off

12
ArduBoat/SailboatLaser.h
View File

@ -14,8 +14,8 @@ using namespace apo;
// vehicle options // vehicle options
static const AP_Board::options_t options = AP_Board::opt_gps | AP_Board::opt_baro | AP_Board::opt_compass; 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 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_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_LIVE;
static const uint8_t heartBeatTimeout = 0; static const uint8_t heartBeatTimeout = 0;
// algorithm selection // algorithm selection
@ -24,9 +24,9 @@ static const uint8_t heartBeatTimeout = 0;
#define NAVIGATOR_CLASS Navigator_Dcm #define NAVIGATOR_CLASS Navigator_Dcm
// hardware selection // hardware selection
//#define BOARD_TYPE Board_APM1 #define BOARD_TYPE Board_APM1
//#define BOARD_TYPE Board_APM1_2560 //#define BOARD_TYPE Board_APM1_2560
#define BOARD_TYPE Board_APM2 //#define BOARD_TYPE Board_APM2
// loop rates // loop rates
static const float loopRate = 150; // attitude nav 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 throttleP = 0.1;
static const float throttleI = 0.0; 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 throttleIMax = 0.0;
static const float throttleYMax = 1; static const float throttleYMax = 1;
static const float throttleDFCut = 0.0; static const float throttleDFCut = 0.0;
// guidance // guidance
static const float velCmd = 5; static const float velCmd = 2;
static const float xt = 10; static const float xt = 10;
static const float xtLim = 90; 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); float bearing = _previousCommand.bearingTo(_command);
_headingCommand = bearing - temp; _headingCommand = bearing - temp;
_yawCommand = _command.getYawCommand(); _yawCommand = _command.getYawCommand();
//_board->debug->printf_P( _board->debug->printf_P(
//PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\tyaw command: %f\n"), 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); bearing * rad2Deg, dXt, _headingCommand * rad2Deg, getDistanceToNextWaypoint(), alongTrack, _yawCommand*rad2Deg);
// for these modes just head to current command // for these modes just head to current command
} else if ( } 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 // calculates along track distance of a current location
float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous, float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous,
int32_t lat_degInt, int32_t lon_degInt) const { int32_t lat_degInt, int32_t lon_degInt) const {
float dXt = crossTrack(previous,lat_degInt,lon_degInt); float t1N = previous.getPN(lat_degInt, lon_degInt);
float d = previous.distanceTo(getLat_degInt(),getLon_degInt()); float t1E = previous.getPE(lat_degInt, lon_degInt);
float bCurrent = previous.bearingTo(lat_degInt, lon_degInt); float t2N = previous.getPN(getLat_degInt(), getLon_degInt());
float bNext = previous.bearingTo(*this); float t2E = previous.getPE(getLat_degInt(), getLon_degInt());
float y = acos(cos(d/rEarth)/cos(dXt/rEarth))*rEarth; float segmentLength = previous.distanceTo(*this);
if (cos(bCurrent-bNext) < 0) y = -y; if (segmentLength == 0) return 0;
return y; return (t1N*t2N + t1E*t2E)/segmentLength;
} }