zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Adding sailboat controller.

mission-4.1.18
Jason Kemmerling 13 years ago
parent
7fae764c55
commit
cea4759630
4 changed files with 231 additions and 30 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. 5
      ArduBoat/ArduBoat.pde
  2. 52
      ArduBoat/ControllerBoat.h
  3. 117
      ArduBoat/ControllerSailboat.h
  4. 87
      ArduBoat/SailboatLaser.h

5
ArduBoat/ArduBoat.pde
Unescape View File

@ -1,4 +1,5 @@ @@ -1,4 +1,5 @@
// Libraries
#include <Wire.h>
#include <FastSerial.h>
#include <AP_Common.h>
#include <APM_RC.h>
@ -17,8 +18,8 @@ @@ -17,8 +18,8 @@
#include <AP_InertialSensor.h>
// Vehicle Configuration
#include "BoatGeneric.h"
//#include "BoatGeneric.h"
#include "SailboatLaser.h"
// ArduPilotOne Default Setup
#include "APO_DefaultSetup.h"
// vim:ts=4:sw=4:expandtab

52
ArduBoat/ControllerBoat.h
Unescape View File

@ -1,26 +1,24 @@ @@ -1,26 +1,24 @@
/*
* ControllerBoat.h
*
* Created on: Nov 1, 2011
* Created on: Jun 30, 2011
* Author: jgoppert
*/
#ifndef CONTROLLERBOAT_H_
#define CONTROLLERBOAT_H_
#include "../APO/APO.h"
#include "../APO/AP_Controller.h"
namespace apo {
class ControllerBoat: public AP_Controller {
public:
ControllerBoat(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl),
AP_HardwareAbstractionLayer * hal) :
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),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax, throttleDFCut), _strCmd(0), _thrustCmd(0)
steeringI, steeringD, steeringIMax, steeringYMax)
{
_hal->debug->println_P(PSTR("initializing boat controller"));
@ -31,51 +29,49 @@ public: @@ -31,51 +29,49 @@ 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_chThrust, PSTR("THR_"), hal->radio, 2, 1100, 1500,
new AP_RcChannel(k_chSail, PSTR("SAIL_"), hal->radio, 2, 1100, 1500,
1900, RC_MODE_INOUT, false));
}
private:
// methdos
// methods
void manualLoop(const float dt) {
setAllRadioChannelsManually();
_strCmd = _hal->rc[ch_str]->getRadioPosition();
_thrustCmd = _hal->rc[ch_thrust]->getRadioPosition();
_sailCmd = _hal->rc[ch_sail]->getRadioPosition();
}
void autoLoop(const float dt) {
// insert tacking logic here
// neglects heading command derivative
_strCmd = pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt);
_thrustCmd = pidThrust.update(
_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt);
float steering = pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt);
_strCmd = steering;
// insert sail command calculation based on sensor position here
_sailCmd = 0;
}
void setMotors() {
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
} else {
_hal->rc[ch_thrust]->setPosition(_thrustCmd);
_hal->rc[ch_str]->setPosition(_strCmd);
}
_hal->rc[ch_str]->setPosition(_strCmd);
_hal->rc[ch_sail]->setPosition(_sailCmd);
}
void handleFailsafe() {
// failsafe is to turn off
// turn off
setMode(MAV_MODE_LOCKED);
}
// attributes
enum {
ch_mode = 0, ch_str, ch_thrust
ch_mode = 0, ch_str, ch_sail
};
enum {
k_chMode = k_radioChannelsStart, k_chStr, k_chThrust
k_chMode = k_radioChannelsStart, k_chStr, k_chSail
};
enum {
k_pidStr = k_controllersStart, k_pidThrust
k_pidStr = k_controllersStart
};
BlockPIDDfb pidStr;
BlockPID pidThrust;
float _strCmd, _thrustCmd;
float _strCmd;
float _sailCmd;
};
} // namespace apo

117
ArduBoat/ControllerSailboat.h
Unescape View File

