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

ArduBoat/ControllerBoat.h

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

ArduCopter/ArduCopter.pde

@@ -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;

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;

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

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

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 = []
@@ -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

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

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);

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() {

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;

libraries/Desktop/support/FastSerial.cpp

@@ -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);
 		}
 	}