aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

Browse Source
This commit is contained in:
Jason Short 2011-11-01 21:18:56 -07:00
commit 3f211121fe
11 changed files with 65 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduBoat/ControllerBoat.h
View File

@ -1,7 +1,7 @@
/*
* ControllerBoat.h
*
* Created on: Jun 30, 2011
* Created on: Nov 1, 2011
* Author: jgoppert
*/

2
ArduCopter/ArduCopter.pde
View File

@ -724,7 +724,9 @@ static void medium_loop()
// Do an extra baro read
// ---------------------
#if HIL_MODE != HIL_MODE_ATTITUDE
barometer.Read();
#endif
slow_loop();
break;

2
ArduRover/CarStampede.h
View File

@ -48,6 +48,8 @@ static const float batteryVoltageDivRatio = 6;
static const float batteryMinVolt = 10.0;
static const float batteryMaxVolt = 12.4;
static const bool useForwardReverseSwitch = false;
static const bool rangeFinderFrontEnabled = false;
static const bool rangeFinderBackEnabled = false;
static const bool rangeFinderLeftEnabled = false;

57
ArduRover/ControllerCar.h
View File

@ -21,7 +21,8 @@ public:
steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
throttleDFCut), _strCmd(0), _thrustCmd(0)
throttleDFCut), _strCmd(0), _thrustCmd(0),
_rangeFinderFront()
{
_hal->debug->println_P(PSTR("initializing car controller"));
@ -34,38 +35,66 @@ public:
_hal->rc.push_back(
new AP_RcChannel(k_chThrust, PSTR("THR_"), APM_RC, 2, 1100, 1500,
1900, RC_MODE_INOUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chFwdRev, PSTR("FWDREV_"), APM_RC, 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) {
setAllRadioChannelsManually();
_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());
_strCmd = pidStr.update(_guide->getHeadingError(), _nav->getYawRate(), dt);
_thrustCmd = pidThrust.update(
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 setMotorsActive() {
// 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_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd);
}
// attributes
enum {
ch_mode = 0, ch_str, ch_thrust
ch_mode = 0, ch_str, ch_thrust, ch_FwdRev
};
enum {
k_chMode = k_radioChannelsStart, k_chStr, k_chThrust
k_chMode = k_radioChannelsStart, k_chStr, k_chThrust, k_chFwdRev
};
enum {
k_pidStr = k_controllersStart, k_pidThrust
@ -73,6 +102,8 @@ private:
BlockPIDDfb pidStr;
BlockPID pidThrust;
float _strCmd, _thrustCmd;
RangeFinder * _rangeFinderFront;
};
} // namespace apo

9
Tools/autotest/ArduCopter.parm
View File

@ -29,12 +29,3 @@ FLTMODE3 2
FLTMODE4 6
FLTMODE5 5
FLTMODE6 0
NAV_LAT_I 0
NAV_LON_I 0
NAV_LAT_P 1
NAV_LON_P 1
STB_PIT_P 2
STB_RLL_P 2
XTRACK_P 1
RATE_PIT_P 0.1
RATE_RLL_P 0.1

14
Tools/autotest/arducopter.py
View File

@ -10,6 +10,8 @@ import mavutil
HOME_LOCATION='-35.362938,149.165085,650,270'
homeloc = None
# a list of pexpect objects to read while waiting for
# messages. This keeps the output to stdout flowing
expect_list = []
@ -229,9 +231,9 @@ def land(mavproxy, mav, timeout=60):
return False
def fly_mission(mavproxy, mav, filename, timeout=120):
def fly_mission(mavproxy, mav, filename):
'''fly a mission from a file'''
startloc = current_location(mav)
global homeloc
mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('flight plan received')
mavproxy.send('wp list\n')
@ -239,7 +241,7 @@ def fly_mission(mavproxy, mav, filename, timeout=120):
mavproxy.send('switch 1\n') # auto mode
mavproxy.expect('AUTO>')
wait_distance(mav, 30, timeout=120)
wait_location(mav, startloc, timeout=300)
wait_location(mav, homeloc, timeout=600)
def setup_rc(mavproxy):
@ -252,7 +254,7 @@ def setup_rc(mavproxy):
def fly_ArduCopter():
'''fly ArduCopter in SIL'''
global expect_list
global expect_list, homeloc
util.rmfile('eeprom.bin')
sil = util.start_SIL('ArduCopter')
@ -298,14 +300,16 @@ def fly_ArduCopter():
failed = False
try:
mav.wait_heartbeat()
mav.recv_match(type='GPS_RAW')
mav.recv_match(type='GPS_RAW', blocking=True)
setup_rc(mavproxy)
homeloc = current_location(mav)
arm_motors(mavproxy)
takeoff(mavproxy, mav)
fly_square(mavproxy, mav)
loiter(mavproxy, mav)
land(mavproxy, mav)
fly_mission(mavproxy, mav, os.path.join(testdir, "mission1.txt"))
land(mavproxy, mav)
disarm_motors(mavproxy)
except pexpect.TIMEOUT, e:
failed = True

2
Tools/autotest/util.py
View File

@ -50,7 +50,7 @@ def build_SIL(atype):
def start_SIL(atype):
'''launch a SIL instance'''
ret = pexpect.spawn('tmp/%s.build/%s.elf' % (atype, atype),
ret = pexpect.spawn(reltopdir('tmp/%s.build/%s.elf' % (atype, atype)),
logfile=sys.stdout, timeout=5)
ret.expect('Waiting for connection')
return ret

2
libraries/APO/APO_DefaultSetup.h
View File

@ -86,7 +86,7 @@ void setup() {
if (rangeFinderFrontEnabled) {
hal->debug->println_P(PSTR("initializing front range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(NULL,new ModeFilter);
rangeFinder->set_analog_port(1);
rangeFinder->set_orientation(1, 0, 0);
hal->rangeFinders.push_back(rangeFinder);

57
libraries/APO/AP_Guide.cpp
View File

@ -44,71 +44,16 @@ float AP_Guide::getHeadingError() {
MavlinkGuide::MavlinkGuide(AP_Navigator * navigator,
AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) :
AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(),
_rangeFinderLeft(), _rangeFinderRight(),
AP_Guide(navigator, hal),
_group(k_guide, PSTR("guide_")),
_velocityCommand(&_group, 1, velCmd, PSTR("velCmd")),
_crossTrackGain(&_group, 2, xt, PSTR("xt")),
_crossTrackLim(&_group, 3, xtLim, PSTR("xtLim")) {
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;
else if (rF->orientation_x == -1 && rF->orientation_y == 0
&& rF->orientation_z == 0)
_rangeFinderBack = rF;
else if (rF->orientation_x == 0 && rF->orientation_y == 1
&& rF->orientation_z == 0)
_rangeFinderRight = rF;
else if (rF->orientation_x == 0 && rF->orientation_y == -1
&& rF->orientation_z == 0)
_rangeFinderLeft = rF;
}
}
void MavlinkGuide::update() {
// process mavlink commands
handleCommand();
// obstacle avoidance overrides
// stop if your going to drive into something in front of you
for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++)
_hal->rangeFinders[i]->read();
float frontDistance = _rangeFinderFront->distance / 200.0; //convert for other adc
if (_rangeFinderFront && frontDistance < 2) {
_mode = MAV_NAV_VECTOR;
//airSpeedCommand = 0;
//groundSpeedCommand = 0;
// _headingCommand -= 45 * deg2Rad;
// _hal->debug->print("Obstacle Distance (m): ");
// _hal->debug->println(frontDistance);
// _hal->debug->print("Obstacle avoidance Heading Command: ");
// _hal->debug->println(headingCommand);
// _hal->debug->printf_P(
// PSTR("Front Distance, %f\n"),
// frontDistance);
}
if (_rangeFinderBack && _rangeFinderBack->distance < 5) {
_airSpeedCommand = 0;
_groundSpeedCommand = 0;
}
if (_rangeFinderLeft && _rangeFinderLeft->distance < 5) {
_airSpeedCommand = 0;
_groundSpeedCommand = 0;
}
if (_rangeFinderRight && _rangeFinderRight->distance < 5) {
_airSpeedCommand = 0;
_groundSpeedCommand = 0;
}
}
void MavlinkGuide::nextCommand() {

4
libraries/APO/AP_Guide.h
View File

@ -136,10 +136,6 @@ public:
void updateCommand();
private:
RangeFinder * _rangeFinderFront;
RangeFinder * _rangeFinderBack;
RangeFinder * _rangeFinderLeft;
RangeFinder * _rangeFinderRight;
AP_Var_group _group;
AP_Float _velocityCommand;
AP_Float _crossTrackGain;

4
libraries/Desktop/support/FastSerial.cpp
View File

@ -45,6 +45,7 @@
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include "desktop.h"
#define LISTEN_BASE_PORT 5760
@ -131,6 +132,7 @@ static void tcp_start_connection(unsigned int serial_port, bool wait_for_connect
fprintf(stderr, "accept() error - %s", strerror(errno));
exit(1);
}
setsockopt(s->fd, SOL_TCP, TCP_NODELAY, &one, sizeof(one));
s->connected = true;
}
}
@ -170,7 +172,9 @@ static void check_connection(struct tcp_state *s)
if (select_check(s->listen_fd)) {
s->fd = accept(s->listen_fd, NULL, NULL);
if (s->fd != -1) {
int one = 1;
s->connected = true;
setsockopt(s->fd, SOL_TCP, TCP_NODELAY, &one, sizeof(one));
printf("New connection on serial port %u\n", s->serial_port);
}
}