diff --git a/ArduBoat/ControllerSailboat.h b/ArduBoat/ControllerSailboat.h index ded0b9a2e2..fa84f9c452 100644 --- a/ArduBoat/ControllerSailboat.h +++ b/ArduBoat/ControllerSailboat.h @@ -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 diff --git a/ArduBoat/SailboatLaser.h b/ArduBoat/SailboatLaser.h index a24ad87819..74b4194438 100644 --- a/ArduBoat/SailboatLaser.h +++ b/ArduBoat/SailboatLaser.h @@ -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; diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index 5ea84bdcb4..82a46d1a24 100644 --- a/libraries/APO/AP_Guide.cpp +++ b/libraries/APO/AP_Guide.cpp @@ -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 ( diff --git a/libraries/APO/AP_MavlinkCommand.cpp b/libraries/APO/AP_MavlinkCommand.cpp index 91ccafd8d1..5b976edb30 100644 --- a/libraries/APO/AP_MavlinkCommand.cpp +++ b/libraries/APO/AP_MavlinkCommand.cpp @@ -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; }