From 1b0328e8893144ac90046388e2855a2a3076a43c Mon Sep 17 00:00:00 2001 From: Wenyao Xie Date: Tue, 1 Nov 2011 13:59:34 -0400 Subject: [PATCH 1/7] Updated date on file. --- ArduBoat/ControllerBoat.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h index 97ae1ec10e..2b222a7ee0 100644 --- a/ArduBoat/ControllerBoat.h +++ b/ArduBoat/ControllerBoat.h @@ -1,7 +1,7 @@ /* * ControllerBoat.h * - * Created on: Jun 30, 2011 + * Created on: Nov 1, 2011 * Author: jgoppert */ From 04d4aa431016f1ed124c108a0134686648a98601 Mon Sep 17 00:00:00 2001 From: Simon Salykov Date: Tue, 1 Nov 2011 19:09:57 +0100 Subject: [PATCH 2/7] Obstacle avoidance moved from AP_Guide to ControllerCar (works only for cars for now), algorithm changed New feature Forward/Reverse in Controller Car Misc bug fixes in ControllerCar --- ArduRover/CarStampede.h | 2 ++ ArduRover/ControllerCar.h | 57 ++++++++++++++++++++++++-------- libraries/APO/APO_DefaultSetup.h | 2 +- libraries/APO/AP_Guide.cpp | 57 +------------------------------- libraries/APO/AP_Guide.h | 4 --- 5 files changed, 48 insertions(+), 74 deletions(-) diff --git a/ArduRover/CarStampede.h b/ArduRover/CarStampede.h index 57de768a4b..82dc03b9cd 100644 --- a/ArduRover/CarStampede.h +++ b/ArduRover/CarStampede.h @@ -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; diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h index c1951cf786..75a2b76a73 100644 --- a/ArduRover/ControllerCar.h +++ b/ArduRover/ControllerCar.h @@ -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 diff --git a/libraries/APO/APO_DefaultSetup.h b/libraries/APO/APO_DefaultSetup.h index 82ee0250b8..b6e842f746 100644 --- a/libraries/APO/APO_DefaultSetup.h +++ b/libraries/APO/APO_DefaultSetup.h @@ -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); diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index 3437415967..49eaf263a8 100644 --- a/libraries/APO/AP_Guide.cpp +++ b/libraries/APO/AP_Guide.cpp @@ -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() { diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index f01258a42d..033e27ed45 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -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; From acc03753abfc7b7370ff99dcfdf70d6238600e2a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Nov 2011 08:13:57 +1100 Subject: [PATCH 3/7] fixed HIL build --- ArduCopter/ArduCopter.pde | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 113dbe5a4a..36c5fbbd4a 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -732,7 +732,9 @@ static void medium_loop() // Do an extra baro read // --------------------- +#if HIL_MODE != HIL_MODE_ATTITUDE barometer.Read(); +#endif slow_loop(); break; From 19d5e983ba5b80c40780ee84fc2308d6cd6d4229 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Nov 2011 09:14:10 +1100 Subject: [PATCH 4/7] autotest: fixed path to SIL binary --- Tools/autotest/util.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/util.py b/Tools/autotest/util.py index df6fd981be..f08421044f 100644 --- a/Tools/autotest/util.py +++ b/Tools/autotest/util.py @@ -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 From e7671178b3ac2f09d9d48b2eb3f797d11bd13513 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Nov 2011 09:20:46 +1100 Subject: [PATCH 5/7] desktop: fixed TCP buffering issue with HIL the TCP layer was buffering the servo updates, which caused very poor HIL flight --- libraries/Desktop/support/FastSerial.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/Desktop/support/FastSerial.cpp b/libraries/Desktop/support/FastSerial.cpp index 04623fa8d0..79dbc1a78a 100644 --- a/libraries/Desktop/support/FastSerial.cpp +++ b/libraries/Desktop/support/FastSerial.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #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); } } From b6dcfa416b8c3865d4fb2add538cf8096dfc513d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Nov 2011 09:21:21 +1100 Subject: [PATCH 6/7] autotest: a complete mission now flies with the TCP buffering fixed, the mission flies quite well with standard parameters --- Tools/autotest/ArduCopter.parm | 9 --------- Tools/autotest/arducopter.py | 5 +++-- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/Tools/autotest/ArduCopter.parm b/Tools/autotest/ArduCopter.parm index 3465fc56d2..5e9a2a45be 100644 --- a/Tools/autotest/ArduCopter.parm +++ b/Tools/autotest/ArduCopter.parm @@ -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 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index a98c8bf337..593ad682fe 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -229,7 +229,7 @@ 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) mavproxy.send('wp load %s\n' % filename) @@ -239,7 +239,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, startloc, timeout=600) def setup_rc(mavproxy): @@ -306,6 +306,7 @@ def fly_ArduCopter(): 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 From 40c87dd295b4f5f538a204fde893104923081409 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Nov 2011 10:26:24 +1100 Subject: [PATCH 7/7] fixed home location --- Tools/autotest/arducopter.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 593ad682fe..2e0807a088 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -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 = [] @@ -231,7 +233,7 @@ def land(mavproxy, mav, timeout=60): 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): mavproxy.send('switch 1\n') # auto mode mavproxy.expect('AUTO>') wait_distance(mav, 30, timeout=120) - wait_location(mav, startloc, timeout=600) + 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,8 +300,9 @@ 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)