@ -0,0 +1,117 @@ @@ -0,0 +1,117 @@
/*
* ControllerSailboat.h
*
* Created on: Jun 30, 2011
* Author: jgoppert
*/
#ifndef CONTROLLERSAILBOAT_H_
#define CONTROLLERSAILBOAT_H_
#include "../APO/AP_Controller.h"
namespace apo {
class ControllerSailboat: public AP_Controller {
public:
ControllerSailboat(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,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),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
throttleDFCut), _strCmd(0), _thrustCmd(0),
_rangeFinderFront()
{
_hal->debug->println_P(PSTR("initializing sailboat controller"));
_hal->rc.push_back(
new AP_RcChannel(k_chMode, PSTR("MODE_"), hal->radio, 5, 1100,
1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
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_chThrust, PSTR("THR_"), hal->radio, 2, 1100, 1500,
1900, RC_MODE_INOUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chFwdRev, PSTR("FWDREV_"), hal->radio, 4, 1100, 1500,
1900, RC_MODE_IN, false));
for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) {
RangeFinder * rF = _hal->rangeFinders[i];
if (rF == NULL)
continue;
if (rF->orientation_x == 1 && rF->orientation_y == 0
&& rF->orientation_z == 0)
_rangeFinderFront = rF;
}
}
private:
// methods
void manualLoop(const float dt) {
_strCmd = _hal->rc[ch_str]->getRadioPosition();
_thrustCmd = _hal->rc[ch_thrust]->getRadioPosition();
if (useForwardReverseSwitch && _hal->rc[ch_FwdRev]->getRadioPosition() < 0.0)
_thrustCmd = -_thrustCmd;
}
void autoLoop(const float dt) {
//_hal->debug->printf_P(PSTR("cont: ch1: %f\tch2: %f\n"),_hal->rc[ch_thrust]->getRadioPosition(), _hal->rc[ch_str]->getRadioPosition());
// neglects heading command derivative
float steering = pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt);
float thrust = pidThrust.update(
_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt);
// obstacle avoidance overrides
// try to drive around the obstacle in front. if this fails, stop
if (_rangeFinderFront) {
_rangeFinderFront->read();
int distanceToObstacle = _rangeFinderFront->distance;
if (distanceToObstacle < 100) {
thrust = 0; // Avoidance didn't work out. Stopping
}
else if (distanceToObstacle < 650) {
// Deviating from the course. Deviation angle is inverse proportional
// to the distance to the obstacle, with 15 degrees min angle, 180 - max
steering += (15 + 165 *
(1 - ((float)(distanceToObstacle - 100)) / 550)) * deg2Rad;
}
}
_strCmd = steering;
_thrustCmd = thrust;
}
void setMotors() {
_hal->rc[ch_str]->setPosition(_strCmd);
_hal->rc[ch_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd);
}
void handleFailsafe() {
// turn off
setMode(MAV_MODE_LOCKED);
}
// attributes
enum {
ch_mode = 0, ch_str, ch_thrust, ch_FwdRev
};
enum {
k_chMode = k_radioChannelsStart, k_chStr, k_chThrust, k_chFwdRev
};
enum {
k_pidStr = k_controllersStart, k_pidThrust
};
BlockPIDDfb pidStr;
BlockPID pidThrust;
float _strCmd, _thrustCmd;
RangeFinder * _rangeFinderFront;
};
} // namespace apo
#endif /* CONTROLLERSAILBOAT_H_ */
// vim:ts=4:sw=4:expandtab

87
ArduBoat/SailboatLaser.h
Unescape View File

@ -0,0 +1,87 @@ @@ -0,0 +1,87 @@
/*
* SailboatLaser.h
*
* Created on: May 1, 2011
* Author: jgoppert
*
*/
#ifndef SAILBOATLASER_H_
#define SAILBOATLASER_H_
// vehicle options
static const MAV_TYPE vehicle = UGV_SURFACE_SHIP;
static const apo::halMode_t halMode = apo::MODE_LIVE;
static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
static const uint8_t heartBeatTimeout = 3;
// algorithm selection
#define CONTROLLER_CLASS ControllerSailboat
#define GUIDE_CLASS MavlinkGuide
#define NAVIGATOR_CLASS DcmNavigator
#define COMMLINK_CLASS MavlinkComm
// hardware selection
#define ADC_CLASS AP_ADC_ADS7844
#define COMPASS_CLASS AP_Compass_HMC5843
#define BARO_CLASS APM_BMP085_Class
#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL
// baud rates
static const uint32_t debugBaud = 57600;
static const uint32_t telemBaud = 57600;
static const uint32_t gpsBaud = 38400;
static const uint32_t hilBaud = 115200;
// optional sensors
static const bool gpsEnabled = false;
static const bool baroEnabled = false;
static const bool compassEnabled = true;
static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD;
// compass orientation: See AP_Compass_HMC5843.h for possible values
// battery monitoring
static const bool batteryMonitorEnabled = true;
static const uint8_t batteryPin = 0;
static const float batteryVoltageDivRatio = 6;
static const float batteryMinVolt = 10.0;
static const float batteryMaxVolt = 12.4;
static const bool rangeFinderFrontEnabled = false;
static const bool rangeFinderBackEnabled = false;
static const bool rangeFinderLeftEnabled = false;
static const bool rangeFinderRightEnabled = false;
static const bool rangeFinderUpEnabled = false;
static const bool rangeFinderDownEnabled = false;
// loop rates
static const float loopRate = 150; // attitude nav
static const float loop0Rate = 50; // controller
static const float loop1Rate = 5; // pos nav/ gcs fast
static const float loop2Rate = 1; // gcs slow
static const float loop3Rate = 0.1;
// gains
static const float steeringP = 0.1;
static const float steeringI = 0.0;
static const float steeringD = 0.1;
static const float steeringIMax = 0.0;
static const float steeringYMax = 1;
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 throttleIMax = 0.0;
static const float throttleYMax = 1;
static const float throttleDFCut = 0.0;
// guidance
static const float velCmd = 5;
static const float xt = 10;
static const float xtLim = 90;
#include "ControllerSailboat.h"
#endif /* SAILBOATLASER_H_ */
// vim:ts=4:sw=4:expandtab
Write Preview
Loading…
Cancel
Save