diff --git a/ArduBoat/ControllerSailboat.h b/ArduBoat/ControllerSailboat.h index f7431b07ca..ace0ab33a6 100644 --- a/ArduBoat/ControllerSailboat.h +++ b/ArduBoat/ControllerSailboat.h @@ -19,7 +19,7 @@ public: AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,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, + pidSail(new AP_Var_group(k_pidSail, PSTR("SAIL_")), 1, throttleP, throttleI, throttleD, throttleIMax, throttleYMax, throttleDFCut), _strCmd(0), _sailCmd(0) { @@ -32,7 +32,7 @@ public: new AP_RcChannel(k_chStr, PSTR("STR_"), hal->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, + new AP_RcChannel(k_chSail, PSTR("SAIL_"), hal->radio, 2, 1100, 1100, 1900, RC_MODE_INOUT, false)); } @@ -41,20 +41,46 @@ private: 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); } 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()); - float windDir = analogRead(1); - _hal->debug->printf_P(PSTR("wind directiono: %f\n"),windDir); + float windDir = -.339373*analogRead(1)+175.999; // neglects heading command derivative float steering = pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt); - float sail = 0; + 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 + + 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() { _hal->rc[ch_str]->setPosition(_strCmd); - _hal->rc[ch_sail]->setPosition(fabs(_sailCmd) < 0.1 ? 0 : _sailCmd); + _hal->rc[ch_sail]->setPosition(_sailCmd); } void handleFailsafe() { // turn off @@ -66,13 +92,13 @@ private: ch_mode = 0, ch_str, ch_sail }; enum { - k_chMode = k_radioChannelsStart, k_chStr, k_chsail + k_chMode = k_radioChannelsStart, k_chStr, k_chSail }; enum { - k_pidStr = k_controllersStart, k_pidsail + k_pidStr = k_controllersStart, k_pidSail }; BlockPIDDfb pidStr; - BlockPID pidsail; + BlockPID pidSail; float _strCmd, _sailCmd; }; diff --git a/ArduBoat/SailboatLaser.h b/ArduBoat/SailboatLaser.h index 4bf037496d..c6e52f7677 100644 --- a/ArduBoat/SailboatLaser.h +++ b/ArduBoat/SailboatLaser.h @@ -11,7 +11,8 @@ // vehicle options static const MAV_TYPE vehicle = UGV_SURFACE_SHIP; -static const apo::halMode_t halMode = apo::MODE_LIVE; +//static const apo::halMode_t halMode = apo::MODE_LIVE; +static const apo::halMode_t halMode = apo::MODE_HIL_CNTL; static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; static const uint8_t heartBeatTimeout = 3;