State machine (#13)

* simulator versions for gps and rover

* Initial state machine changes for rover sim

* Update to state machine handling

* merged sim version and real version

* external logger using Serial2.

* Update logging, ping sensor protocol, ping sensor sim, and adjusted scalar for nav error calculations.

* Updated navigation logic and state machines.  Disabled gyro for now

* Minor tweaks.  Point target and approach radius are increased
bug fix for avoid state

* Disabled serial logging output by default. (Used in testing)

* Minor filtering of ping sensors
Expose more variables to settings

* General cleanup: comments, placement (location), extlog changes
Added version control
Fixed compile warning in gps sim file
Version: 0.1.0

* Potential fix for the radio (manual) control.

* Updated version

* Changed how radio state goes into previous state
Added GPS state (1 = bad / 0 = good )

* updated default values for settings
updated version

* Update GroundSettings info, remove sim build

-The GroundSettings comments are parsed by the Dashboards settingsDescriptionParse.py script during a release build. It uses these values to build the ground settings file. The min and max values have been adjusted so that information is parsed as expected.

-Removed the old, no longer used sim build which was causing duplicate ground settings entries. The parsing script looks at every file in the project folder.

* Added mpu6000 hardware dmp driver

* Version 0.1.4

Lots of changes to RoboMagellan6x6: mostly debug, gyro hw.  Some commented out option for compass.
Debug messages in commmanager
Gyro hw driver added

* Fixed arduino reset->gps module init issue
Fixed issue with baud switch needing more delay before additional commands
Restructured gps->nmea protocol to parse more then just single message type.
Parsing both rmc and gns messages now.
Added number of gps sats and HDOP to telemetry protocol
Sending number of gps sats and HDOP in telemtry data now

* Added bumper detection
-example test
-new telemetry message
-added delay for clearing avoid flag (not specificaly related to bumper changes)
-added mapping for determining avoid backup direction with sensor and bumper inputs

* First pass of fixing wiggle issue.
Still need to work on math for amount of degrees in the "perfered" left/right direction

* Modified detection logic to trigger again if button still pressed (not event driven)
Tested with hardware but not on rover.

* Turn around output angle calculated simply as output angle from "normal" path following calcuation.
This is simplier then an true angle error but should be close enough.  Also only off for angles close to 180.
Once rover starts to turn around then output angle should be correct.
The rover can only turn up to maximum steerthrow.

* Fixed issues with flag and varables

* Initial pass at adding GPS_COORD functionality

* first pass at commmanager and waypoint updates to use gps_coord

* updated backup to use states as well as timers

* Added substates to the avoid state
Changed some timing.
write directly to outputs (speed/directions) and use break/stop/backup+ state transistions
Changed logic for backup.  Bumper now primary decider of backup direction.  Ultrasonics only decide if no bumper sensor active or both active.
Fixed bug with backup direction always being the same (unsigned var)

* Forgot to add changes that fixed left/right swap on backward turn direciton for ping sensors

* Fixed up a few more compile issues.  Added back ability to get lat/log even though at reduced precision.

* Increased times for backup on avoid.  This will change the values on the dashboard setttings file.

* Fixed uint/int issue with var for backup direction
Fixed bounds check issue (or vs and)
Fixed value bounds on angle error var

* Add bumper Enable/Disable callbacks and functionality

* Removed UI Compass (angle widget) offset

* Bug fix (A-03) - line gravity backWaypoint now resets when entering Autonomy

Line gravity was getting invalid backWaypoint on all subsequent missions.
backWaypoint is now reset every time the state is swiched to autonomy.
Also, this is a test of commiting to the repo for RDG.

* Added GPS checksum

GPS messages data is now not shared until it's checksum has been
verified.  Bad checksum will result in discarded GPS msg data.

* Fixed bug where GNS msg was not being correctly processed.

GNS msg processor had an extra field which was preventing the msg
from passing the checksum correctly.

* Added some changes for sim testing.
Cleaned up some older comments
Improved Waypoint contructors, fixed some minor issues

* Clean up debug message and old commented code

* Add basic logging (#3)

* Added some base logging messages.

* Updated to allow logged data to use high precision GPS.

* Added directives to allow code to operate without debugger library

* Bug fix - debugger code can now bein disabled w/ directive

* Set bumper default to disabled

* Modified AUTO steering angle calculation, CAUTION clearing and values

Steering calculation in auto mode will only use ping values to
modify steering if the CAUTION flag is set.

CAUTION flag will only be unset if all acustic sensors are no
longer in the CAUTION range.

CAUTION range values were extended to give the robot a little more
distance to react to obstacles.

* **Warning update requires jumper to be removed from esc's**
Changed backup states to fit new esc settings (no esc states)
Backup (avoid) to use output() function (pid) for slowing down (braking)

* Kalman heading (#4)

* Added Kalman filtered heading using GPS course to update heading.

* Removed "heading hold" when throttle is zero for 5+ seconds.

This looks like it might have negative effects as it prevents
heading from changing if the vehilce has been sitting for a few
seconds and then someone manually moves the vehicle.

* Added telemetry msg - flag for initial heading lock

* Change Version of APM

* Fix max value groundSettings syntax for Steer Scalar max value

Maximum value was not being reported on file generation due to incorrect formatting for the max value.

* Reduce range of Cruise P Ground Setting

* Changed max forward speed to 5mph when in manual mode

* Bump Rev version: 0.1.11

* Reset mission after complete (#5)

* Reset target to first mission waypoint after mission completion.

* Set default steering bias to zero instead of RDGs system.

* Fixed comments for settings index to match code. Dashboard parses comments to create settings file. (2) (#9)

* Update version.h

Version bump for RoboMagellan6x6

Co-authored-by: Chris Park <[email protected]>
Co-authored-by: Richard Garcia <[email protected]>
Co-authored-by: Richard Garcia <[email protected]>

examples/RoboMagellan6x6/version.h

@@ -0,0 +1,8 @@
+#ifndef ROBOMAG6X6_VERSION_H
+#define ROBOMAG6X6_VERSION_H
+
+uint8_t	 version_major = 0;
+uint8_t	 version_minor = 1;
+uint8_t	 version_rev = 12;
+
+#endif

examples/bumperTest/bumperTest.ino

@@ -0,0 +1,41 @@
+#include "MINDS-i-Drone.h"
+
+
+bumper bumperSensor;
+
+
+void setup()
+{
+
+	Serial.begin(115200);
+	delay(500);
+
+	bumperSensor.begin(A5,A6);
+	Serial.println("Starting Bumper sensor test");	
+}
+
+void loop()
+{
+
+	bumperSensor.update();
+
+
+	if (bumperSensor.leftButtonEvent())
+	{
+		if (bumperSensor.leftButtonState())
+			Serial.println("Left Button Pressed");
+		else
+			Serial.println("Left Button Released");		
+	}
+
+	if (bumperSensor.rightButtonEvent())
+	{
+		if (bumperSensor.rightButtonState())
+			Serial.println("Right Button Pressed");
+		else
+			Serial.println("Right Button Released");		
+	}
+
+	delay(25);
+
+}

examples/streamGPS_new/streamGPS_new.ino

@@ -0,0 +1,53 @@
+#include "MINDS-i-Drone.h"
+
+LEA6H gps;
+
+void setup()
+{
+    Serial.begin(115200);
+    gps.begin();
+}
+
+void loop() 
+{
+    gps.update();
+    static size_t lastGpsDataIndex = gps.dataIndex();
+    if(gps.dataIndex() > lastGpsDataIndex)
+    {
+        lastGpsDataIndex = gps.dataIndex();
+        Serial.print("gps.getWarning()     ");
+        Serial.println(gps.getWarning()    );
+        Serial.print("gps.getCourse()      ");
+        Serial.println(gps.getCourse()     );
+        Serial.print("gps.getDateOfFix()   ");
+        Serial.println(gps.getDateOfFix()  );
+        Serial.print("gps.getGroundSpeed() ");
+        Serial.println(gps.getGroundSpeed());
+        Serial.print("gps.getLatitude()    ");
+        Serial.println(gps.getLatitude(),9   );
+        Serial.print("gps.getLongitude()   ");
+        Serial.println(gps.getLongitude(),9  );
+        Serial.print("gps.getMagVar()      ");
+        Serial.println(gps.getMagVar()     );
+        Serial.print("gps.getTimeOfFix()   ");
+        Serial.println(gps.getTimeOfFix()  );
+        GPS_COORD tempGPSCoord = gps.getGPS_COORD();
+        char str[32]; 
+        print_gps(&Serial, "GPS_COORD: ",&tempGPSCoord);
+        gps_coord_to_str(&tempGPSCoord,str,32,9,"DD");
+        Serial.print("GPS: ");
+        Serial.println(str);
+
+
+        float_to_gps_angle(gps.getLatitude(), &tempGPSCoord.latitude);
+        float_to_gps_angle(gps.getLongitude(), &tempGPSCoord.longitude);
+        gps_coord_to_str(&tempGPSCoord,str,32,9,"DD");
+        Serial.print("GPS (from float): ");
+        Serial.println(str);
+
+
+
+        Serial.println("-----");
+        delay(500);
+    }
+}

library.properties

@@ -1,9 +1,9 @@
-name=MINDS-i-Drone
-version=1.3.0
-author=MINDS-i corp.
-maintainer=MINDS-i <[email protected]>
-sentence=Code to assist with running MINDS-i Drones
-paragraph=This library contains code for running MINDS-i Drone hardware
-category=Device Control
-url=http://mindsieducation.com/programming-resources
-architectures=avr
+name=MINDS-i-Drone
+version=1.4.0
+author=MINDS-i corp.
+maintainer=MINDS-i <[email protected]>
+sentence=Code to assist with running MINDS-i Drones
+paragraph=This library contains code for running MINDS-i Drone hardware
+category=Device Control
+url=http://mindsieducation.com/programming-resources
+architectures=avr

src/MINDS-i-Drone.h

@@ -59,21 +59,32 @@ Copyright 2015 MINDS-i Inc.
 #include "input/altIMU/STMtwi.h"
 #include "input/APM/HMC5883L.h"
 #include "input/APM/LEA6H.h"
+
+#include "input/APM/LEA6H_sim.h"
+
 #include "input/APM/MPU6000.h"
+#include "input/APM/MPU6000_HW.h"
 #include "input/APM/MS5611.h"
 #include "input/APM/Power.h"
 #include "input/AxisTranslator.h"
 #include "input/InertialManager.h"
 //#include "input/Sensor.h"
 #include "input/SPIcontroller.h"
 #include "input/UM7.h"
+#include "input/Bumper.h"
+
 
 #include "math/Algebra.h"
 #include "math/Quaternion.h"
 #include "math/SpatialMath.h"
 #include "math/Vec3.h"
 #include "math/Waypoint.h"
 
+#include "math/gps_angle.h"
+#include "math/floatgps.h"
+#include "math/gps_print.h"
+#include "math/ftoa.h"
+
 #include "output/AfroESC.h"
 #include "output/EMaxESC.h"
 #include "output/FlightStrategy.h"

src/MINDS-i-Drone_gps.h

@@ -0,0 +1,6 @@
+#ifndef MINDSIDRONELIBS_GPS_H
+#define MINDSIDRONELIBS_GPS_H
+
+#include "comms/NMEA.h"
+
+#endif

src/MINDS-i-Drone_mpu6000.h

@@ -0,0 +1,6 @@
+#ifndef MINDSIDRONELIBS_MPU6000_H
+#define MINDSIDRONELIBS_MPU6000_H
+
+#include "input/APM/MPU6000_HW.h"
+
+#endif