zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Fixed along track bug.

master
Jason Kemmerling 13 years ago
parent
603f274ea3
commit
21c3282c9c
4 changed files with 43 additions and 52 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 63
      ArduBoat/ControllerSailboat.h
  2. 12
      ArduBoat/SailboatLaser.h
  3. 6
      libraries/APO/AP_Guide.cpp
  4. 14
      libraries/APO/AP_MavlinkCommand.cpp

63
ArduBoat/ControllerSailboat.h
Unescape View File

@ -32,60 +32,51 @@ public: @@ -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
Unescape View File

@ -14,8 +14,8 @@ using namespace apo; @@ -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; @@ -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; @@ -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
Unescape View File

@ -193,9 +193,9 @@ void MavlinkGuide::handleCommand() { @@ -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
Unescape View File

@ -196,13 +196,13 @@ float AP_MavlinkCommand::crossTrack(const AP_MavlinkCommand & previous, @@ -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;
}

Write Preview
Loading…
Cancel
Save