diff --git a/examples/RoboMagellan6x6/RoboMagellan6x6.ino b/examples/RoboMagellan6x6/RoboMagellan6x6.ino index ed1664f..0975522 100644 --- a/examples/RoboMagellan6x6/RoboMagellan6x6.ino +++ b/examples/RoboMagellan6x6/RoboMagellan6x6.ino @@ -4,31 +4,106 @@ #include "Encoder.h" #include "MINDS-i-Drone.h" #include "util/callbackTemplate.h" +#include "version.h" -//Constants that should never change during driving and never/rarely tuned +//#define M_DEBUG //comment out to disable debugger + +#ifdef M_DEBUG + #include "MINDSiDebugger.h" + MINDSiDebugger debugger; +#endif + +#define MIN_GOOD_HEADINGS 5 +#define HEADING_LOCK_RANGE 10 +bool kalman_heading = true; +float k_heading = 0; +bool heading_lock = false; +bool new_gps = false; + +int8_t steer_bias = 0; + +bool driving_straight = false; +//=============================================// +// Defines used to control compile options +//=============================================// + +// == Sim mode == +//#define simMode true + +//== encoder == #define useEncoder true + +//== external logger == +#define extLogger false + +//==== debug vars ====/ +bool extLogger_gps = false; +bool extLogger_gen = true; + +//==== Peripheral HW vars ====/ +bool isBumperEnabled = false; + +//=============================================// +//Constants that should never change during +// driving and never/rarely tuned +//=============================================// + +//== pin defines == const uint8_t VoltagePin = 67; const uint8_t LEDpin[] = {25, 26, 27}; //blue, yellow, red const uint8_t PingPin[] = {A0, A1, A2, A3, A4}; //left to right const uint8_t ServoPin[] = {12, 11, 8};//drive, steer, backS; APM 1,2,3 resp. const uint8_t RadioPin[] = {7, 0, 1}; //auto switch, drive, steer const uint8_t EncoderPin[]= {2/*APM pin 7*/, 3 /*APM pin 6*/}; -const double pAngle[5] = { 79.27, 36.83, 0.0, -36.83, -79.27}; -const int ScheduleDelay = 22; -const uint16_t warn[] = {1000, 1600, 3000, 1600, 1000}; -const double PointRadius = .001; //in miles, margin for error in rover location - //tire circ in miles per inch diameter * diff ratio + +//== ping sensor vars == + +const float pAngle[5] = { 79.27, 36.83, 0.0, -36.83, -79.27}; +//about 500 us per inch +int blockLevel[] = {1000, 1600, 3000, 1600, 1000}; +int warnLevel[] = {2000, 3200, 6000, 3200, 2000}; + +//== Tire related vars/defines == + +//tire circ in miles per inch diameter * diff ratio const float MilesPerRev = (((PI)/12.f)/5280.f) * (13.f/37.f); - //hours per min rev per mile +//hours per min rev per mile const float MPHvRPM = (1.f/60.f) * (1.f/MilesPerRev); -//Global variables used throught the program +#define MAN_MAX_FWD (5) + +//== steering related == + +//All Headings are Clockwise+, -179 to 180, 0=north +double pathHeading; +double trueHeading; + +//test for correcting mechanically caused drift left-right +float steerSkew = 0; + +int8_t turnAroundDir=0; + +//== hardware related == + HardwareSerial *commSerial = &Serial; Storage *storage = eeStorage::getInstance(); +#ifdef simMode +LEA6H_sim gps; +#else +LEA6H gps; +#endif + +//compass +//HMC5883L hmc; + +//MPU6000 mpu; +MPU6000_DMP mpudmp; + + +bumper bumperSensor; + CommManager manager(commSerial, storage); Settings settings(storage); -LEA6H gps; -MPU6000 mpu; Waypoint location(0,0); Waypoint backWaypoint(0,0); HLA lowFilter (600000, 0);//10 minutes @@ -38,33 +113,188 @@ HLA roll ( 100, 0); PIDparameters cruisePID(0,0,0,-90,90); PIDcontroller cruise(&cruisePID); ServoGenerator::Servo servo[3]; //drive, steer, backSteer - //scheduler, navigation, obstacle, stop times -uint32_t uTime = 0, nTime = 0, oTime = 0, sTime = 0; + +//== scheduler, navigation, obstacle, stop times == + +uint32_t uTime = 0, nTime = 0, sTime = 0, pTime; +#ifdef simMode +uint32_t simTime = 0; +#endif uint32_t gpsHalfTime = 0, gpsTime = 0; - int32_t Ax,Ay,Az; //used in accelerometer calculations -uint16_t ping[5] = {20000,20000,20000,20000,20000}; +//used in accelerometer calculations +int32_t Ax,Ay,Az; + +//== Ping == + +enum PING_HIST { + PING_CUR = 0, + PING_LAST, +}; + +uint16_t ping[5][2] = {20000,20000,20000,20000,20000}; uint8_t sIter,pIter; //iterators for scheduler and ping -double pathHeading; //All Headings are Clockwise+, -179 to 180, 0=north -double trueHeading; + +//== mpu6000 ==// +float euler_z_offset=0; +float last_euler_z=0; +float cur_euler_z=0; + +//== gyro == + double gyroHalf; //Store Gyro Heading halfway between gps points double distance; boolean stop = true; -boolean backDir; +int8_t backDir; + +//todo testing +float lastOutputAngle=0; +float lastAngularError=0; + + +//==================================== +// State related variables/enums +//==================================== + +enum APM_STATES { + APM_STATE_INVALID = 0, + APM_STATE_INIT, + APM_STATE_SELF_TEST, + APM_STATE_DRIVE +}; + +enum DRIVE_STATES { + DRIVE_STATE_INVALID = 0, + DRIVE_STATE_STOP, + DRIVE_STATE_AUTO, + DRIVE_STATE_RADIO +}; + +enum AUTO_STATES { + AUTO_STATE_INVALID = 0, + AUTO_STATE_FULL, + AUTO_STATE_AVOID, + AUTO_STATE_STALLED, +}; + +enum AVOID_STATES { + AVOID_STATE_ENTER = 0, + AVOID_STATE_FWD_BRAKE, + AVOID_STATE_STRAIGHTBACK, + AVOID_STATE_STEER, + AVOID_STATE_REV_BRAKE, + AVOID_STATE_DONE, + AVOID_STATE_NUM_STATES +}; + +const char AVOID_STATES_STRING[AVOID_STATE_NUM_STATES][32] = +{ + "Avoid State Enter", + "Avoid State Forward Brake", + "Avoid State Straightback", + "Avoid State Steer", + "Avoid State Reverse Brake", + "Avoid State Done" +}; + +//warning assign as flags. Unique bits 1,2,4,8,etc +enum AUTO_STATE_FLAGS { + AUTO_STATE_FLAGS_NONE =0x00, + AUTO_STATE_FLAG_CAUTION =0x01, + AUTO_STATE_FLAG_APPROACH =0x02, + AUTO_STATE_FLAG_TURNAROUND =0x04, +}; + + +//global state vars (maybe move to class at some point?) +uint8_t apmState=APM_STATE_INVALID; +uint8_t driveState=DRIVE_STATE_INVALID; +uint8_t prevDriveState=DRIVE_STATE_INVALID; +uint8_t autoState=AUTO_STATE_INVALID; +uint8_t autoStateFlags=AUTO_STATE_FLAGS_NONE; +uint8_t avoidState=AVOID_STATE_DONE; + + + +//================================== +// Function prototypes +//================================== void checkPing(); +void checkBumperSensor(); void readAccelerometer(); void reportLocation(); +void reportState(); void extrapPosition(); -void (*schedule[])(void) = { extrapPosition, - checkPing, - readAccelerometer, - reportLocation, - }; - -//These parameters are loaded from eeprom if the code has not been reuploaded -//since when they were last set. -//the dashboard can update records in the storage instance passed into manager -//The callbacks keep them up to date while the code is running +void navigate(); +void compass_sync(); +#ifdef simMode +void updateSIM(); +#endif + +void stateStop(); +void stateStart(); +void bumperDisable(); +void bumperEnable(); +void version(); + +void setupSettings(); + +//=================================== +// Scheduler structures +//=================================== + +struct schedulerData +{ + void (*function)(void); + uint16_t delay; + uint32_t lastTime; + boolean enabled; +}; + +//delay,last scheduled time,enabled +struct schedulerData scheduler[] = +{ + {extrapPosition, 110 ,0 ,1}, + {readAccelerometer, 110 ,22 ,1}, + {reportLocation, 110 ,44 ,1}, + {reportState, 110 ,110 ,1}, + {checkPing, 22 ,88 ,1}, + {checkBumperSensor, 80 ,55 ,1}, + //{navigate, 0 ,66 ,1}, + {compass_sync, 500 ,1000 ,0}, + #ifdef simMode + {updateSIM, 500 ,100 ,1} + #endif +}; + +enum SCHEDULED_FUNCTIONS +{ + SCHD_FUNC_EXTRPPOS=0, + SCHD_FUNC_RDACC, + SCHD_FUNC_RPRTLOC, + SCHD_FUNC_RPRTSTATE, + SCHD_FUNC_CHKPING, + SCHD_FUNC_BUMPSENSOR, + //SCHD_FUNC_NAVIGATE, + SCHD_FUNC_COMPASS_SYNC, + #ifdef simMode + SCHD_FUNC_UPDATESIM, + #endif +}; + + + +//============================================== +// Vars stored in Settings +// +// These parameters are loaded from eeprom if +// the code has not been reuploaded since +// when they were last set. the dashboard +// can update records in the storage instance +// passed into manager. The callbacks keep +// them up to date while the code is running +//============================================== + float lineGravity; int steerThrow, steerStyle; float steerFactor; @@ -72,253 +302,1570 @@ float minFwd, maxFwd; int revThrow; float revSpeed; float pingWeight; -int coastTime, dangerTime; //danger should include coastTime +int avoidCoastTime, avoidStraightBack, avoidSteerBack; +int cautionTime=1000; float tireDiameter; int steerCenter; -void dangerTimeCallback(float in){ dangerTime = coastTime+in; } +//in miles, margin for error in rover location +float PointRadius = .0015; +float approachRadius = .0038; + + +float compassOffset = M_PI; + + +//=========================== +// Settings callbacks +//=========================== + +void dangerTimeCallback(float in){ + avoidStraightBack = in; + avoidSteerBack = in; +} + +void pingBlockLevelEdgesCallback(float in){ + blockLevel[0] = blockLevel[4] = (int)in; +} +void pingBlockLevelMiddlesCallback(float in){ + blockLevel[1] = blockLevel[3] = (int)in; +} +void pingBlockLevelCenterCallback(float in){ + blockLevel[2] = (int)in; +} +void pingWarnLevelEdgesCallback(float in){ + warnLevel[0] = warnLevel[4] = (int)in; +} +void pingWarnLevelMiddlesCallback(float in){ + warnLevel[1] = warnLevel[3] = (int)in; +} +void pingWarnLevelCenterCallback(float in){ + warnLevel[2] = (int)in; +} +//testing +void triggerGyroSyncNorth(float in){ + euler_z_offset = mpudmp.getEulerZ(); +} + + +//=========================== +// Utility functions/inlines +//=========================== + inline float MPHtoRPM(float mph){ return (mph*MPHvRPM)/tireDiameter; } inline float RPMtoMPH(float rpm){ return (rpm*tireDiameter)/MPHvRPM; } -void setupSettings(); -void setup() { - setupSettings(); +//============================ +// external Logging functions +//============================ + + +#ifdef extLogger +unsigned int extlogSeqNum=0; + +#define LOG_TYPE_ERROR 0x01 +#define LOG_TYPE_INFO 0x02 +#define LOG_TYPE_VERBOSE 0x04 +#define LOG_TYPE_NONE 0x00 +#define LOG_TYPE_ALL ( LOG_TYPE_ERROR & LOG_TYPE_INFO & LOG_TYPE_VERBOSE ) + + +#define ENTITY_TYPE_GENERAL 0x01 +#define ENTITY_TYPE_GPS 0x02 +#define ENTITY_TYPE_COMPASS 0x04 +#define ENTITY_TYPE_NAV 0x08 +#define ENTITY_TYPE_STATE 0x10 +#define ENTITY_TYPE_MISC 0x11 +#define ENTITY_TYPE_NONE 0x00 +#define ENTITY_TYPE_ALL (ENTITY_TYPE_GENERAL & ENTITY_TYPE_GPS & ENTITY_TYPE_COMPASS & ENTITY_TYPE_MISCENTITY_TYPE_NAV & ENTITY_TYPE_MISC ) + + +void extLog(const char type[], float val, int format=6) +{ + if (!extLogger_gen) + return; + Serial2.print(extlogSeqNum++); + Serial2.print(" "); + Serial2.print(millis()); + Serial2.print(" "); + Serial2.print(type); + Serial2.print(": "); + Serial2.println(val,format); +} +void extLog(const char type[], const char val[]) +{ + if (!extLogger_gen) + return; + + Serial2.print(extlogSeqNum++); + Serial2.print(" "); + Serial2.print(millis()); + Serial2.print(" "); + Serial2.print(type); + Serial2.print(": "); + Serial2.println(val); +} + +void extLog(const char type[], int value) +{ + if (!extLogger_gen) + return; + Serial2.print(extlogSeqNum++); + Serial2.print(" "); + Serial2.print(millis()); + Serial2.print(" "); + Serial2.print(type); + Serial2.print(": "); + Serial2.println(value); +} + + +#else +void extLog(const char type[], float val, int format=6) { return; } +void extLog(const char type[], const char val[]) { return; } +void extLog(const char type[], int value) { return; } + +#endif + + +//======================================= +// State flag utilities +//======================================= + +void setAutoStateFlag(uint8_t flag) +{ + String msg("Setting Flag " + String(flag)); + manager.sendString(msg.c_str()); + + extLog("Setting Flag",flag); + + + autoStateFlags |= flag; +} + +void clearAutoStateFlag(uint8_t flag) +{ + String msg("Clearing Flag " + String(flag)); + manager.sendString(msg.c_str()); + + extLog("Clearing Flag",flag); + + + autoStateFlags &= ~flag; +} + +bool isSetAutoStateFlag(uint8_t flag) +{ + return ( autoStateFlags & flag ) > 0 ? true : false; +} + + + + + + + + + + +//===================================== +// Main Functions +//===================================== + + +void setup() +{ + + + //commSerial->begin(Protocol::BAUD_RATE); + commSerial->begin(57600); + + #ifdef extLogger + Serial2.begin(115200); + #endif + + changeAPMState(APM_STATE_INIT); + setupSettings(); + + //initialize vars with sane values + lastOutputAngle=steerCenter; + lastAngularError=steerCenter; + + + // // Load accelerometer/magnetometer parameters from EEPROM + // if(!settings.foundIMUTune()) + // { + // /*#IMULOAD Couldn't load a valid Accelerometer and + // * Magnetometer tune from EEPROM + // **/ + // //errorsDetected = true; + // //comms.sendString("IMULOAD Failed"); + // manager.sendString("IMULOAD Failed"); + + // } + // else + // { + // //mpu.tuneAccl(settings.getAccelTune()); + // Serial2.println("Reading mag tune"); + // hmc.tune(settings.getMagTune()); + // Serial2.println("Done Reading mag tune"); + // } + + //start gps + //todo check if ok? + //set mag dec from settings? + //hmc.begin(); + + + gps.begin(); + + bumperSensor.begin(A6,A5); + + //*important* disable cs on Pressure sensor + // that is on same spi bus + // (mpu6000 driver did this in the drivers begin()) + pinMode(40, OUTPUT); + digitalWrite(40, HIGH); + mpudmp.begin(); + + for(int i=0; i<3; i++) + pinMode(LEDpin[i], OUTPUT); + + for(int i=0; i<3; i++) + servo[i].attach(ServoPin[i]); + + output(0.0f,steerCenter); + + delay(2000); + //calibrateGyro(); //this also takes one second + + APMRadio::setup(); + #if useEncoder + encoder::begin(EncoderPin[0], EncoderPin[1]); + #endif + manager.requestResync(); + + + //add state callbacks + manager.setStateStopCallback(stateStop); + manager.setStateStartCallback(stateStart); + manager.setBumperDisableCallback(bumperDisable); + manager.setBumperEnableCallback(bumperEnable); + manager.setVersionCallback(version); + + // //=== ba testing ===// + + // pinMode(A7, OUTPUT); + // pinMode(A8, OUTPUT); + + // pinMode(13, OUTPUT); + + // #ifdef extLogger + // pinMode(45, OUTPUT); + // #endif + + //=== end ba testing ===// + + + + + //while (gps.getWarning()) + + //todo loop while testing systems + //Maybe do ping sensor check? + //gps lock test? + //drive test? + // short move forward and read accel/gyro, pulse encoder + // have to include manager.update() in loop + //maybe move this all down into loop() + + + changeAPMState(APM_STATE_DRIVE); + changeDriveState(DRIVE_STATE_STOP); + changeAutoState(AUTO_STATE_FULL); + +//this all needs to be cleaned up after it works (hopefully) + for (uint8_t i=0;i<10;i++) + { + updateGyro(); + manager.update(); + delay(10); + } + + // compass_sync(); + // compass_sync(); + // compass_sync(); + // compass_sync(); + + euler_z_offset = mpudmp.getEulerZ(); + + + changeAPMState(APM_STATE_SELF_TEST); + +} + + + +void loop() +{ + int i; + + + + + + + //====================================================== + //Functions that don't directely affect the rover. + //====================================================== + + manager.update(); + + updateGPS(); + updateGyro(); + bumperSensor.update(); + + //====================================================== + //Functions that are run in various states and at + //various freqency + //====================================================== + + //Was thinking of trying to only run one task per loop but + //not sure if needed. Keep this around for a bit to decide + // //find the best task to run this time around + // for (i=0;i offset) + // { + // offset=lastTime; + // best_task=i; + // } + // } + // } + + for (i=0;i any other state + apmState=APM_STATE_INVALID; + break; + } + +} + + +void changeDriveState(uint8_t newState) +{ + + + //ignore if we are not changing state + if (driveState == newState) + return; + + + switch (newState) + { + case DRIVE_STATE_INVALID: + //Ignore change to invalid state because it is invalid + break; + + case DRIVE_STATE_STOP: + + switch(driveState) + { + + case DRIVE_STATE_INVALID: + case DRIVE_STATE_AUTO: + case DRIVE_STATE_RADIO: + manager.sendString("Drive State: Stop"); + + extLog("Drive State","Stop"); + + + #ifdef simMode + gps.setGroundSpeed(0); + #else + output(0,steerCenter); + #endif + + scheduler[SCHD_FUNC_EXTRPPOS].enabled = false; + scheduler[SCHD_FUNC_RDACC].enabled = true; + scheduler[SCHD_FUNC_RPRTLOC].enabled = true; + scheduler[SCHD_FUNC_RPRTSTATE].enabled = true; + scheduler[SCHD_FUNC_CHKPING].enabled = true; + scheduler[SCHD_FUNC_BUMPSENSOR].enabled = true; + + //scheduler[SCHD_FUNC_NAVIGATE].enabled = true; + #ifdef simMode + scheduler[SCHD_FUNC_UPDATESIM].enabled = true; + #endif + + driveState=newState; + + //still might be rolling so not best place? + //compass_sync(); + + if (isSetAutoStateFlag(AUTO_STATE_FLAG_CAUTION)) + clearAutoStateFlag(AUTO_STATE_FLAG_CAUTION); + break; + } + + break; + + case DRIVE_STATE_AUTO: + switch(driveState) + { + case DRIVE_STATE_INVALID: + case DRIVE_STATE_STOP: + case DRIVE_STATE_RADIO: + manager.sendString("Drive State: Auto"); + + extLog("Drive State","Auto"); + + backWaypoint = location; + + scheduler[SCHD_FUNC_EXTRPPOS].enabled = true; + scheduler[SCHD_FUNC_RDACC].enabled = true; + scheduler[SCHD_FUNC_RPRTLOC].enabled = true; + scheduler[SCHD_FUNC_RPRTSTATE].enabled = true; + scheduler[SCHD_FUNC_CHKPING].enabled = true; + scheduler[SCHD_FUNC_BUMPSENSOR].enabled = true; + //scheduler[SCHD_FUNC_NAVIGATE].enabled = true; + + #ifdef simMode + scheduler[SCHD_FUNC_UPDATESIM].enabled = true; + simTime=millis(); + #endif + + + nTime = millis(); + + + driveState=newState; + + break; + } + break; + + #ifndef simMode + case DRIVE_STATE_RADIO: + switch(driveState) + { + case DRIVE_STATE_INVALID: + case DRIVE_STATE_STOP: + case DRIVE_STATE_AUTO: + manager.sendString("Drive State: Radio"); + + extLog("Drive State","Radio"); + + + scheduler[SCHD_FUNC_EXTRPPOS].enabled = false; + scheduler[SCHD_FUNC_RDACC].enabled = true; + scheduler[SCHD_FUNC_RPRTLOC].enabled = true; + scheduler[SCHD_FUNC_RPRTSTATE].enabled = true; + scheduler[SCHD_FUNC_CHKPING].enabled = true; + scheduler[SCHD_FUNC_BUMPSENSOR].enabled = true; + //scheduler[SCHD_FUNC_NAVIGATE].enabled = true; + + //Only radio state sets previous drivestate + //so it knows what to revert back to + prevDriveState=driveState; + driveState=newState; + break; + + } + break; + #endif + } +} + + + +void changeAutoState(uint8_t newState) +{ + //ignore if we are not changing state + if (autoState == newState) + return; + + + switch (newState) + { + case AUTO_STATE_INVALID: + //Ignore change to invalid state because it is invalid + break; + + case AUTO_STATE_FULL: + switch(autoState) + { + case AUTO_STATE_INVALID: + case AUTO_STATE_AVOID: + manager.sendString("Auto State: Full"); + + extLog("Auto State","Full"); + + + sTime = 0; + + //maybe here? + //compass_sync(); + + + //change speed + autoState=newState; + break; + case AUTO_STATE_STALLED: + //don't transition from stalled + //do we allow a way of getting out of stall other then power cycle? + break; + } + break; + + case AUTO_STATE_AVOID: + switch(autoState) + { + case AUTO_STATE_INVALID: + case AUTO_STATE_FULL: + manager.sendString("Auto State: Avoid"); + + extLog("Auto State","Avoid"); + + + //If we had set caution flag then entered avoid we need to clear + //might consider leaving in caution but for now... + clearAutoStateFlag(AUTO_STATE_FLAG_CAUTION); + + + avoidState=AVOID_STATE_ENTER; + + + int8_t bumperBackDir; + + if (bumperSensor.leftButtonState()==bumperSensor.rightButtonState()) + { + //Caution bumperBackDir of 0 doesn't mean bumper sensor wasn't triggered + //It is either both sensors or triggered or neither + bumperBackDir = 0; + } + else if (bumperSensor.leftButtonState() > bumperSensor.rightButtonState()) + bumperBackDir = 1; + else + bumperBackDir = -1; + + + //bumper always takes precidence over ultrasonic + //except when both bumper sensors are triggered. Then use altrasonic to decide + if (bumperBackDir!=0) + { + backDir=bumperBackDir; + } + else + { + //if bumper isn't set (or both were triggered) then use ultrasonic to decide + if (ping[0][PING_CUR] < ping[4][PING_CUR]) + backDir = 1; + else + backDir = -1; + + } + + + + autoState=newState; + break; + case AUTO_STATE_STALLED: + //don't transition from stalled + //do we allow a way of getting out of stall other then power cycle? + break; + + } + + break; + case AUTO_STATE_STALLED: + switch(autoState) + { + case AUTO_STATE_INVALID: + case AUTO_STATE_FULL: + case AUTO_STATE_AVOID: + manager.sendString("Auto State: Stalled"); + + extLog("Auto State","Stalled"); + + autoState=newState; + //stop motors + break; + + } + break; + + } + +} + + + +#ifdef simMode +void updateSIM() +{ + //todo Set some randomness to path? + float steering_error=0; + + + extLog("updateSIM","====="); + + + //in sim always act like we are still getting gps updates + gps.setUpdatedRMC(); + + gps.setCourse(gps.getLocation().headingTo(manager.getTargetWaypoint())+steering_error); + + String msg("SetCourseSim " + String(gps.getCourse())); + manager.sendString(msg.c_str()); + + + //Only move if in auto mode + //Stalled is simulated but still handle + if (driveState == DRIVE_STATE_AUTO && autoState != AUTO_STATE_STALLED) + { + float dT = millis()-simTime; + + //3600000 = milliseconds per hour + float dTraveled = gps.getGroundSpeed()*dT/3600000.f; + + + String msg("sim ground speed " + String(gps.getGroundSpeed())); + manager.sendString(msg.c_str()); + + + char fbuff[32]; + char sbuff[32]; + dtostrf(dTraveled, 6, 6, fbuff); + sprintf(sbuff,"%s %s\n","Distance traveled",fbuff); + + //String msg1("sim traveled " + String(dTraveled)); + manager.sendString(sbuff); + + + //todo. Add Some kind of randomness? + Waypoint newLocation = location.extrapolate(gps.getCourse(), dTraveled); + + gps.setLocation(newLocation); + + //gps.setLatitude(newLocation.degLatitude()); + ////Serial.println(newLocation.degLatitude(),8); + //gps.setLongitude(newLocation.degLongitude()); + ////Serial.println(newLocation.degLongitude(),8); + + simTime=millis(); + + } + + +} +#endif + + +void handleAvoidState(int8_t speed, int8_t steer, uint8_t nextState, uint32_t timeout) +{ + output(speed,steer); + + if (millis() > sTime+timeout) + { + //reset timer + sTime=millis(); + //setup for next state + avoidState=nextState; + manager.sendString(AVOID_STATES_STRING[nextState]); + + } +} + +void navigate() +{ + float mph = ((APMRadio::get(RadioPin[1])-90) / 90.f)*MAN_MAX_FWD; + uint8_t steer = APMRadio::get(RadioPin[2]); + + + if (abs(steer-steerCenter) > 5 || fabs(mph)>0.8f ) + { + changeDriveState(DRIVE_STATE_RADIO); + + output(mph, steer); + } + else + { + if (driveState == DRIVE_STATE_RADIO ) + changeDriveState(prevDriveState); + } + + + if (driveState == DRIVE_STATE_AUTO ) + { + + if ( autoState == AUTO_STATE_AVOID) + { + //TODO: could be simplified into function calls + //inputs: speed,steer,nextstate,timeout + switch (avoidState) + { + case AVOID_STATE_ENTER: + //set timer + sTime=millis(); + //setup for next state + avoidState=AVOID_STATE_FWD_BRAKE; + manager.sendString(AVOID_STATES_STRING[AVOID_STATE_FWD_BRAKE]); + break; + case AVOID_STATE_FWD_BRAKE: + { + //todo consider consolitating foward and reverse into single function + + float current_speed = RPMtoMPH(encoder::getRPM()); + + //String msg("MPH " + String(mph)); + //manager.sendString(msg.c_str()); + //String msg1("RPM " + String(encoder::getRPM())); + //manager.sendString(msg1.c_str()); + + + //If new speed is low enough move on to new state + //TODO timeout as well? + if (current_speed < .5) + { + avoidState=AVOID_STATE_STRAIGHTBACK; + manager.sendString(AVOID_STATES_STRING[AVOID_STATE_STRAIGHTBACK]); + } + else + { + output(0,steerCenter); + } + sTime = millis(); + } + break; + case AVOID_STATE_STRAIGHTBACK: + handleAvoidState(revSpeed,steerCenter,AVOID_STATE_STEER,avoidStraightBack); + break; + case AVOID_STATE_STEER: + int8_t temp_steer; + if(backDir == 0) + { + temp_steer=steerCenter; + } + else if (backDir < 0) + { + temp_steer=steerCenter+revThrow; + } + else + { + temp_steer=steerCenter-revThrow; + } + handleAvoidState(revSpeed,temp_steer,AVOID_STATE_REV_BRAKE,avoidSteerBack); + break; + case AVOID_STATE_REV_BRAKE: + { + //todo consider consolitating foward and reverse into single function + + float current_speed = RPMtoMPH(encoder::getRPM()); + //calculate and set a reduce speed (maybe use non-linear in future?) + //current_speed=current_speed/2.0; + //If new speed is slow enough move on to new state + if (current_speed > -.5) + { + avoidState=AVOID_STATE_DONE; + manager.sendString(AVOID_STATES_STRING[AVOID_STATE_DONE]); + } + else + { + output(0,steerCenter); + } + sTime=millis(); + } + break; + case AVOID_STATE_DONE: + changeAutoState(AUTO_STATE_FULL); + break; + + } + + + } + else if (autoState == AUTO_STATE_FULL) + { + //drive based on pathHeading and side ping sensors + + float x,y; + float approachSpeed; + float angularError; + float outputAngle; + + //####################// + // Angle calculations // + //####################// + + + //difference between pathheading (based on waypoints (previous and current)) + //error can only be as much as 180 degrees off (opposite directions). + angularError = truncateDegree(pathHeading - trueHeading); + + extLog("angularError",angularError,6); + + //angularCorrection = (lastAngularError - angularError) - lastOutputAngle - lastAngularCorrection; + + + + if (!isSetAutoStateFlag(AUTO_STATE_FLAG_TURNAROUND)) + { + //turn around if next waypoint is behind the rover + if (angularError > 65) + { + setAutoStateFlag(AUTO_STATE_FLAG_TURNAROUND); + turnAroundDir=1; + } + else if (angularError < -65) + { + setAutoStateFlag(AUTO_STATE_FLAG_TURNAROUND); + turnAroundDir=-1; + } + } + else + { + //stop turning around when we are close to the right + //direction + if (angularError <= 35 && angularError >= -35) + { + clearAutoStateFlag(AUTO_STATE_FLAG_TURNAROUND); + turnAroundDir=0; + } + } + + + + + switch(steerStyle) + { + case 0: + //not sure. Ratio of opposite/adjacent multiplied by steering throw + //convert from degrees to radians for use with atan + outputAngle = atan( angularError*PI/180.l )*(2*steerThrow/PI); + break; + case 1: + //get squared of percentage of error from 180. x^2 function where 180=1.00 + outputAngle = ((angularError/180.l)*(angularError/180.l)); + + //above percentage of the full steerthrow + if (isSetAutoStateFlag(AUTO_STATE_FLAG_APPROACH) == false ) + outputAngle *=(4.l*steerThrow); + else + outputAngle *=(8.l*steerThrow); + + //negative angle if left turning vs right turning ( or opposite?) + if(angularError < 0) + outputAngle *= -1; + break; + default: + //simplest. Percentage of error multipled by 2x the steerthow + // if angularError is 90 degrees off we max out the steering (left/right) + // this is constrained below + // at this point we could be 180 degress off and have output angle of 90 + outputAngle = (angularError/180.l)*(2.l*steerThrow); + break; + } + + + if (isSetAutoStateFlag(AUTO_STATE_FLAG_TURNAROUND)) + { + //If the sign is swapped it is because the rover crossed over center line + //which means angularError is not 100% correct (i.e. 182 vs 178) but is close enough + //given that the outputAngle will be reduced to steerThrow + + //if we are forcing right turn but calculations says left; Force right. + //else if we are forcing left turn but calculations says right; Force left. + //else do nothing as we are turning in correct direction. + if (turnAroundDir == 1 && angularError < 0) + outputAngle *= -1.0; + else if (turnAroundDir == -1 && angularError > 0) + outputAngle *= -1.0; + } + + + //scale angle (default of 1.5) This is to account for the 45 deg steer really is 30 deg + outputAngle *= steerFactor; + + //Keep angle somewhat sensable to avoid issues with values getting larger the +/-180 + //arbitrarily using +/- 90 as it still give some "room" while still clamping. + outputAngle = constrain(outputAngle,double(-90.0),double(90.0)); + + //possible correction in steering (pulling left or right) + //outputAngle += steerSkew; + + extLog("nav outputAngle post err correct",outputAngle); + + //only adjust steering using the ping sensors if CAUTION flag is active + if (isSetAutoStateFlag(AUTO_STATE_FLAG_CAUTION)) { + //find x and y component of output angle + x = cos(toRad(outputAngle)); + y = sin(toRad(outputAngle)); + + //Not 100 sure: Generally using pings sensor values to adjust output angle + //modify x and y based on what pings are seeing + for(int i=0; i<5; i++) + { + //temp is some percentage (at ping of 1400 would mean tmp==1) + float tmp = ping[i][PING_CUR]/pingWeight; + //value is squared? + tmp *= tmp; + //add to x,y based on set angle of sensor inverse scaled of percentage + x += cos(toRad(pAngle[i]))/tmp; + y += sin(toRad(pAngle[i]))/tmp; + } + - gps.begin(); - mpu.begin(); - commSerial->begin(Protocol::BAUD_RATE); - for(int i=0; i<3; i++) pinMode(LEDpin[i], OUTPUT); - for(int i=0; i<3; i++) servo[i].attach(ServoPin[i]); - output(0.0f,steerCenter); + extLog("nav ping X adjust",x,6); + extLog("nav ping Y adjust",y,6); + extLog("nav outputAngle ping",toDeg(atan2(y,x)),6); - delay(2000); - calibrateGyro(); //this also takes one second - APMRadio::setup(); - #if useEncoder - encoder::begin(EncoderPin[0], EncoderPin[1]); - #endif - manager.requestResync(); - uTime = millis(); -} + //determine angle based on x,y then adjust from steering center ( 90 ) + outputAngle = toDeg(atan2(y,x))+steerCenter; + } + else { + outputAngle += steerCenter; + } + //Can't steer more then throw + outputAngle = constrain(outputAngle, + double(steerCenter-steerThrow), + double(steerCenter+steerThrow)); -void loop(){ - manager.update(); - updateGPS(); - updateGyro(); - if(uTime <= millis()){ - uTime += ScheduleDelay; - schedule[sIter](); - sIter++; - sIter = sIter%(sizeof(schedule)/sizeof(*schedule)); - navigate(); - } -} + //outputAngle is [45,135] steerCenter == 90 and steerThrow == 45 -void navigate(){ - float mph = ((APMRadio::get(RadioPin[1])-90) / 90.f)*maxFwd; - uint8_t steer = APMRadio::get(RadioPin[2]); - if (abs(steer-steerCenter) > 5 || fabs(mph)>0.8f ) { - //(APMRadio::get(RadioPin[0]) > 120) { - output(mph, steer); - } else if (oTime != 0) { - //Back Up - if(sTime == 0){ - output(0, steerCenter); - sTime = millis(); - backDir = ping[0] 45 + //90-135 => 45 + //45 - [0,45] + //disp is between 45 and 0 + //disp is largest the closest to steerCenter + //If we are turning hard with disp of 43,44 that could be low enough + //to set speed to less the maxFwd (assuming defaults) + //why? Basically if we turn too much or get within a few feet or target + //the speed is reduced. + //turn hard (45) means disp is close to 0 + //get within a couple of feet from target the speed is reduced + + float disp = steerThrow - abs(steerCenter-outputAngle); + //distance is in miles. Not sure why we convert to feet? + //this isn't really speed + //get slower and slower once we are less then maxfwd away? + float speed = (distance*5280.l); + + //not sure why one would use disp vs speed? Is this for turning distance? + //speed is even a distance. if we get Less the x feet away and it is smaller + //then approachSpeed and turning angle then we us it? + + speed = min(speed, disp)/6.f; //logical speed clamps + approachSpeed = manager.getTargetWaypoint().getApproachSpeed(); + speed = min(speed, approachSpeed); //put in target approach speed + + speed = constrain(speed, minFwd, maxFwd); + + //deal with speed reduction for caution and approach + if (isSetAutoStateFlag(AUTO_STATE_FLAG_CAUTION) || isSetAutoStateFlag(AUTO_STATE_FLAG_APPROACH)) + { + speed = speed/2; + } + + output(speed, outputAngle); + + lastOutputAngle=outputAngle; + lastAngularError=angularError; } + }//end drive state auto check + +} + +void extrapPosition() +{ + float dTraveled; + //digitalWrite(A7,HIGH); - outputAngle = toDeg(atan2(y,x))+steerCenter; - outputAngle = constrain(outputAngle, - double(steerCenter-steerThrow), - double(steerCenter+steerThrow)); + float dT = millis()-nTime; + //ignore irrational values + if(dT < 1000 && !gps.getWarning()) + { + //3600000 = milliseconds per hour + dTraveled = gps.getGroundSpeed()*dT/3600000.f; + dTraveled *= (2.l/3.l);//purposly undershoot + location = location.extrapolate(trueHeading, dTraveled); - float disp = steerThrow - abs(steerCenter-outputAngle); - float speed = (distance*5280.l); - speed = min(speed, disp)/6.f; //logical speed clamps - float approachSpeed = manager.getTargetWaypoint().getApproachSpeed(); - speed = min(speed, approachSpeed); //put in target approach speed - speed = constrain(speed, minFwd, maxFwd); + char str[32]; + GPS_COORD tempGPSCoord = location.m_gpsCoord; + gps_coord_to_str(&tempGPSCoord,str,32,9,"DD"); + extLog("Extrap coord",str); + //extLog("extrap lat",location.degLatitude(),6); + //extLog("extrap long",location.degLongitude(),6); - if(stop) output(0 , steerCenter); - else output(speed, outputAngle); + #ifdef M_DEBUG + // Logger Msg + ExtrapolatedPositionMsg_t msg; + GPS_ANGLE loc_lat = location.angLatitude(); + GPS_ANGLE loc_lon = location.angLongitude(); + msg.latitude.minutes = int16_t(loc_lat.minutes); + msg.latitude.frac = debugger.frac_float_to_int32(loc_lat.frac); + msg.longitude.minutes = int16_t(loc_lon.minutes); + msg.longitude.frac = debugger.frac_float_to_int32(loc_lon.frac); + msg.altitude = debugger.alt_float_to_uint16(0); + debugger.send(msg); + #endif } + + positionChanged(); + //digitalWrite(A7,LOW); + } -void updateGPS(){ + +void updateGPS() +{ + gps.update(); - // Read gps only when new data is available - static auto readIdx = gps.dataIndex(); - if(gps.dataIndex() != readIdx){ - readIdx = gps.dataIndex(); + + if(gps.getUpdatedRMC()) + { + new_gps = true; + digitalWrite(A8, HIGH); + + //todo is this not used in sim? + gps.clearUpdatedRMC(); + location = gps.getLocation(); - waypointUpdated(); - syncHeading(); - positionChanged(); - gpsTime = millis(); + + #ifdef extLogger + if (extLogger_gps) + { + char str[32]; + GPS_COORD tempGPSCoord = location.m_gpsCoord; + gps_coord_to_str(&tempGPSCoord,str,32,9,"DD"); + extLog("GPS coord",str); + // extLog("GPS lat",location.degLatitude(),6); + // extLog("GPS long",location.degLongitude(),6); + } + #endif + + #ifdef M_DEBUG + // Logger Msg + RawPositionMsg_t msg; + GPS_ANGLE loc_lat = location.angLatitude(); + GPS_ANGLE loc_lon = location.angLongitude(); + msg.latitude.minutes = int16_t(loc_lat.minutes); + msg.latitude.frac = debugger.frac_float_to_int32(loc_lat.frac); + msg.longitude.minutes = int16_t(loc_lon.minutes); + msg.longitude.frac = debugger.frac_float_to_int32(loc_lon.frac); + msg.altitude = debugger.alt_float_to_uint16(0); + debugger.send(msg); + #endif + + if (driveState == DRIVE_STATE_AUTO) + { + waypointUpdated(); + syncHeading(); + positionChanged(); + // currently not used + //gpsTime = millis(); + } + //digitalWrite(A8, LOW); } } -void waypointUpdated(){ - if(manager.numWaypoints() > 0 && !gps.getWarning()){ - stop = false; - //distance = calcDistance(manager.getTargetWaypoint(), location); - distance = manager.getTargetWaypoint().distanceTo(location); +void waypointUpdated() +{ + + distance = manager.getTargetWaypoint().distanceTo(location); + - if(distance > PointRadius) return; + extLog("Target lat", manager.getTargetWaypoint().degLatitude(),6); + extLog("Target long", manager.getTargetWaypoint().degLongitude(),6); - if(manager.getTargetIndex() < manager.numWaypoints()-1){ + //approach flag gets locked in until we we move to another waypoint or stop. + if (distance < approachRadius && isSetAutoStateFlag(AUTO_STATE_FLAG_APPROACH) == false ) + setAutoStateFlag(AUTO_STATE_FLAG_APPROACH); + + if(distance <= PointRadius) + { + + if(manager.getTargetIndex() < manager.numWaypoints()-1) + { backWaypoint = manager.getTargetWaypoint(); manager.advanceTargetIndex(); + } - else if (manager.loopWaypoints()){ + else if (manager.loopWaypoints()) + { backWaypoint = manager.getTargetWaypoint(); manager.setTargetIndex(0); } - else{ - stop = true; + else + { + changeDriveState(DRIVE_STATE_STOP); + manager.setTargetIndex(0); } + + clearAutoStateFlag(AUTO_STATE_FLAG_APPROACH); } } -void updateGyro(){ - float dt = lowFilter.millisSinceUpdate(); - float Gz = -toDeg(mpu.gyroZ()); - lowFilter.update(Gz); - highFilter.update(Gz-lowFilter.get()); - trueHeading = truncateDegree(trueHeading + dt*(highFilter.get())); +void syncHeading() +{ + // if(!gps.getWarning() && gps.getCourse() != 0) + // { - if(gpsHalfTime < millis() && gpsHalfTime!=0){ - gyroHalf = trueHeading; - gpsHalfTime = 0; - } -} + // if (!isSetAutoStateFlag(AUTO_STATE_AVOID) && !isSetAutoStateFlag(AUTO_STATE_FLAG_APPROACH) && + // (fabs(lastOutputAngle)<20.0) ) + // { + // trueHeading = gps.getCourse(); + // euler_z_offset = last_euler_z - trueHeading; -void syncHeading(){ - if(!gps.getWarning() && gps.getCourse()!=0){ - trueHeading = gps.getCourse(); - /* - if(millis() - gpsTime < 1500) //dont use gyrohalf if it is too old - trueHeading = truncateDegree(gps.getCourse() + trueHeading - gyroHalf); - else - trueHeading = truncateDegree(gps.getCourse()); - gpsHalfTime = millis()+(millis()-gpsTime)/2;*/ - } - else if(stop) trueHeading = pathHeading; -} -void checkPing(){ - ping[pIter] = getPing(PingPin[pIter]); - pIter+=2; - pIter = pIter%5; - if(ping[pIter] < warn[pIter]) oTime = millis(); -} + // extLog("SH trueHeading", trueHeading,6); -void extrapPosition(){ - float dT = millis()-nTime; - if(dT < 1000 && !gps.getWarning()){ //ignore irrational values - //3600000 = milliseconds per hour - float dTraveled = gps.getGroundSpeed()*dT/3600000.f; - dTraveled *= (2.l/3.l);//purposly undershoot - location = location.extrapolate(trueHeading, dTraveled); - } - positionChanged(); + // /* + // if(millis() - gpsTime < 1500) //dont use gyrohalf if it is too old + // trueHeading = truncateDegree(gps.getCourse() + trueHeading - gyroHalf); + // else + // trueHeading = truncateDegree(gps.getCourse()); + // gpsHalfTime = millis()+(millis()-gpsTime)/2;*/ + // } + // } + } -void positionChanged(){ + + + +void positionChanged() +{ nTime = millis(); - if(manager.numWaypoints() <= 0) return; + distance = manager.getTargetWaypoint().distanceTo(location); - if(backWaypoint.radLongitude() == 0 || distance*5280.l < 25){ + + extLog("PC distance",distance,6); + extLog("PC lat",location.degLatitude(),6); + extLog("PC long",location.degLongitude(),6); + + #ifdef M_DEBUG + // Logger Msg + WaypointMsg_t msg; + + GPS_ANGLE backWpLat = backWaypoint.angLatitude(); + GPS_ANGLE backWpLon = backWaypoint.angLongitude(); + msg.latStart.minutes = int16_t(backWpLat.minutes); + msg.latStart.frac = debugger.frac_float_to_int32(backWpLat.frac); + msg.lonStart.minutes = int16_t(backWpLon.minutes); + msg.lonStart.frac = debugger.frac_float_to_int32(backWpLon.frac); + + Waypoint end_target = manager.getTargetWaypoint(); + GPS_ANGLE endTargetWpLat = end_target.angLatitude(); + GPS_ANGLE endTargetWpLon = end_target.angLongitude(); + msg.latTarget.minutes = int16_t(endTargetWpLat.minutes); + msg.latTarget.frac = debugger.frac_float_to_int32(endTargetWpLat.frac); + msg.lonTarget.minutes = int16_t(endTargetWpLon.minutes); + msg.lonTarget.frac = debugger.frac_float_to_int32(endTargetWpLon.frac); + #endif + + if (kalman_heading && !heading_lock) { + pathHeading = trueHeading; //drive straight + return; + } + + if (backWaypoint.radLongitude() == 0 || distance*5280.l < 25) + { + #ifdef M_DEBUG + msg.latIntermediate.minutes = 0; + msg.latIntermediate.frac = 0; + msg.lonIntermediate.minutes = 0; + msg.lonIntermediate.frac = 0; + #endif pathHeading = location.headingTo(manager.getTargetWaypoint()); - } else { - double full = backWaypoint.distanceTo(manager.getTargetWaypoint()); - double AB = backWaypoint.headingTo(manager.getTargetWaypoint()); - double AL = backWaypoint.headingTo(location); - double d = cos(toRad(AL-AB)) * backWaypoint.distanceTo(location); - double D = d + (full-d)*(1.l-lineGravity); - Waypoint target = backWaypoint.extrapolate(AB, D); - pathHeading = location.headingTo(target); + } + else + { + // find a point along path from backwaypoint to targetwaypoint in which to drive toward (pathHeading) + // First we determine how much of our current vector "counts" toward the target (cos). + // Whatever is left of distance along that original (optimal) vector is scalled by lineGravity. + // LineGravity will scale the point to either heading directory orthogonal to original (optimal) vector or + // directly toward the target waypoint + + //full distance from previous waypoint to target waypoint + float full = backWaypoint.distanceTo(manager.getTargetWaypoint()); + //heading (degrees) to the target waypoint from previous waypoint + float AB = backWaypoint.headingTo(manager.getTargetWaypoint()); + //heading from where we are at compared to previous waypoint + float AL = backWaypoint.headingTo(location); + //percentage of our current vector. what amount of distance in in direction we should be going. + float d = cos(toRad(AL-AB)) * backWaypoint.distanceTo(location); + //scale remaining distance by linegravity (0,1) then add the 'd' distance. + float D = d + (full-d)*(1.l-lineGravity); + //find waypoint for this new distance along the previous-to-target path + Waypoint intermediate = backWaypoint.extrapolate(AB, D); + //Find the heading to get there from current location + pathHeading = location.headingTo(intermediate); + + #ifdef M_DEBUG + GPS_ANGLE intermediateWpLat = intermediate.angLatitude(); + GPS_ANGLE intermediateWpLon = intermediate.angLongitude(); + + msg.latIntermediate.minutes = int16_t(intermediateWpLat.minutes); + msg.latIntermediate.frac = debugger.frac_float_to_int32(intermediateWpLat.frac); + msg.lonIntermediate.minutes = int16_t(intermediateWpLon.minutes); + msg.lonIntermediate.frac = debugger.frac_float_to_int32(intermediateWpLon.frac); + #endif } -} -void readAccelerometer(){ - Ax = mpu.acclX(); - Ay = mpu.acclY(); - Az = mpu.acclZ(); - pitch.update( atan2(sqrt(Ax*Ax+Az*Az), Ay) ); - roll .update( atan2(sqrt(Ay*Ay+Az*Az),-Ax) ); + #ifdef M_DEBUG + msg.pathHeading = (int16_t)(truncateDegree(pathHeading)*100.0); + debugger.send(msg); + #endif + + extLog("PC PathHeading",pathHeading,6); + } -void reportLocation(){ - float voltage = float(analogRead(67)/1024.l*5.l*10.1f); - float amperage = float(analogRead(66)/1024.l*5.l*17.0f); - manager.sendTelem(Protocol::telemetryType(LATITUDE), location.degLatitude()); - manager.sendTelem(Protocol::telemetryType(LONGITUDE), location.degLongitude()); - manager.sendTelem(Protocol::telemetryType(HEADING), trueHeading); - manager.sendTelem(Protocol::telemetryType(PITCH), toDeg(pitch.get())-90); - manager.sendTelem(Protocol::telemetryType(ROLL), toDeg(roll.get())-90); - manager.sendTelem(Protocol::telemetryType(GROUNDSPEED), RPMtoMPH(encoder::getRPM())); - manager.sendTelem(Protocol::telemetryType(VOLTAGE), voltage); - manager.sendTelem(Protocol::telemetryType(VOLTAGE+1), amperage); +void checkBumperSensor() +{ + //If bumper is disabled, do nothing. + if(!isBumperEnabled) return; + + uint8_t leftButtonState=bumperSensor.leftButtonState(); + uint8_t rightButtonState=bumperSensor.rightButtonState(); + + //String msg("Left: " + String(leftButtonState) + " Right: " + String(rightButtonState)); + // manager.sendString(msg.c_str()); + + if ( driveState == DRIVE_STATE_AUTO && autoState == AUTO_STATE_FULL) + { + if (leftButtonState || rightButtonState) + { + + changeAutoState(AUTO_STATE_AVOID); + } + } } -void calibrateGyro(){ //takes one second - double tmp = 0; - for(int i=0; i<100; i++){ - double Gz = toDeg(mpu.gyroZ()); - tmp += Gz/100; - delay(10); +void checkPing() +{ + + //digitalWrite(A8,HIGH); + + //copy cur value into last + ping[pIter][PING_LAST] = ping[pIter][PING_CUR]; + + #ifndef simMode + ping[pIter][PING_CUR] = getPing(PingPin[pIter]); + #else + pinMode(PingPin[pIter], INPUT); + ping[pIter][PING_CUR] = map(analogRead(PingPin[pIter]),0,1023,0,10000); + #endif + + //iteration goes 0 2 4 1 3 + pIter+=2; + pIter = pIter%5; + + + if ( driveState == DRIVE_STATE_AUTO && autoState == AUTO_STATE_FULL) + { + + if(ping[pIter][PING_CUR] < blockLevel[pIter] && ping[pIter][PING_LAST] < blockLevel[pIter]) + { + changeAutoState(AUTO_STATE_AVOID); + } + else + { + + if (ping[pIter][PING_CUR] < warnLevel[pIter] && ping[pIter][PING_LAST] < warnLevel[pIter]) + { + if ( !isSetAutoStateFlag(AUTO_STATE_FLAG_CAUTION)) + { + setAutoStateFlag(AUTO_STATE_FLAG_CAUTION); + //extLog("Caution flag: ","1"); + } + //set current time + pTime=millis(); + } + else + { + if (isSetAutoStateFlag(AUTO_STATE_FLAG_CAUTION)) + { + //check if all sonars before clearing flag + int clear_caution = 0; + for(int i=0;i<5;i++) { + if (ping[i][PING_CUR] >= warnLevel[i]) { + clear_caution++; + } + } + + if (clear_caution >= 5) { + clearAutoStateFlag(AUTO_STATE_FLAG_CAUTION); + } + //extLog("Caution flag: ","0"); + } + } + } + } + + #ifdef M_DEBUG + if (pIter == 0) //only log when all ping values have been updated + { + // Logger Msg + SonarMsg_t msg; + msg.ping1 = ping[0][PING_CUR]; + msg.ping2 = ping[1][PING_CUR]; + msg.ping3 = ping[2][PING_CUR]; + msg.ping4 = ping[3][PING_CUR]; + msg.ping5 = ping[4][PING_CUR]; + debugger.send(msg); } - lowFilter.set(tmp); + #endif + + //digitalWrite(A8,LOW); } -void output(float mph, uint8_t steer){ + +void output(float mph, uint8_t steer) +{ + static uint8_t straight_ctr = 0; + + if (abs(steer - steerCenter) > 5) { + driving_straight = false; + straight_ctr = 0; + } + else { + straight_ctr++; + if (straight_ctr >= 5) { + driving_straight = true; + straight_ctr = 5; //don't let overflow + } + } + steer += steer_bias; + + #ifdef simMode + //set speed to gps simulator (simulator only) + gps.setGroundSpeed(mph); + #endif + + + extLog("mph",mph,6); + extLog("steer",steer,6); + + #ifdef M_DEBUG + // Logger Msg + ControlMsg_t msg; + msg.speed = (int16_t)(mph*100); + msg.steering = steer; + debugger.send(msg); + #endif + #if useEncoder //the one wire encoder would need to feed fabs(mph) to cruise //and the output's sign would need to be flipped based on sign(mph) - if(abs(mph)<0.5f){ + if(abs(mph)<0.5f) + { cruise.stop(); - } else { + } + else + { cruise.set(MPHtoRPM(mph)); } float outputval = cruise.update(encoder::getRPM()); @@ -330,92 +1877,468 @@ void output(float mph, uint8_t steer){ servo[2].write(180-steer); } -void newPIDparam(float x){ +void updateHeading() +{ + static bool gps_heading_init = false; //5 good GPS course msgs + + //Tuning parameters + float error_propagation_rate = 0.5729578; // rad/s in deg + float gps_heading_error = 57.2958; // 1 rad in deg + float speed_thresh = 0.5592341; // 0.25 m/s in mph-- GPS heading will be bad at very low speeds +// bool apply_heading_lock = true; +// int heading_lock_init_time = 5000; // milliseconds + + //The main estimate (initialized to 0) + static float cur_heading_est = 0.0; + static float cur_heading_variance = 1000000; + + // To be populated with actual data from the system +// float cur_gyro_meas = -mpudmp.getGyroZ_raw(); // This should update at least once per estimation cycle (otherwise there's no reason do the estimation so frequently). Make sure that the direction is consistent with orientation. + float cur_wheel_speed = RPMtoMPH(encoder::getRPM()); + + static int last_time = mpudmp.lastUpdateTime(); +// static int stopped_time = 0; //in milliseconds + + static int heading_good_counter = 0; + + static float offset = 0; + int cur_time = mpudmp.lastUpdateTime(); + int dt = cur_time - last_time; + last_time = cur_time; + +/* // Heading lock + if (cur_wheel_speed == 0.0) { + stopped_time += dt; + if (apply_heading_lock and stopped_time > heading_lock_init_time) { + return; + } + } + else { + // rover is moving, reset "stop_time" + stopped_time = 0; + } +*/ + + // prediction: + float euler_z = toDeg(mpudmp.getEulerZ()); + cur_heading_est = truncateDegree(euler_z - offset); + float old_heading_est = truncateDegree(euler_z - offset); + cur_heading_variance += pow((error_propagation_rate * dt/1000.0),2); + + if (new_gps) { + float cur_gps_heading_meas = truncateDegree(gps.getCourse()); // this will update only once a second? + new_gps = false; + + //ensure that we have gotten at least one valid gps course + if (!gps_heading_init && fabs(cur_wheel_speed) > speed_thresh) { + if (cur_gps_heading_meas > 0.0001 || cur_gps_heading_meas < -0.0001) { + gps_heading_init = true; + } + } + + // correction: + // only apply correction if: + // - vehicle speed is above speed threshold + // - a valid GPS course is available (start using course before turning allowed) + // - we are not currently in a hard turn (turn around) mode + // - we are driving relativly straight + if (fabs(cur_wheel_speed) > speed_thresh && gps_heading_init && !isSetAutoStateFlag(AUTO_STATE_FLAG_TURNAROUND) && driving_straight) { + float update_heading = cur_gps_heading_meas; + if (cur_wheel_speed < 0) { + // driving backwards, flip the heading measurement + update_heading = truncateDegree(update_heading + 180.0); + } + float gps_heading_var = pow(gps_heading_error,2); + float heading_diff = truncateDegree(update_heading - cur_heading_est); + float var_denominator = (1.0 / cur_heading_variance + 1.0 / gps_heading_var); + cur_heading_est = truncateDegree((cur_heading_est / cur_heading_variance + (cur_heading_est + heading_diff) / gps_heading_var) / var_denominator); + cur_heading_variance = 1.0 / var_denominator; + + offset -= truncateDegree(cur_heading_est - old_heading_est); + + //keep variance high until we have a stable solution + if (heading_lock == false) + { + if (abs(heading_diff) < HEADING_LOCK_RANGE) { + heading_good_counter++; + if (heading_good_counter >= MIN_GOOD_HEADINGS) { + heading_lock = true; + } + else { + cur_heading_variance *= 1000; + } + } + else { + cur_heading_variance *= 1000; + heading_good_counter = 0; + } + } + } + } + + k_heading = truncateDegree(euler_z - offset); +} + +void updateGyro() +{ + //update gyro and accel here (mpu6000_dmp) + mpudmp.update(); + + if (mpudmp.updateReady()) + { + updateHeading(); + float euler_x, euler_y, euler_z; + + //mpu.getQ(q_w,q_x,q_y,q_z); + + mpudmp.getEuler(euler_x,euler_z,euler_z); + + + + //Serial2.print("eulerz; "); + //Serial2.println(euler_z); + + + last_euler_z = cur_euler_z; + cur_euler_z = euler_z; + trueHeading = cur_euler_z - euler_z_offset; + + //correct for any wraps out of the -3.14 to 3.14 + if (trueHeading < -PI) + trueHeading += 2*PI; + + if (trueHeading > PI) + trueHeading -= 2*PI; + + trueHeading = toDeg(trueHeading); + + mpudmp.clearUpdateReady(); + } + + if (mpudmp.lastUpdateTime() > (millis() + 2000 )) + { + if ( mpudmp.lastUpdateTime() > (millis() + 4000)) + { + manager.sendString("Gyro: error"); + mpudmp.setLastUpdateTime(millis()); + } + + } + + if (kalman_heading) { + trueHeading = k_heading; + } + + // float dt = lowFilter.millisSinceUpdate(); + // float Gz = -toDeg(mpu.gyroZ()); + // lowFilter.update(Gz); + // highFilter.update(Gz-lowFilter.get()); + + // //todo think about simulating the gyro? + // #ifndef simMode + // trueHeading = truncateDegree(trueHeading + dt*(highFilter.get())); + + + // extLog("UG trueHeading", trueHeading,6); + + + // if(gpsHalfTime < millis() && gpsHalfTime!=0){ + // gyroHalf = trueHeading; + // gpsHalfTime = 0; + // } + // #endif + + #ifdef M_DEBUG + // Logger Msg + OrientationMsg_t msg; + msg.heading = (uint16_t)(trueHeading*100); + msg.roll = (uint16_t)(toDeg(mpudmp.getEulerY())*100); + msg.pitch = (uint16_t)(toDeg(mpudmp.getEulerX())*100); + debugger.send(msg); + #endif +} + + +void readAccelerometer() +{ + + // digitalWrite(13,HIGH); + // Ax = mpu.acclX(); + // Ay = mpu.acclY(); + // Az = mpu.acclZ(); + // //atan2 gets angle of x and y vectors + // pitch.update( atan2(sqrt(Ax*Ax+Az*Az), Ay) ); + // //atan2 gets angle of y and z vectors + // roll.update( atan2(sqrt(Ay*Ay+Az*Az),-Ax) ); + // digitalWrite(13,LOW); +} + +void compass_update() +{ + //cur_compass_heading = hmc.getAzimuth(); + //cur_compass_heading_avg; +} + +void compass_sync() +{ + + // if ( driveState == DRIVE_STATE_STOP) + // { + // //=== use gps to set offset of gyro ===// + // //todo clean this up + // float tmp = hmc.getAzimuth(); + // float gyro_tmp = mpudmp.getEulerZ(); + // float compassRaw = hmc.getRawAzimuth(); + + // Serial2.print("compass raw: "); + // Serial2.print(toDeg(compassRaw)); + // Serial2.print(" "); + // Serial2.println(compassRaw); + + + // Serial2.print("compass: "); + // Serial2.print(toDeg(tmp)); + // Serial2.print(" "); + // Serial2.println(tmp); + + // Serial2.print("Compass \"Calib\" offset: "); + // Serial2.println(compassOffset); + + + // //tmp += (M_PI+0.34); + // tmp += (compassOffset+0.34); + // if (tmp < -M_PI) + // tmp = tmp + (2*M_PI); + // if (tmp > M_PI) + // tmp = tmp - (2*M_PI); + + + // Serial2.print("compass_corrected: "); + // Serial2.print(toDeg(tmp)); + // Serial2.print(" "); + // Serial2.println(tmp); + + + + // Serial2.print("gyro: "); + // Serial2.print(toDeg(gyro_tmp)); + // Serial2.print(" "); + // Serial2.println(gyro_tmp); + + + // euler_z_offset=gyro_tmp-tmp; + // //correct for any wraps out of the -3.14 to 3.14 + // if (euler_z_offset < -PI) + // euler_z_offset += 2*PI; + + // if (euler_z_offset > PI) + // euler_z_offset -= 2*PI; + + + // Serial2.print("offset: "); + // Serial2.print(toDeg(euler_z_offset)); + // Serial2.print(" "); + // Serial2.println(euler_z_offset); + + + + // Serial2.print("trueHeading: "); + // Serial2.println(trueHeading); + + // } +} + +void reportLocation() +{ + digitalWrite(45,HIGH); + + float voltage = float(analogRead(67)/1024.l*5.l*10.1f); + float amperage = float(analogRead(66)/1024.l*5.l*17.0f); + manager.sendTelem(Protocol::telemetryType(LATITUDE), location.degLatitude()); + manager.sendTelem(Protocol::telemetryType(LONGITUDE), location.degLongitude()); + + #ifdef simMode + manager.sendTelem(Protocol::telemetryType(HEADING), gps.getCourse()); + #else + manager.sendTelem(Protocol::telemetryType(HEADING), trueHeading); + #endif + + //manager.sendTelem(Protocol::telemetryType(PITCH), toDeg(pitch.get())-90); + //manager.sendTelem(Protocol::telemetryType(ROLL), toDeg(roll.get())-90); + manager.sendTelem(Protocol::telemetryType(PITCH), toDeg(mpudmp.getEulerX())); + manager.sendTelem(Protocol::telemetryType(ROLL), toDeg(mpudmp.getEulerY())); + + #ifdef simMode + manager.sendTelem(Protocol::telemetryType(GROUNDSPEED), gps.getGroundSpeed()); + #else + manager.sendTelem(Protocol::telemetryType(GROUNDSPEED), RPMtoMPH(encoder::getRPM())); + #endif + manager.sendTelem(Protocol::telemetryType(VOLTAGE), voltage); + manager.sendTelem(Protocol::telemetryType(VOLTAGE+1), amperage); + + //TODO define num of ping sensors and loop? + manager.sendSensor(Protocol::dataSubtype(OBJDETECT_SONIC),0, ping[0][PING_CUR] ); + manager.sendSensor(Protocol::dataSubtype(OBJDETECT_SONIC),1, ping[1][PING_CUR] ); + manager.sendSensor(Protocol::dataSubtype(OBJDETECT_SONIC),2, ping[2][PING_CUR] ); + manager.sendSensor(Protocol::dataSubtype(OBJDETECT_SONIC),3, ping[3][PING_CUR] ); + manager.sendSensor(Protocol::dataSubtype(OBJDETECT_SONIC),4, ping[4][PING_CUR] ); + + manager.sendSensor(Protocol::dataSubtype(OBJDETECT_BUMPER),BUMPER_BUTTON_LEFT, bumperSensor.leftButtonState()); + manager.sendSensor(Protocol::dataSubtype(OBJDETECT_BUMPER),BUMPER_BUTTON_RIGHT, bumperSensor.rightButtonState()); + + //extra gps info + manager.sendTelem(Protocol::telemetryType(GPSNUMSAT), gps.getNumSat()); + manager.sendTelem(Protocol::telemetryType(GPSHDOP), gps.getHDOP()); + + //extra gps info + manager.sendTelem(Protocol::telemetryType(HEADING_LOCK), ( heading_lock ) == true ? 1 : 0); + + digitalWrite(45,LOW); +} + +void reportState() +{ + digitalWrite(45,HIGH); + + manager.sendState(Protocol::stateType(APM_STATE),apmState); + manager.sendState(Protocol::stateType(DRIVE_STATE),driveState); + manager.sendState(Protocol::stateType(AUTO_STATE),autoState); + manager.sendState(Protocol::stateType(AUTO_FLAGS),autoStateFlags); + manager.sendState(Protocol::stateType(GPS_STATE),gps.getWarning()); + + + digitalWrite(45,LOW); +} + + +// void calibrateGyro(){ //takes one second +// float tmp = 0; +// for(int i=0; i<100; i++) +// { +// float Gz = toDeg(mpu.gyroZ()); +// tmp += Gz/100; +// delay(10); +// } +// lowFilter.set(tmp); +// } + + + +void newPIDparam(float x) +{ // indexes for cruise control PID settings defined below cruisePID = PIDparameters(settings.get(11), settings.get(12), settings.get(13), -90, 90 ); } +void stateStop() +{ + changeDriveState(DRIVE_STATE_STOP); +} + +void stateStart() +{ + changeDriveState(DRIVE_STATE_AUTO); +} + +void bumperEnable() +{ + isBumperEnabled = true; +} + +void bumperDisable() +{ + isBumperEnabled = false; +} + +void version() +{ + manager.sendVersion(version_major, version_minor, version_rev); +} + -void setupSettings(){ - /*GROUNDSETTING index="0" name="line gravity " min="0" max="1" def="0.50" +void setupSettings() +{ + /*GROUNDSETTING index="0" name="Line Gravity" min="0" max="1" def="0.50" *Defines how strongly the rover should attempt to return to the original *course between waypoints, verses the direct path from its current location * to the target
*/ settings.attach(0, .50, callback); - /*GROUNDSETTING index="1" name="steer throw" min="0" max="90" def="45" + /*GROUNDSETTING index="1" name="Steer Throw" min="0" max="90" def="45" *The number of degrees that rover will turn its wheels when it needs to *to turn its most extreme amount */ settings.attach(1, 45, callback); - /*GROUNDSETTING index="2" name="steer style" min="0" max="2" def="1" - *switches between arctangent of error steering (0)
+ /*GROUNDSETTING index="2" name="Steer Style" min="0" max="2" def="1" + *Switches between arctangent of error steering (0)
*square of error steering (1)
*and proportional to error steering (2) */ settings.attach(2, 1, callback); - /*GROUNDSETTING index="3" name="steer scalar" min="0" max="+inf" def="1" + /*GROUNDSETTING index="3" name="Steer Scalar" min="0" max="8" def="1.5" *Multiplier that determines how aggressively to steer */ - settings.attach(3, 1.0, callback); + settings.attach(3, 1.5, callback); - /*GROUNDSETTING index="4" name="min fwd speed" min="0" max="+inf" def="1.5" - *minimum forward driving speed in MPH + + /*GROUNDSETTING index="4" name="Min Fwd Speed" min="1" max="3" def="1.5" + *Minimum forward driving speed in MPH */ settings.attach(4, 1.5, callback); - /*GROUNDSETTING index="5" name="max fwd speed" min="0" max="+inf" def="6.0" - *maximum forward driving speed in MPH + /*GROUNDSETTING index="5" name="Max Fwd Speed" min="1.5" max="3" def="2.0" + *Maximum forward driving speed in MPH */ - settings.attach(5, 6.0, callback); + settings.attach(5, 2.0, callback); - /*GROUNDSETTING index="6" name="rev str throw" min="0" max="90" def="20" + /*GROUNDSETTING index="6" name="Rev Str Throw" min="0" max="90" def="20" *How far to turn the wheels when backing away from an obstacle */ settings.attach(6, 20, callback); - /*GROUNDSETTING index="7" name="reverse speed" min="-inf" max="0" def="-1.5" - *speed in MPH to drive in reverse + /*GROUNDSETTING index="7" name="Reverse Speed" min="-2" max="-1" def="-1.0" + *Speed in MPH to drive in reverse */ - settings.attach(7, -1.5, callback); + settings.attach(7, -1.0, callback); - /*GROUNDSETTING index="8" name="ping factor" min="1" max="+inf" def="1400" + /*GROUNDSETTING index="8" name="Ping Factor" min="1" max="20000" def="1400" *Factor to determine how strongly obstacles effect the rover's course
*Larger numbers correspond to larger effects from obstacles */ settings.attach(8, 1400, callback); - /*GROUNDSETTING index="9" name="coast time" min="0" max="+inf" def="1500" + /*GROUNDSETTING index="9" name="Coast Time" min="2000" max="8000" def="2000" *Time in milliseconds to coast before reversing when an obstacle is encountered */ - settings.attach(9, 1500, callback); + settings.attach(9, 2000, callback); - /*GROUNDSETTING index="10" name="min rev time" min="0" max="+inf" def="800" - *minimum time in milliseconds to reverse away from an obstacle + /*GROUNDSETTING index="10" name="Min Rev Time" min="500" max="2000" def="1000" + *Minimum time in seconds to reverse away from an obstacle */ - settings.attach(10, 800, &dangerTimeCallback); + settings.attach(10, 1000, &dangerTimeCallback); - /*GROUNDSETTING index="11" name="Cruise P" min="0" max="+inf" def="0.05" + /*GROUNDSETTING index="11" name="Cruise P" min="0" max="1" def="0.05" *P term in cruise control PID loop */ settings.attach(11, 0.05, &newPIDparam); - /*GROUNDSETTING index="12" name="Cruise I" min="0" max="+inf" def="0.1" + /*GROUNDSETTING index="12" name="Cruise I" min="0" max="10" def="0.1" *I term in cruise control PID loop */ settings.attach(12, 0.1, &newPIDparam); - /*GROUNDSETTING index="13" name="Cruise D" min="0" max="+inf" def="0.0" + /*GROUNDSETTING index="13" name="Cruise D" min="0" max="10" def="0.0" *D term in cruise control PID loop */ settings.attach(13, 0.0, &newPIDparam); - /*GROUNDSETTING index="14" name="Tire Diameter" min="0" max="+inf" def="5.85" + + /*GROUNDSETTING index="14" name="Tire Diameter" min="0" max="12" def="5.85" *Tire Diameter in inches, used to calculate MPH */ settings.attach(14, 5.85, callback); @@ -424,5 +2347,74 @@ void setupSettings(){ *Center point in degrees corresponding to driving straight */ settings.attach(15, 90, callback); + +//Target radi settings + + + /*GROUNDSETTING index="16" name="Waypoint acheived radius in miles" min="0" max=".003" def=".0015" + * Radius centered at waypoint where target is determined to be meet + */ + settings.attach(16, .0015, callback); + + /*GROUNDSETTING index="17" name="Approach radius" min="0" max=".0076" def=".0038" + * Radius cneter at waypoint where the approach flag is set + */ + settings.attach(17, .0038, callback); + + //testing: gyro sync with north + /*GROUNDSETTING index="18" name="GryoSync" min="0" max="1" def="0" + * Change to init gyro sync to north + */ + settings.attach(18, 0, &triggerGyroSyncNorth); + +//skew + + /*GROUNDSETTING index="20" name="Steer Skew" min="-45" max="45" def="0" + * + */ + settings.attach(20, 0, callback); + +//ping + + /*GROUNDSETTING index="21" name="Avoid Ping value Edges" min="500" max="10000" def="1000" + * + */ + settings.attach(21, 1000, &pingBlockLevelEdgesCallback); + + /*GROUNDSETTING index="22" name="Avoid Ping value Middles" min="500" max="10000" def="1600" + * + */ + settings.attach(22, 1600, &pingBlockLevelMiddlesCallback); + + /*GROUNDSETTING index="23" name="Avoid Ping value center" min="500" max="10000" def="3000" + * + */ + settings.attach(23, 3000, &pingBlockLevelCenterCallback); + + + + + /*GROUNDSETTING index="24" name="Warn Ping value Edges" min="500" max="10000" def="2500" + * + */ + settings.attach(24, 2500, &pingWarnLevelEdgesCallback); + + /*GROUNDSETTING index="25" name="Warn Ping value Middles" min="500" max="10000" def="5000" + * + */ + settings.attach(25, 5000, &pingWarnLevelMiddlesCallback); + + /*GROUNDSETTING index="26" name="Warn Ping value center" min="500" max="10000" def="8000" + * + */ + settings.attach(26, 8000, &pingWarnLevelCenterCallback); + + + + + settings.attach(27, compassOffset, callback); + } + + diff --git a/examples/RoboMagellan6x6/version.h b/examples/RoboMagellan6x6/version.h new file mode 100644 index 0000000..9f4f23f --- /dev/null +++ b/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 diff --git a/examples/bumperTest/bumperTest.ino b/examples/bumperTest/bumperTest.ino new file mode 100644 index 0000000..2061037 --- /dev/null +++ b/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); + +} \ No newline at end of file diff --git a/examples/streamGPS_new/streamGPS_new.ino b/examples/streamGPS_new/streamGPS_new.ino new file mode 100644 index 0000000..68ba628 --- /dev/null +++ b/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); + } +} diff --git a/library.properties b/library.properties index 11d127e..d24298d 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ -name=MINDS-i-Drone -version=1.3.0 -author=MINDS-i corp. -maintainer=MINDS-i -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 +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 diff --git a/src/MINDS-i-Drone.h b/src/MINDS-i-Drone.h index 25acb6f..5b95ea9 100644 --- a/src/MINDS-i-Drone.h +++ b/src/MINDS-i-Drone.h @@ -59,7 +59,11 @@ 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" @@ -67,6 +71,8 @@ Copyright 2015 MINDS-i Inc. //#include "input/Sensor.h" #include "input/SPIcontroller.h" #include "input/UM7.h" +#include "input/Bumper.h" + #include "math/Algebra.h" #include "math/Quaternion.h" @@ -74,6 +80,11 @@ Copyright 2015 MINDS-i Inc. #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" diff --git a/src/MINDS-i-Drone_gps.h b/src/MINDS-i-Drone_gps.h new file mode 100644 index 0000000..8f50120 --- /dev/null +++ b/src/MINDS-i-Drone_gps.h @@ -0,0 +1,6 @@ +#ifndef MINDSIDRONELIBS_GPS_H +#define MINDSIDRONELIBS_GPS_H + +#include "comms/NMEA.h" + +#endif \ No newline at end of file diff --git a/src/MINDS-i-Drone_mpu6000.h b/src/MINDS-i-Drone_mpu6000.h new file mode 100644 index 0000000..b39edc2 --- /dev/null +++ b/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 diff --git a/src/comms/CommManager.cpp b/src/comms/CommManager.cpp index 8e0bd99..2b81f9a 100644 --- a/src/comms/CommManager.cpp +++ b/src/comms/CommManager.cpp @@ -1,8 +1,9 @@ #include "CommManager.h" using namespace Protocol; - -CommManager::CommManager(HardwareSerial *inStream, Storage *settings): + + CommManager::CommManager(HardwareSerial *inStream, Storage *settings) + : stream(inStream), bufPos(0), storage(settings), @@ -11,40 +12,140 @@ CommManager::CommManager(HardwareSerial *inStream, Storage *settings): targetIndex(0), waypointsLooped(false), connectCallback(NULL), - eStopCallback(NULL) { + eStopCallback(NULL), + stateStopCallback(NULL), + stateStartCallback(NULL), + bumperDisableCallback(NULL), + bumperEnableCallback(NULL), + versionCallback(NULL), + chksumFailureCount(0), + pktMismatchCount(0), + pktFormatErrorCount(0), + pktDataCount(0), + pktDataLen(0), + pktDecodeState(READ_HEADER1) +{ waypoints = new SRAMlist(MAX_WAYPOINTS); } -void -CommManager::update(){ - while(stream->available()){ + +void CommManager::update() +{ + while(stream->available()) + { uint8_t tmp = stream->read(); buf[bufPos] = tmp; bufPos++; - if(bufPos > BUFF_LEN) bufPos = 0; - else if(rightMatch( buf, bufPos, - HEADER, HEADER_SIZE)){ + if(bufPos > BUFF_LEN) + { + bufPos = 0; + } + else if(rightMatch( buf, bufPos, HEADER, HEADER_SIZE)) + { bufPos = 0; } - else if(rightMatch( buf, bufPos, - FOOTER, FOOTER_SIZE)){ + else if(rightMatch( buf, bufPos, FOOTER, FOOTER_SIZE)) + { processMessage(buf, bufPos-FOOTER_SIZE); } } } -boolean //returns if the rightmost characters of lhs match rhs -CommManager::rightMatch(const uint8_t* lhs, const uint8_t llen, - const uint8_t* rhs, const uint8_t rlen){ - if(rlen > llen) return false; - for(int i=0; i < rlen; i++){ - if( lhs[llen-i-1] != rhs[rlen-i-1] ) return false; + + +// void CommManager::update() +// { + +// while(stream->available()) +// { +// buf[bufPos] = stream->read(); +// switch(pktDecodeState) +// { +// case READ_HEADER1: +// if (buf[bufPos] == HEADER[0]) +// { +// pktDecodeState=READ_HEADER2; +// } +// break; +// case READ_HEADER2: +// if (buf[bufPos] == HEADER[1]) +// { +// pktDecodeState=READ_LABEL; +// } +// else +// { +// pktDecodeState=READ_HEADER1; +// } +// bufPos=0; +// break; +// case READ_LABEL: +// pktDataLen=( buf[bufPos] & 0xf0 ) >> 4; +// if (pktDataLen == 0) +// { +// pktDecodeState=READ_CHKSUM1; +// } +// else +// { +// pktDataCount=0; +// pktDecodeState=READ_DATA; +// } +// bufPos++; +// break; +// case READ_DATA: +// pktDataCount++; +// if (pktDataCount >= pktDataLen) +// pktDecodeState = READ_CHKSUM1; +// bufPos++; +// break; +// case READ_CHKSUM1: +// pktDecodeState = READ_CHKSUM2; +// bufPos++; +// break; +// case READ_CHKSUM2: +// pktDecodeState = READ_FOOTER; +// bufPos++; +// break; +// case READ_FOOTER: +// if (buf[bufPos] == FOOTER[0]) +// { +// processMessage(buf, bufPos-FOOTER_SIZE); +// } +// else +// { +// pktFormatErrorCount++; +// } +// pktDecodeState=READ_HEADER1; +// bufPos=0; +// break; + +// } +// } + +// } + + +//returns if the rightmost characters of lhs match rhs +boolean CommManager::rightMatch(const uint8_t* lhs, const uint8_t llen ,const uint8_t* rhs, const uint8_t rlen) +{ + if(rlen > llen) + return false; + + for(int i=0; i < rlen; i++) + { + if( lhs[llen-i-1] != rhs[rlen-i-1] ) + return false; } return true; } -void -CommManager::processMessage(uint8_t* msg, uint8_t length){ - if(!fletcher(msg, length)) return; + +void CommManager::processMessage(uint8_t* msg, uint8_t length) +{ + if(!fletcher(msg, length)) + { + chksumFailureCount++; + return; + } messageType type = getMessageType(msg[0]); - switch(type){ + switch(type) + { case WAYPOINT: handleWaypoint(msg,length); break; @@ -58,60 +159,135 @@ CommManager::processMessage(uint8_t* msg, uint8_t length){ handleString(msg,length); break; } - if(needsConfirmation(msg[0])){ + if(needsConfirmation(msg[0])) + { sendConfirm(fletcher16(msg, length)); } } -inline void -CommManager::handleWaypoint(uint8_t* msg, uint8_t length){ +//BA added for testing. Should make handleWaypoint call this function to avoid +//duplicate code. Leaving for now +void CommManager::addWaypoint(float lat, float lng, uint8_t index, uint16_t alt ) +{ + if(index > waypoints->size()) + { + requestResync(); + return; + } + //Waypoint data = Waypoint(lat, lng, Units::DEGREES, alt); + Waypoint data = Waypoint(lat, lng); + //todo fix. Now we assume index is ok? + //add would returns error but ignored + waypoints->add(index, data); + if(index <= getTargetIndex()) + advanceTargetIndex(); + if(index == getTargetIndex()) + cachedTarget = getWaypoint(index); + +} + +inline void CommManager::handleWaypoint(uint8_t* msg, uint8_t length) +{ uint8_t subtype = getSubtype(msg[0]); byteConv lat, lon; - for(int i=0; i<4; i++){ + for(int i=0; i<4; i++) + { lat.bytes[3-i] = msg[1+i]; lon.bytes[3-i] = msg[5+i]; } uint16_t alt = (((uint16_t)msg[9])<<8) | msg[10]; - Waypoint data = Waypoint(lat.f, lon.f, Units::DEGREES, alt); + //Waypoint data = Waypoint(lat.f, lon.f, Units::DEGREES, alt); + Waypoint data = Waypoint(lat.f, lon.f); uint8_t index = msg[11]; - switch(subtype){ + switch(subtype) + { case ADD: - if(index > waypoints->size()) { + #ifdef extLogger + Serial2.print("Adding waypoint "); + Serial2.print(index); + Serial2.println(": "); + #endif + if(index > waypoints->size()) + { requestResync(); + #ifdef extLogger + Serial2.println("Failed, Out of range"); + #endif + return; } + + #ifdef extLogger + Serial2.println("Adding"); + #endif + waypoints->add(index, data); - if(index <= getTargetIndex()) advanceTargetIndex(); - if(index == getTargetIndex()) cachedTarget = getWaypoint(index); + if(index <= getTargetIndex()) + advanceTargetIndex(); + if(index == getTargetIndex()) + cachedTarget = getWaypoint(index); break; case ALTER: - if(index >= waypoints->size()) { + Serial2.print("Alter waypoint "); + Serial2.print(index); + Serial2.println(": "); + + if(index >= waypoints->size()) + { requestResync(); + #ifdef extLogger + Serial2.println("Failed, Out of range"); + #endif return; } + #ifdef extLogger + Serial2.println("Altering"); + #endif + waypoints->set(index, data); - if(index == getTargetIndex()) cachedTarget = getWaypoint(index); + if(index == getTargetIndex()) + cachedTarget = getWaypoint(index); + break; + default: + //todo packet subtype error break; } } -inline void -CommManager::handleData(uint8_t* msg, uint8_t length){ + +inline void CommManager::handleData(uint8_t* msg, uint8_t length) +{ uint8_t subtype = getSubtype(msg[0]); uint8_t index = msg[1]; byteConv conv; - for(int i=0; i<4; i++) conv.bytes[3-i] = msg[2+i]; - switch(subtype){ + for(int i=0; i<4; i++) + { + conv.bytes[3-i] = msg[2+i]; + } + + switch(subtype) + { case TELEMETRY: //arduino doesn't keep track of received telemetry break; case SETTING: setSetting(index, conv.f); break; + case INFO: + switch (msg[1]) + { + case (APM_VERSION): + if (versionCallback != NULL) + versionCallback(); + break; + } + break; } + } -inline void -CommManager::handleWord(uint8_t* msg, uint8_t length){ + +inline void CommManager::handleWord(uint8_t* msg, uint8_t length) +{ uint8_t subtype = getSubtype(msg[0]); uint8_t a = msg[1]; uint8_t b = msg[2]; @@ -129,8 +305,9 @@ CommManager::handleWord(uint8_t* msg, uint8_t length){ break; } } -inline void -CommManager::handleCommands(uint8_t a, uint8_t b){ + +inline void CommManager::handleCommands(uint8_t a, uint8_t b) +{ switch(a){ case ESTOP: if(eStopCallback != NULL) eStopCallback(); @@ -140,108 +317,216 @@ CommManager::handleCommands(uint8_t a, uint8_t b){ break; case LOOPING: waypointsLooped = (b!=0); + #ifdef extLogger + Serial2.print("Looping: "); + Serial2.println(b!=0); + #endif break; case CLEAR_WAYPOINTS: sendString("Cleared Waypoints"); + #ifdef extLogger + Serial2.print("Cleared Waypoints"); + #endif waypoints->clear(); break; case DELETE_WAYPOINT: - if(b < 0 || b >= waypoints->size()){ + #ifdef extLogger + Serial2.print("Delete Waypoint "); + Serial2.print(b); + Serial2.print(": "); + #endif + + if(b < 0 || b >= waypoints->size()) + { requestResync(); + #ifdef extLogger + Serial2.println("failed, Index out of bounds"); + #endif + return; } - waypoints->remove(b); - if(b <= getTargetIndex()) retardTargetIndex(); - if(waypoints->size()==0) cachedTarget = Waypoint(); + #ifdef extLogger + Serial2.println("Removed"); + #endif + + waypoints->remove(b); + if(b <= getTargetIndex()) + retardTargetIndex(); + if(waypoints->size()==0) + cachedTarget = Waypoint(); + break; + case STATE_STOP: + if (stateStopCallback != NULL) + { + sendString("Stopping"); + stateStopCallback(); + } + break; + case STATE_START: + if (stateStartCallback != NULL) + { + sendString("Starting"); + stateStartCallback(); + } + break; + case BUMPER_DISABLE: + if (bumperDisableCallback != NULL) + { + sendString("Bumper Disabled"); + bumperDisableCallback(); + } + break; + case BUMPER_ENABLE: + if (bumperEnableCallback != NULL) + { + sendString("Bumper Enabled"); + bumperEnableCallback(); + } break; } } -void -CommManager::handleString(uint8_t* msg, uint8_t length){ + +void CommManager::handleString(uint8_t* msg, uint8_t length) +{ /*dead end for strings*/ } -Waypoint -CommManager::getWaypoint(uint16_t index){ + +Waypoint CommManager::getWaypoint(uint16_t index) +{ if(index >= waypoints->size()) return Waypoint(); return waypoints->get(index); } -void -CommManager::clearWaypointList(){ + +void CommManager::clearWaypointList() +{ waypoints->clear(); } -void -CommManager::sendConfirm(uint16_t digest){ + +void CommManager::sendConfirm(uint16_t digest) +{ byte datum[3]; datum[0] = buildMessageLabel(wordSubtype(CONFIRMATION)); datum[1] = (digest>>8 ); datum[2] = (digest&0xff); sendMessage(datum, 3, stream); } -uint16_t -CommManager::numWaypoints(){ + +uint16_t CommManager::numWaypoints() +{ return waypoints->size(); } -bool -CommManager::loopWaypoints(){ + +bool CommManager::loopWaypoints() +{ return waypointsLooped; } -void -CommManager::setTargetIndex(uint16_t index){ + +void CommManager::setTargetIndex(uint16_t index) +{ if(index >= waypoints->size()) return; targetIndex = index; cachedTarget = getWaypoint(index); sendCommand(commandType(TARGET), targetIndex); + + #ifdef extLogger + char buff[32]; + gps_coord_to_str(&cachedTarget.m_gpsCoord,buff,32,6,"DD"); + Serial2.print("Target lonlat: "); + Serial2.println(buff); + //Serial2.print("target lat:"); + //Serial2.println(cachedTarget.degLatitude(),6); + //Serial2.print("target long:"); + //Serial2.println(cachedTarget.degLongitude(),6); + #endif + } -void -CommManager::advanceTargetIndex(){ + +void CommManager::advanceTargetIndex() +{ setTargetIndex(getTargetIndex()+1); } -void -CommManager::retardTargetIndex(){ + +void CommManager::retardTargetIndex() +{ setTargetIndex(getTargetIndex()-1); } -uint16_t -CommManager::getTargetIndex(){ + +uint16_t CommManager::getTargetIndex() +{ return targetIndex; } -Waypoint -CommManager::getTargetWaypoint(){ + +Waypoint CommManager::getTargetWaypoint() +{ return cachedTarget; } -void -CommManager::setSetting(uint8_t id, float input){ + +void CommManager::setSetting(uint8_t id, float input) +{ storage->updateRecord(id, input); sendSetting(id, input); } -void -CommManager::inputSetting(uint8_t id, float input){ + +void CommManager::inputSetting(uint8_t id, float input) +{ storage->updateRecord(id, input); } -float -CommManager::getSetting(uint8_t id){ + +float CommManager::getSetting(uint8_t id) +{ return storage->getRecord(id); } -void -CommManager::requestResync(){ + +void CommManager::requestResync() +{ sendSyncMessage(Protocol::SYNC_REQUEST); } -void -CommManager::setConnectCallback(void (*call)(void)){ + +void CommManager::setConnectCallback(void (*call)(void)) +{ connectCallback = call; } -void -CommManager::setEStopCallback(void (*call)(void)){ + +void CommManager::setEStopCallback(void (*call)(void)) +{ eStopCallback = call; } -void -CommManager::onConnect(){ + +void CommManager::setStateStopCallback(void (*call)(void)) +{ + stateStopCallback = call; +} + +void CommManager::setStateStartCallback(void (*call)(void)) +{ + stateStartCallback = call; +} + +void CommManager::setBumperDisableCallback(void (*call)(void)) +{ + bumperDisableCallback = call; +} + +void CommManager::setBumperEnableCallback(void (*call)(void)) +{ + bumperEnableCallback = call; +} + +void CommManager::setVersionCallback(void (*call)(void)) +{ + versionCallback = call; +} + +void CommManager::onConnect() +{ for(int i=0; i *settings); bool loopWaypoints(); @@ -48,8 +77,18 @@ class CommManager{ uint16_t getTargetIndex(); uint16_t numWaypoints(); void sendTelem(uint8_t id , float value); + void sendState(uint8_t stateTypeId, uint8_t stateID); + void sendSensor(uint8_t sensorTypeId, uint8_t sensorNum, uint32_t value); + void sendVersion(uint8_t version_major, uint8_t version_minor, uint8_t version_rev); + void setConnectCallback(void (*call)(void)); void setEStopCallback(void (*call)(void)); + void setStateStopCallback(void (*call)(void)); + void setStateStartCallback(void (*call)(void)); + void setBumperDisableCallback(void (*call)(void)); + void setBumperEnableCallback(void (*call)(void)); + void setVersionCallback(void (*call)(void)); + void clearWaypointList(); void requestResync(); void update(); @@ -62,9 +101,12 @@ class CommManager{ void sendError(char const * msg); Waypoint getTargetWaypoint(); Waypoint getWaypoint(uint16_t index); + + void addWaypoint(float lat, float lng, uint8_t index, uint16_t alt ); + private: CommManager(const CommManager& copy); //intentionally not implemented - boolean recieveWaypoint(waypointSubtype type, uint8_t index, Waypoint point); + //No implementation? //boolean recieveWaypoint(waypointSubtype type, uint8_t index, Waypoint point); boolean rightMatch(const uint8_t* lhs, const uint8_t llen, const uint8_t* rhs, const uint8_t rlen); void onConnect(); @@ -73,6 +115,7 @@ class CommManager{ void inputSetting(uint8_t id, float input); void processMessage(uint8_t* msg, uint8_t length); void sendConfirm(uint16_t digest); + void sendSetting(uint8_t id, float value); void sendTargetIndex(); void handleCommands(uint8_t a , uint8_t b); diff --git a/src/comms/NMEA.cpp b/src/comms/NMEA.cpp index 6e1fd1e..ca72efa 100644 --- a/src/comms/NMEA.cpp +++ b/src/comms/NMEA.cpp @@ -13,28 +13,69 @@ namespace{ } } + +enum NMEA_MSG_TYPE_ENUM +{ + NMEA_MSG_TYPE_UNKNOWN = 0, + NMEA_MSG_TYPE_GPRMC, + NMEA_MSG_TYPE_GPGNS, +}; + void NMEA::update(){ - while(inStream.available()){ + while(inStream.available()) + { char n = inStream.read(); // NMEA strings always begin with a '$', followed by comma sep. values // Attempt to parse any NMEA string, parsing each value as it comes // until the end of the string is reached or a value fails to parse - if(n == '$') { + //Serial.print(n); + + if(n == '$') + { + //Serial.println(n); dataFrameIndex++; seqPos = 0; + msgLen = 0; clearBuffer(); - } else if (seqPos != -1) { - if((n != ',') && (n != '*')){ + } + else if (seqPos != -1) + { + if((n != ',') && (n != '*') && (n != '\r')) + { + msg[msgLen] = n; //capture full msg for later checksum + msgLen++; bool success = pushToBuffer(n); if(!success) seqPos = -1; //buffer full - } else { + } + else + { + msg[msgLen] = n; //capture full msg for later checksum + msgLen++; + + bool parseSuccess=false; + // two consective commas is not an error, just skip it - bool parseSuccess = (sectionBufPos==0)? true : handleSections(); - seqPos = (parseSuccess)? seqPos+1 : -1; //reset parser on fail + if (sectionBufPos==0) + { + parseSuccess=true; + } + else + { + parseSuccess = handleSections(); + } + + if (!parseSuccess) + seqPos=-1; + else + seqPos+=1; + + clearBuffer(); - if(seqPos >= NumSections) { + + if(seqPos >= numSections) + { // dataFrameIndex++; could be here to only advance the // index when a full packed is completed, but the // position hold algorithm was tested with it marking @@ -42,9 +83,26 @@ void NMEA::update(){ // it is until enough testing of position hold can be done // to validate its placement here seqPos = -1; //done reading + updatedRMC = true; + nmeaMsgType = NMEA_MSG_TYPE_UNKNOWN; } } } + + } +} + +bool NMEA::verifyChecksum(char msg[],uint8_t msgLen,uint8_t msgCheckSum){ + uint8_t checksum = 0x00; + for(uint8_t i=0;i bool { + //Serial.println(nmea.sectionBuf); return strcmp("GPRMC", nmea.sectionBuf) == 0 || strcmp("GNRMC", nmea.sectionBuf) == 0; }, //TimeOfFix - [](NMEA& nmea) -> bool { return nmea.readFloat(nmea.timeOfFix); }, + [](NMEA& nmea) -> bool { return nmea.readFloat(nmea.rmc_msg.timeOfFix); }, //Status [](NMEA& nmea) -> bool { if(nmea.sectionBufPos != 1) return false; @@ -84,45 +224,152 @@ const SectionHandler NMEA::sectionHandlers[] { return true; }, //Latitude - [](NMEA& nmea) -> bool { return nmea.readFloat(nmea.tmpLatLon); }, + [](NMEA& nmea) -> bool { return nmea.readFloat(nmea.tmpLatLon) && !get_latitude(nmea.sectionBuf, &nmea.curGPSCoord.latitude); }, //Latitude Hemisphere [](NMEA& nmea) -> bool { - if(nmea.sectionBufPos != 1) return false; + if(nmea.sectionBufPos != 1) + { + return false; + } else if(nmea.sectionBuf[0] == 'N') - nmea.latitude = toDecDegrees(nmea.tmpLatLon); + { + nmea.rmc_msg.latitude = toDecDegrees(nmea.tmpLatLon); + gps_angle_apply_sign(&nmea.curGPSCoord.latitude,'N'); + } else if(nmea.sectionBuf[0] == 'S') - nmea.latitude = -toDecDegrees(nmea.tmpLatLon); - else return false; + { + nmea.rmc_msg.latitude = -toDecDegrees(nmea.tmpLatLon); + gps_angle_apply_sign(&nmea.curGPSCoord.latitude,'S'); + } + else + { + return false; + } + return true; }, //Longitude - [](NMEA& nmea) -> bool { return nmea.readFloat(nmea.tmpLatLon); }, + [](NMEA& nmea) -> bool { return nmea.readFloat(nmea.tmpLatLon) && !get_longitude(nmea.sectionBuf, &nmea.curGPSCoord.longitude); }, //Longitude Hemisphere [](NMEA& nmea) -> bool { - if(nmea.sectionBufPos != 1) return false; + if(nmea.sectionBufPos != 1) + { + return false; + } else if(nmea.sectionBuf[0] == 'E') - nmea.longitude = toDecDegrees(nmea.tmpLatLon); + { + nmea.rmc_msg.longitude = toDecDegrees(nmea.tmpLatLon); + gps_angle_apply_sign(&nmea.curGPSCoord.longitude,'E'); + } else if(nmea.sectionBuf[0] == 'W') - nmea.longitude = -toDecDegrees(nmea.tmpLatLon); - else return false; + { + nmea.rmc_msg.longitude = -toDecDegrees(nmea.tmpLatLon); + gps_angle_apply_sign(&nmea.curGPSCoord.longitude,'W'); + } + else + { + return false; + } + return true; }, //Ground Speed [](NMEA& nmea) -> bool { - bool success = nmea.readFloat(nmea.groundSpeed); - if(success) nmea.groundSpeed = toMilesPerHours(nmea.groundSpeed); + bool success = nmea.readFloat(nmea.rmc_msg.groundSpeed); + if(success) nmea.rmc_msg.groundSpeed = toMilesPerHours(nmea.rmc_msg.groundSpeed); return success; }, //Track Angle - [](NMEA& nmea) -> bool { return nmea.readFloat(nmea.course); }, + [](NMEA& nmea) -> bool { return nmea.readFloat(nmea.rmc_msg.course); }, //Date - [](NMEA& nmea) -> bool { return nmea.readFloat(nmea.dateOfFix); }, + [](NMEA& nmea) -> bool { return nmea.readFloat(nmea.rmc_msg.dateOfFix); }, //Magnetic Variation - [](NMEA& nmea) -> bool { return nmea.readFloat(nmea.magVar); }, + [](NMEA& nmea) -> bool { return nmea.readFloat(nmea.rmc_msg.magVar); }, //magnetic Variation direction [](NMEA& nmea) -> bool { - if(nmea.sectionBuf[0] == 'E') nmea.magVar *= -1; + if(nmea.sectionBuf[0] == 'E') nmea.rmc_msg.magVar *= -1; return true; }, + //Mode Indicator + [](NMEA& nmea) -> bool { + return true; + }, + //Checksum + [](NMEA& nmea) -> bool { + uint8_t msgCheckSum; + if(nmea.sectionBufPos == 2) { + sscanf(&nmea.sectionBuf[0], "%2hhx",&msgCheckSum); + // verify checksum is correct before moving data externally accessible variables + if (nmea.verifyChecksum(nmea.msg,nmea.msgLen-4,msgCheckSum)) { + nmea.latitude = nmea.rmc_msg.latitude; + nmea.longitude = nmea.rmc_msg.longitude; + nmea.timeOfFix = nmea.rmc_msg.timeOfFix; + nmea.dateOfFix = nmea.rmc_msg.dateOfFix; + nmea.warning = nmea.rmc_msg.warning; + nmea.groundSpeed = nmea.rmc_msg.groundSpeed; + nmea.course = nmea.rmc_msg.course; + nmea.magVar = nmea.rmc_msg.magVar; + return true; + } + else { + return false; + } + } + } +}; + + +const sectionHandler NMEA::sectionHandlersGPGNS[] { + //GPRMC + [](NMEA& nmea) -> bool { + return strcmp("GPGNS", nmea.sectionBuf) == 0 + || strcmp("GNGNS", nmea.sectionBuf) == 0; + }, + //utc time + [](NMEA& nmea) -> bool { return 1; }, + //lat + [](NMEA& nmea) -> bool { return 1; }, + //lat n/s + [](NMEA& nmea) -> bool { return 1; }, + //long + [](NMEA& nmea) -> bool { return 1; }, + //long e/w + [](NMEA& nmea) -> bool { return 1; }, + //posMode (status for gps,glonass,galileo,Beidou) + [](NMEA& nmea) -> bool { return 1; }, + //NumSV + [](NMEA& nmea) -> bool { + bool success = nmea.readUInt(nmea.gns_msg.numSV); + return success; + }, + //HDOP + [](NMEA& nmea) -> bool { return nmea.readFloat(nmea.gns_msg.hdop); }, + //alt + [](NMEA& nmea) -> bool { return 1; }, + //sep + [](NMEA& nmea) -> bool { return 1; }, + //diffAge + [](NMEA& nmea) -> bool { return 1; }, + //difstation + [](NMEA& nmea) -> bool { return 1; }, + //checksum + [](NMEA& nmea) -> bool { + uint8_t msgCheckSum; + if(nmea.sectionBufPos == 2) { + sscanf(&nmea.sectionBuf[0], "%2hhx",&msgCheckSum); + // verify checksum is correct before moving data externally accessible variables + if (nmea.verifyChecksum(nmea.msg,nmea.msgLen-4,msgCheckSum)) { + nmea.numSV = nmea.gns_msg.numSV; + nmea.hdop = nmea.gns_msg.hdop; + return true; + } + else { + return false; + } + } + } + }; -const int NMEA::NumSections =sizeof(sectionHandlers)/sizeof(sectionHandlers[0]); + +const int NMEA::numSectionsGPRMC =sizeof(sectionHandlersGPRMC)/sizeof(sectionHandlersGPRMC[0]); +const int NMEA::numSectionsGPGNS =sizeof(sectionHandlersGPGNS)/sizeof(sectionHandlersGPGNS[0]); diff --git a/src/comms/NMEA.h b/src/comms/NMEA.h index f93f7ef..704e54a 100644 --- a/src/comms/NMEA.h +++ b/src/comms/NMEA.h @@ -3,6 +3,7 @@ #include "Arduino.h" #include "math/SpatialMath.h" #include "math/Waypoint.h" +#include "math/floatgps.h" class NMEA; /** @@ -12,60 +13,86 @@ class NMEA; * on success * return weather or not the parse succeeded */ -typedef bool (*SectionHandler)(NMEA&); +typedef bool (*sectionHandler)(NMEA&); + +struct rmc_data{ + float latitude; + float longitude; + float timeOfFix; + float dateOfFix; + bool warning; + float groundSpeed; + float course; + float magVar; +}; + +struct gns_data{ + unsigned int numSV; + float hdop; +}; class NMEA{ public: - explicit NMEA(Stream& stream): inStream(stream) { stream.setTimeout(0); } + explicit NMEA(Stream& stream): inStream(stream) { stream.setTimeout(0); memset(&curGPSCoord,0,sizeof(GPS_COORD)); } /** Read more data from the input stream and parse whats available */ void update(); + + /** Verify that the checksum is correct */ + bool verifyChecksum(char msg[],uint8_t msgLen,uint8_t msgCheckSum); + /** Start reading from a different input stream */ void newStream(Stream& stream){ inStream = stream; stream.setTimeout(0); } /** true if data has been updated since the last time anything was read */ - uint16_t dataIndex(){ - return dataFrameIndex; - } + uint16_t dataIndex(){ return dataFrameIndex; } + + // + //NOTE: this are function that will return less percision + // /** Latitude in decimal degrees, north is positive */ - float getLatitude(){ - return latitude; - } + float getLatitude(){ return gps_angle_to_float(&curGPSCoord.latitude); } /** Longitude in decimal degrees, east is positive */ - float getLongitude(){ - return longitude; - } + float getLongitude(){ return gps_angle_to_float(&curGPSCoord.longitude); } /** Time of GPS fix in HHMMSS format */ - float getTimeOfFix(){ - return timeOfFix; - } + + /** Lat/long in GPS_COORD */ + GPS_COORD getGPS_COORD() { return curGPSCoord; } + + float getTimeOfFix(){ return timeOfFix; } /** Date of fix in DDMMYY format */ - float getDateOfFix(){ - return dateOfFix; - } + float getDateOfFix(){ return dateOfFix; } /** True if the location data may be missing or incorrect */ - bool getWarning(){ - return warning; - } + bool getWarning(){ return warning; } /** Get ground speed in miles per hours */ - float getGroundSpeed(){ - return groundSpeed; - } + float getGroundSpeed(){ return groundSpeed; } /** Get course in degrees true (relative true north, clockwise positive) */ - float getCourse(){ - return course; - } + float getCourse(){ return course; } /** Angle between magnetic north and true north */ - float getMagVar(){ - return magVar; - } + float getMagVar(){ return magVar; } + /** Latitude/Longitude location as a Waypoint, CCW positive */ - Waypoint getLocation(){ - return Waypoint(latitude,longitude); + Waypoint getLocation(){ return Waypoint(curGPSCoord); } + + uint16_t getNumSat(){ return numSV; } + float getHDOP(){ return hdop; } + + + bool getUpdatedRMC() + { + return updatedRMC; + } + + void clearUpdatedRMC() + { + updatedRMC=false; } private: Stream& inStream; + + GPS_COORD curGPSCoord; + float latitude = 0.0; float longitude = 0.0; float timeOfFix = 0, dateOfFix = 0; @@ -73,10 +100,21 @@ class NMEA{ float groundSpeed = 0; float course = 0; float magVar = 0; + unsigned int numSV=0; + float hdop = 0.0; uint16_t dataFrameIndex = 0; + + rmc_data rmc_msg; + gns_data gns_msg; + char msg[256]; + uint8_t msgLen; + + bool updatedRMC = false; + //holds a section between commas in a GPRMC string char sectionBuf[16]; int sectionBufPos = 0; + int nmeaMsgType = -1; const int sizeSectionBuf = sizeof(sectionBuf)/sizeof(sectionBuf[0]); void clearBuffer(){ //here for inlining sectionBufPos = 0; @@ -93,13 +131,19 @@ class NMEA{ * on failure, return false and leave `store` alone */ bool readFloat(float& store); + //bool readGPSCoordFloat(GPS_COORD coord, uint8_t type ); + //same as readFloat but with unsigned int type + bool readUInt(unsigned int& store); //holds the parsers sequence position in the $GPRMC string, -1 otherwise int seqPos = -1; //holds a temporary latitude or longitude value that has not been fully read float tmpLatLon; //an array of section handlers, coresponding to sections of a GPRMC string - static const SectionHandler sectionHandlers[]; - static const int NumSections; + static const sectionHandler sectionHandlersGPRMC[]; + static const sectionHandler sectionHandlersGPGNS[]; + static const int numSectionsGPRMC; + static const int numSectionsGPGNS; + int numSections=0; bool handleSections(); }; diff --git a/src/comms/Protocol.cpp b/src/comms/Protocol.cpp index e88ca75..db3b240 100644 --- a/src/comms/Protocol.cpp +++ b/src/comms/Protocol.cpp @@ -2,13 +2,15 @@ #include #include "Arduino.h" -namespace Protocol{ - uint16_t - fletcher16(uint8_t const *data, int length){ +namespace Protocol +{ + uint16_t fletcher16(uint8_t const *data, int length) + { return fletcher16_resume(data, length, 0xFFFF); } - uint16_t - fletcher16_resume(uint8_t const *data, int length, uint16_t lastResult){ + + uint16_t fletcher16_resume(uint8_t const *data, int length, uint16_t lastResult) + { uint16_t Asum, Bsum; Asum = lastResult & 0xff; Bsum = (lastResult>>8) & 0xff; @@ -26,14 +28,16 @@ namespace Protocol{ Bsum = (Bsum & 0xff) + (Bsum >> 8); return (Bsum << 8) | Asum; } - bool - fletcher(uint8_t* data, int length){ + + bool fletcher(uint8_t* data, int length) + { uint16_t foundSum = data[length-2]<<8 | data[length-1]; uint16_t calcSum = fletcher16(data, length-2); return foundSum==calcSum; } - void - sendMessage(uint8_t* data, int length, HardwareSerial *stream){ + + void sendMessage(uint8_t* data, int length, HardwareSerial *stream) + { uint16_t sum = Protocol::fletcher16(data, length); stream->write(HEADER, HEADER_SIZE); stream->write(data,length); @@ -41,8 +45,9 @@ namespace Protocol{ stream->write(sum&0xff); stream->write(FOOTER, FOOTER_SIZE); } - void - sendStringMessage(uint8_t label, const char * msg, int len, HardwareSerial* stream){ + + void sendStringMessage(uint8_t label, const char * msg, int len, HardwareSerial* stream) + { uint16_t labelSum = Protocol::fletcher16(&label, 1); uint16_t sum = Protocol::fletcher16_resume((uint8_t const*)msg, len, labelSum); stream->write(HEADER, HEADER_SIZE); @@ -52,8 +57,9 @@ namespace Protocol{ stream->write(sum&0xff); stream->write(FOOTER, FOOTER_SIZE); } - bool - needsConfirmation(uint8_t label){ + + bool needsConfirmation(uint8_t label) + { uint8_t type = getMessageType(label); uint8_t subtype = getSubtype(label); @@ -62,25 +68,34 @@ namespace Protocol{ return false; } - messageType getMessageType(uint8_t label){ + + messageType getMessageType(uint8_t label) + { return (messageType) (label & 0x0F); } - uint8_t getSubtype(uint8_t label){ + + uint8_t getSubtype(uint8_t label) + { return (label>>4) & 0x0F; } - uint8_t buildMessageLabel(waypointSubtype subtype){ + + uint8_t buildMessageLabel(waypointSubtype subtype) + { if(subtype > 0x0F) return 0; return (subtype<<4) | messageType(WAYPOINT); } - uint8_t buildMessageLabel(dataSubtype subtype){ + uint8_t buildMessageLabel(dataSubtype subtype) + { if(subtype > 0x0F) return 0; return (subtype<<4) | messageType(DATA); } - uint8_t buildMessageLabel(wordSubtype subtype){ + uint8_t buildMessageLabel(wordSubtype subtype) + { if(subtype > 0x0F) return 0; return (subtype<<4) | messageType(WORD); } - uint8_t buildMessageLabel(stringSubtype subtype){ + uint8_t buildMessageLabel(stringSubtype subtype) + { if(subtype > 0x0F) return 0; return (subtype<<4) | messageType(STRING); } diff --git a/src/comms/Protocol.h b/src/comms/Protocol.h index 052f9ee..b7716b6 100644 --- a/src/comms/Protocol.h +++ b/src/comms/Protocol.h @@ -33,14 +33,17 @@ namespace Protocol{ ALTER = 1, }; enum dataSubtype{ TELEMETRY = 0, - SETTING = 1 }; + SETTING = 1, + SENSORS = 2, + INFO = 3 }; enum wordSubtype{ CONFIRMATION = 0, SYNC = 1, - COMMAND = 2 }; + COMMAND = 2, + STATE = 3 }; enum stringSubtype{ ERROR = 0, - STATE = 1 }; + STR_STATE = 1 }; enum telemetryType{ LATITUDE = 0, LONGITUDE = 1, @@ -58,13 +61,37 @@ namespace Protocol{ RDGEAR = 13, HOMELATITUDE = 14, HOMELONGITUDE = 15, - HOMEALTITUDE = 16 }; + HOMEALTITUDE = 16, + DELTAALTITUDE = 17, + GPSNUMSAT = 18, + GPSHDOP = 19, + HEADING_LOCK = 20 }; + enum sensorType + { + OBJDETECT_SONIC = 0, + OBJDETECT_BUMPER = 1, + }; + + enum infoType { APM_VERSION = 0, + }; enum commandType{ ESTOP = 0, TARGET = 1, LOOPING = 2, CLEAR_WAYPOINTS = 3, - DELETE_WAYPOINT = 4 }; + DELETE_WAYPOINT = 4, + STATE_STOP = 5, + STATE_START = 6, + BUMPER_DISABLE = 7, + BUMPER_ENABLE = 8 }; + + enum stateType { APM_STATE = 0, + DRIVE_STATE = 1, + AUTO_STATE = 2, + AUTO_FLAGS = 3, + GPS_STATE = 4 + + }; const uint8_t MAX_WAYPOINTS = 64; const uint8_t MAX_SETTINGS = NUM_STORED_RECORDS;//taken from eepromconfig @@ -99,6 +126,7 @@ namespace Protocol{ uint8_t buildMessageLabel(dataSubtype type); uint8_t buildMessageLabel(wordSubtype type); uint8_t buildMessageLabel(stringSubtype type); + } #endif diff --git a/src/input/APM/LEA6H.h b/src/input/APM/LEA6H.h index 5a8ea2c..dd2779f 100644 --- a/src/input/APM/LEA6H.h +++ b/src/input/APM/LEA6H.h @@ -6,15 +6,26 @@ #include "comms/NMEA.h" #include "input/Sensor.h" #include "input/GPS.h" +#include "math/floatgps.h" class LEA6H : public Sensor, public GPS { protected: //GPS_SETUP messages - const static uint8_t GPS_SETUP[]; + const static uint8_t GPS_RESET[]; + //const static uint8_t GPS_SETUP[]; const static uint8_t Pedestrian_Mode[]; const static uint8_t GPRMC_On[]; + const static uint8_t GPGNS_On[]; + const static uint8_t GPGGA_Off[]; + const static uint8_t GPGSA_Off[]; + const static uint8_t GPGSV_Off[]; + const static uint8_t GPVTG_Off[]; + const static uint8_t GNGLL_Off[]; + + const static uint8_t CFG_NMEA[]; const static uint8_t CFG_PRT[]; + const static uint8_t CFG_RATE[]; void sendUBloxMessage(uint8_t Type, uint8_t ID, uint16_t len, const uint8_t* buf); void calcChecksum(const uint8_t* msg, uint8_t len, @@ -32,17 +43,25 @@ class LEA6H : public Sensor, public GPS { void calibrate(); void update(); //expose interface provided by NMEA parser - bool getWarning() { update(); return parser.getWarning(); } - uint16_t dataIndex() { update(); return parser.dataIndex(); } - float getCourse() { update(); return parser.getCourse(); } - float getDateOfFix() { update(); return parser.getDateOfFix(); } - float getGroundSpeed(){ update(); return parser.getGroundSpeed(); } - float getLatitude() { update(); return parser.getLatitude(); } - float getLongitude() { update(); return parser.getLongitude(); } - float getMagVar() { update(); return parser.getMagVar(); } - float getTimeOfFix() { update(); return parser.getTimeOfFix(); } + bool getWarning() { update(); return parser.getWarning(); } + bool getUpdatedRMC() { update(); return parser.getUpdatedRMC(); } + void clearUpdatedRMC() { parser.clearUpdatedRMC(); } + uint16_t dataIndex() { update(); return parser.dataIndex(); } + float getCourse() { update(); return parser.getCourse(); } + float getDateOfFix() { update(); return parser.getDateOfFix(); } + float getGroundSpeed() { update(); return parser.getGroundSpeed(); } + + GPS_COORD getGPS_COORD() { update(); return parser.getGPS_COORD(); } + + float getLatitude() { update(); return parser.getLatitude(); } + float getLongitude() { update(); return parser.getLongitude(); } + + float getMagVar() { update(); return parser.getMagVar(); } + float getTimeOfFix() { update(); return parser.getTimeOfFix(); } + uint16_t getNumSat() { update(); return parser.getNumSat(); } + float getHDOP() { update(); return parser.getHDOP(); } #ifdef WAYPOINT_H - Waypoint getLocation(){ update(); return parser.getLocation(); } + Waypoint getLocation() { update(); return parser.getLocation(); } #endif }; Sensor::Status @@ -50,14 +69,39 @@ LEA6H::status(){ return Sensor::OK; } void -LEA6H::begin(){ +LEA6H::begin() +{ + //reset gps. Both baud rates starting with fastest + stream.begin(38400); + sendUBloxMessage(0x06,0x04, 0x0004,GPS_RESET); + delay(1000); stream.begin(9600); + sendUBloxMessage(0x06,0x04, 0x0004,GPS_RESET); + delay(1000); + //stream.begin(9600); sendUBloxMessage(0x06, 0x00, 0x0014, CFG_PRT); + + //Looks like delay needed when switching baud rates + delay(100); stream.flush(); stream.begin(38400); + + //Turn on RMC and GNS sendUBloxMessage(0x06, 0x01, 0x0003, GPRMC_On); + sendUBloxMessage(0x06, 0x01, 0x0003, GPGNS_On); + + //Turn off all others + sendUBloxMessage(0x06, 0x01, 0x0003, GPGGA_Off); + sendUBloxMessage(0x06, 0x01, 0x0003, GPGSA_Off); + sendUBloxMessage(0x06, 0x01, 0x0003, GPGSV_Off); + sendUBloxMessage(0x06, 0x01, 0x0003, GPVTG_Off); + sendUBloxMessage(0x06, 0x01, 0x0003, GNGLL_Off); + + sendUBloxMessage(0x06, 0x17, 0x0004, CFG_NMEA); sendUBloxMessage(0x06, 0x24, 0x0024, Pedestrian_Mode); + sendUBloxMessage(0x06, 0x08, 0x0006, CFG_RATE); + } void LEA6H::calibrate(){ @@ -93,11 +137,15 @@ LEA6H::calcChecksum(const uint8_t *msg, uint8_t len, uint8_t &c_a, uint8_t &c_b) } } -const uint8_t LEA6H::GPS_SETUP[] = { - 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x01, 0xFF, 0x18, - 0xB5, 0x62, 0x06, 0x17, 0x04, 0x00, 0x00, 0x23, 0x00, 0x00, 0x44, 0x52, - 0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xc0, 0x08, - 0x00, 0x00, 0x00, 0x96, 0x00, 0x00, 0x07, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x82, 0x8A}; +const uint8_t LEA6H::GPS_RESET[] = { + 0x00, 0x00, 0x01, 0x00 +}; + +// const uint8_t LEA6H::GPS_SETUP[] = { +// 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x01, 0xFF, 0x18, +// 0xB5, 0x62, 0x06, 0x17, 0x04, 0x00, 0x00, 0x23, 0x00, 0x00, 0x44, 0x52, +// 0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xc0, 0x08, +// 0x00, 0x00, 0x00, 0x96, 0x00, 0x00, 0x07, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x82, 0x8A}; const uint8_t LEA6H::Pedestrian_Mode[] = {0xFF, 0xFF, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, @@ -105,9 +153,18 @@ const uint8_t LEA6H::Pedestrian_Mode[] = {0xFF, 0xFF, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; const uint8_t LEA6H::GPRMC_On[] = { 0xF0, 0x04, 0x01 }; +const uint8_t LEA6H::GPGNS_On[] = { 0xF0, 0x0D, 0x01 }; +const uint8_t LEA6H::GPGGA_Off[] = { 0xF0, 0x00, 0x00 }; +const uint8_t LEA6H::GPGSA_Off[] = { 0xF0, 0x02, 0x00 }; +const uint8_t LEA6H::GPGSV_Off[] = { 0xF0, 0x03, 0x00 }; +const uint8_t LEA6H::GPVTG_Off[] = { 0xF0, 0x05, 0x00 }; +const uint8_t LEA6H::GNGLL_Off[] = { 0xF0, 0x01, 0x00 }; + + const uint8_t LEA6H::CFG_NMEA[] = { 0x00, 0x23, 0x00, 0x00 }; const uint8_t LEA6H::CFG_PRT[] = { 0x01, 0x00, 0x00, 0x00, 0xc0, 0x08, 0x00, 0x00, 0x00, 0x96, 0x00, 0x00, 0x07, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00}; +const uint8_t LEA6H::CFG_RATE[] = { 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00 }; #endif diff --git a/src/input/APM/LEA6H_sim.h b/src/input/APM/LEA6H_sim.h new file mode 100644 index 0000000..51f898e --- /dev/null +++ b/src/input/APM/LEA6H_sim.h @@ -0,0 +1,124 @@ +#ifndef LEA6H_SIM_H +#define LEA6H_SIM_H + +#include "Arduino.h" +#include + +#include "input/Sensor.h" +#include "input/GPS.h" + +#include "math/floatgps.h" + +class LEA6H_sim : public Sensor, public GPS { +protected: + + void calcChecksum(const uint8_t* msg, uint8_t len, + uint8_t &c_a, uint8_t &c_b); + + bool m_warning; + bool m_updatedRMC; + float m_course; + float m_dateOfFix; + float m_groundSpeed; + double m_latitude; + double m_longitude; + float m_magVar; + float m_timeOfFix; + + Waypoint m_location; + + uint16_t m_dataIndex; + + //speed? + //target location? + //amount of noise in gps location? + + +public: + LEA6H_sim(); + void begin(); + void end() {} + Sensor::Status status(); + void calibrate(); + void update(); + + bool getWarning() { return m_warning; } + bool getUpdatedRMC() { return m_updatedRMC; } + void clearUpdatedRMC() { m_updatedRMC=false; } + void setUpdatedRMC() { m_updatedRMC=true; } + uint16_t dataIndex() { return m_dataIndex; } + + //course isn't calculated (in this class) just a set/get var + float getCourse() { return m_course; } + void setCourse(float course) { m_course = course; } + + float getDateOfFix() { return m_dateOfFix; } + + //groundspeed isn't calculated (in this class) just a set/get var + float getGroundSpeed() { return m_groundSpeed; } + void setGroundSpeed(float speed) { m_groundSpeed=speed; } + + + // + //NOTE: this are function that will return less percision + // + /** Latitude in decimal degrees, north is positive */ + float getLatitude(){ return gps_angle_to_float(&m_location.m_gpsCoord.latitude); } + /** Longitude in decimal degrees, east is positive */ + float getLongitude(){ return gps_angle_to_float(&m_location.m_gpsCoord.longitude); } + void setLatitude(double lat) { float_to_gps_angle(lat,&m_location.m_gpsCoord.latitude); } + void setLongitude(double lng) { float_to_gps_angle(lng,&m_location.m_gpsCoord.longitude); } + + float getMagVar() { return m_magVar; } + float getTimeOfFix() { return m_timeOfFix; } + + Waypoint getLocation() { return Waypoint(m_location); } + void setLocation(Waypoint location) { m_location.update(location.m_gpsCoord); } + + //todo + //void handleCommand(); + +}; + + +Sensor::Status LEA6H_sim::status() +{ + return Sensor::OK; +} + +LEA6H_sim::LEA6H_sim() +{ + //adjust location heading to target by some X amount? + m_warning = false; + m_updatedRMC=true; + m_course=90.0; + m_dateOfFix=100620; + m_groundSpeed=0.0; + setLatitude(47.626025); + setLongitude(-117.644087); + m_magVar=1.0; + m_timeOfFix=083559.00; +} + +void LEA6H_sim::begin() +{ + +} + +void LEA6H_sim::calibrate() +{ + +} + +void LEA6H_sim::update() +{ + //todo + + + +} + + + + +#endif diff --git a/src/input/APM/MPU6000_DMP.h b/src/input/APM/MPU6000_DMP.h new file mode 100644 index 0000000..a667444 --- /dev/null +++ b/src/input/APM/MPU6000_DMP.h @@ -0,0 +1,216 @@ +#define MPU6050_DMP_CODE_SIZE 1929 // the number of values for writing the dmpMemory[] +#define MPU6050_DMP_CONFIG_SIZE 192 // the number of values for writing the dmpConfig[] +#define MPU6050_DMP_UPDATES_SIZE 47 // the number of values for writing the dmpUpdates[] + +/* ================================================================================================ *\ + | Default MotionApps v2.0 42-byte FIFO packet structure (each value consists of 2 bytes): | + | -> this is array fifoBuffer[0-41] | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | + | | + | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | + | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | +\* =============================================================================================== */ + +// This array contains the default DMP memory bank binary that gets loaded during dmpInitialize(). +// It was reconstructed from observed I2C traffic generated by the UC3-A3 demo code, and not extracted +// directly from that code. That is true of all transmissions in this sketch, and any documentation has +// been added after the fact by referencing the Invensense code. +// It gets written to volatile memory, so it has to be done at each start (it only takes ~1 second though). +const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { + // bank 0, 256 bytes + 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, + 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, + 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, + 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, + + // bank 1, 256 bytes + 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, + 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, + 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, + 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, + + // bank 2, 256 bytes + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + + // bank 3, 256 bytes + 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, + 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, + 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, + 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, + 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, + 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, + 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, + 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, + 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, + 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, + 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, + 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, + 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, + 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, + 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, + 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, + + // bank 4, 256 bytes + 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, + 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, + 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, + 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, + 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, + 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, + 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, + 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, + 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, + 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, + 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, + 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, + + // bank 5, 256 bytes + 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, + 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, + 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, + 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, + 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, + 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, + 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, + 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, + 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, + 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, + 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, + 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, + 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, + 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, + 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, + 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, + + // bank 6, 256 bytes + 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, + 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, + 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, + 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, + 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, + 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, + 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, + 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, + 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, + 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, + 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, + 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, + 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, + 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, + 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, + 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, + + // bank 7, 137 bytes (remainder) + 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, + 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, + 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, + 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, + 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, + 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, + 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, + 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, + 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF +}; + +// This array contains the DMP configurations that gets loaded during dmpInitialize(). +// thanks to Noah Zerkin for piecing this stuff together! +const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { +// BANK OFFSET LENGTH [DATA] + 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration + 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration + 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration + 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration + 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration + 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration + 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration + 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration + 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration + 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 + 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 + 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 + 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 + 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 + 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 + 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 + 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 + 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel + 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors + 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion + 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update + 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone + // SPECIAL 0x01 = enable interrupts + 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION + 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt + 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion + 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer + 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro + 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo + 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo + 0x02, 0x16, 0x02, 0x00, 0x09 // D_0_22 inv_set_fifo_rate + + // BA: updated freq to 0x09 -> 20 Hz + // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, + // 0x01 is 100 Hz. Going faster than 100 Hz (0x00 = 200 Hz) tends to result in very noisy data. + // DMP output frequency is calculated easily using this equation: (200 Hz / (1 + value)). + + // It is important to make sure the host processor can keep up with reading and processing + // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. +}; + +// This array contains the DMP updates that get loaded during dmpInitialize(). +const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { + 0x01, 0xB2, 0x02, 0xFF, 0xFF, + 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, + 0x01, 0x6A, 0x02, 0x06, 0x00, + 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, + 0x01, 0x62, 0x02, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 +}; + + diff --git a/src/input/APM/MPU6000_HW.cpp b/src/input/APM/MPU6000_HW.cpp new file mode 100644 index 0000000..6dd4900 --- /dev/null +++ b/src/input/APM/MPU6000_HW.cpp @@ -0,0 +1,1064 @@ +#include "MPU6000_HW.h" + + + + + +// INTERRUPT FROM MPU-6000 DETECTION ROUTINE +volatile boolean mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high +volatile uint16_t gyroIrqCount=0; + +void dmpDataReady() +{ + mpuInterrupt = true; + gyroIrqCount++; +} + +uint16_t MPU6000_DMP::irqCount() +{ + return gyroIrqCount; +} + +uint16_t MPU6000_DMP::irqCountClear() +{ + gyroIrqCount=0; +} + + +void MPU6000_DMP::update() +{ + //if we have data ready (interrupt) or if there is data in fifo + if ( mpuInterrupt == true || m_fifoCount >= m_packetSize ) + { + //todo: *fix?* it is possible that interrupt maybe fire between above and now. + + + mpuInterrupt = false; + + // by reading INT_STATUS register, all interrupts are cleared + byte mpuIntStatus = SPIread(0x3A, m_chipSelect); + + // get current FIFO count + m_fifoCount = getFIFOCount(m_chipSelect); + + // check for FIFO overflow + // mpuIntStatus & 0x10 checks register 0x3A for FIFO_OFLOW_INT + // the size of the FIFO buffer is 1024 bytes, but max. set to 1008 so after 24 packets of 42 bytes + // the FIFO is reset, otherwise the last FIFO reading before reaching 1024 contains only 16 bytes + // and can/will produces output value miscalculations + if ((mpuIntStatus & 0x10) || m_fifoCount == 1008) + { + // FIFO_RESET = 1 = reset (ok) only when FIFO_EN = 0 + SPIwriteBit(0x6A, 2, true, m_chipSelect); + } + else if (mpuIntStatus & 0x02) + { + byte trycount=0; + //check for DMP data ready interrupt (this should happen frequently) + + // wait for correct available data length, should be a VERY short wait + while (m_fifoCount < m_packetSize || trycount++ < 3) + { + m_fifoCount = getFIFOCount(m_chipSelect); + } + + + if (m_fifoCount >= m_packetSize) + { + // read a packet from FIFO + SPIreadBytes(0x74, m_packetSize, m_fifoBuffer, m_chipSelect); + + // track FIFO count here in case there is more than one packet (each of 42 bytes) available + // (this lets us immediately read more without waiting for an interrupt) + //todo: Might be easier to just read packet size directly at beginning of function + m_fifoCount = m_fifoCount - m_packetSize; + + m_raw_q_w = ((m_fifoBuffer[0] << 8) + m_fifoBuffer[1]); // W + m_raw_q_x = ((m_fifoBuffer[4] << 8) + m_fifoBuffer[5]); // X + m_raw_q_y = ((m_fifoBuffer[8] << 8) + m_fifoBuffer[9]); // Y + m_raw_q_z = ((m_fifoBuffer[12] << 8) + m_fifoBuffer[13]); // Z + m_q_w = m_raw_q_w / 16384.0f; + m_q_x = m_raw_q_x / 16384.0f; + m_q_y = m_raw_q_y / 16384.0f; + m_q_z = m_raw_q_z / 16384.0f; + m_AcceX = ((m_fifoBuffer[28] << 8) + m_fifoBuffer[29]); + m_AcceY = ((m_fifoBuffer[32] << 8) + m_fifoBuffer[33]); + m_AcceZ = ((m_fifoBuffer[36] << 8) + m_fifoBuffer[37]); + m_Ax = m_AcceX / 8192.0f; // calculate g-value + m_Ay = m_AcceY / 8192.0f; // calculate g-value + m_Az = m_AcceZ / 8192.0f; // calculate g-value + m_GyroX = ((m_fifoBuffer[16] << 8) + m_fifoBuffer[17]); + m_GyroY = ((m_fifoBuffer[20] << 8) + m_fifoBuffer[21]); + m_GyroZ = ((m_fifoBuffer[24] << 8) + m_fifoBuffer[25]); + + + m_euler_x = atan2((2 * m_q_y * m_q_z) - (2 * m_q_w * m_q_x), (2 * m_q_w * m_q_w) + (2 * m_q_z * m_q_z) - 1); // phi + m_euler_y = -asin((2 * m_q_x * m_q_z) + (2 * m_q_w * m_q_y)); // theta + m_euler_z = atan2((2 * m_q_x * m_q_y) - (2 * m_q_w * m_q_z), (2 * m_q_w * m_q_w) + (2 * m_q_x * m_q_x) - 1); // psi + // m_euler_x = m_euler_x * 180/M_PI; // angle in degrees -180 to +180 + // m_euler_y = m_euler_y * 180/M_PI; // angle in degrees + // m_euler_z = m_euler_z * 180/M_PI; // angle in degrees -180 to +180 + + m_updateReady=true; + m_lastUpdateTime=millis(); + + } + else + { + //took to long wait until next interrupt + //want to avoid lockup condition with infinite loop + + + } + + + } + + + } +} + + + +void MPU6000_DMP::begin() +{ + SPI.begin(); // start the SPI library + SPI.setClockDivider(SPI_CLOCK_DIV16); // ArduIMU+ V3 board runs on 16 MHz: 16 MHz / SPI_CLOCK_DIV16 = 1 MHz + // 1 MHz is the maximum SPI clock frequency according to the MPU-6000 Product Specification + SPI.setBitOrder(MSBFIRST); // data delivered MSB first as in MPU-6000 Product Specification + SPI.setDataMode(SPI_MODE0); // latched on rising edge, transitioned on falling edge, active low + + pinMode(m_chipSelect, OUTPUT); + + Serial.println("dmpInitialize..."); + m_devStatus = dmpInitialize(); + Serial.println("done"); + + if (m_devStatus == 0) + { + // now that it's ready, turn on the DMP + //Serial.print("Enabling DMP... "); + SPIwriteBit(0x6A, 7, true, m_chipSelect); // USER_CTRL_DMP_EN + //Serial.println("done."); + + //Serial.print("Enabling interrupt detection... "); + // ArduIMU+ V3 has ATMEGA328 INT0 / D2 pin 32 (input) connected to MPU-6000 INT pin 12 (output) + attachInterrupt(6, dmpDataReady, RISING); + // by reading INT_STATUS register, all interrupts are cleared + byte mpuIntStatus = SPIread(0x3A, m_chipSelect); + //Serial.println("done"); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it + m_dmpReady = true; + + } + else + { + //todo error? + } + +} + + + + +// ############################################################################################## // +// ################################ SPI read/write functions #################################### // +// ############################################################################################## // + +// --- Function for SPI reading one byte from sensor +// reg : MPU-6000 register number to read from +// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by m_chipSelect) +// return > register contents +byte MPU6000_DMP::SPIread(byte reg, int ChipSelPin) +{ + digitalWrite(ChipSelPin, LOW); + SPI.transfer(reg | 0x80); + byte read_value = SPI.transfer(0x00); + digitalWrite(ChipSelPin, HIGH); + return read_value; +} + +// --- Function for SPI writing one byte to sensor +// reg : MPU-6000 register number to write to +// data : data to be written into reg +// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by m_chipSelect) +// return > nothing +void MPU6000_DMP::SPIwrite(byte reg, byte data, int ChipSelPin) +{ + digitalWrite(ChipSelPin, LOW); + SPI.transfer(reg); + SPI.transfer(data); + digitalWrite(ChipSelPin, HIGH); +} + +// --- Function for SPI reading one bit from sensor +// reg : MPU-6000 register number to read from +// bitNum : bit number in the register to read - 7 (MSB) to 0 (LSB) +// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by m_chipSelect) +// return > byte 0x00 if bit is 0, otherwise byte with a 1 at bitNum (rest 0's) +byte MPU6000_DMP::SPIreadBit(byte reg, byte bitNum, int ChipSelPin) +{ + byte byte_value = SPIread(reg, ChipSelPin); + byte bit_value = byte_value & (1 << bitNum); + return bit_value; +} + +//--- Function for SPI writing one bit to sensor +// reg : MPU-6000 register number to write to +// bitNum : bit number in the register to write to - 7 (MSB) to 0 (LSB) +// databit : bit value to be written into reg - false or 0 | true or non-zero (1 will be logical) +// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by m_chipSelect) +// return > nothing +// +// first read byte, then insert bit value, then write byte: +// otherwise all other bits will be written 0, this may trigger unexpected behaviour +void MPU6000_DMP::SPIwriteBit(byte reg, byte bitNum, byte databit, int ChipSelPin) +{ + byte byte_value = SPIread(reg, ChipSelPin); + if (databit == 0) + { + byte_value = byte_value & ~(1 << bitNum); + } + else // databit is intended to be a "1" + { + byte_value = byte_value | (1 << bitNum); + } + SPIwrite(reg, byte_value, ChipSelPin); +} + +//--- Function for SPI reading multiple bytes to sensor +// read multiple bytes from the same device register, most of the times this +// is the FIFO transfer register (which after each read, is automatically +// loaded with new data for the next read) +// reg : MPU-6000 register number to write to +// length : number of bytes to be read +// data : buffer array (starting with [0]) to store the read data in +// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by m_chipSelect) +// return > array of data[0 - length] +void MPU6000_DMP::SPIreadBytes(byte reg, unsigned int length, byte *data, int ChipSelPin) +{ + digitalWrite(ChipSelPin, LOW); + // wait 10 ms for MPU-6000 to react on chipselect (if this is 4 ms or less, SPI.transfer fails) + delay(10); + SPI.transfer(reg | 0x80); + unsigned int count = 0; + byte data_bytes_printed = 0; + for (count = 0; count < length; count ++) + { + data[count] = SPI.transfer(0x00); + } + digitalWrite(ChipSelPin, HIGH); + +} + +//--- Function for SPI reading multiple bits from sensor +// reg : MPU-6000 register number to read from +// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by m_chipSelect) +// return > databits +// +// 01101001 read byte +// 76543210 bit numbers +// xxx bitStart = 4, length = 3 +// 010 masked +// -> 010 shifted +byte MPU6000_DMP::SPIreadBits(byte reg, byte bitStart, byte length, int ChipSelPin) +{ + byte b = SPIread(reg, ChipSelPin); + byte mask = ((1 << length) - 1) << (bitStart - length + 1); + b = b & mask; + b = b >> (bitStart - length + 1); + return b; +} + +//--- Function for SPI writing multiple bits to sensor +// reg : MPU-6000 register number to write to +// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by m_chipSelect) +// +// bbbbb010 -> data (bits to write - leading 0's) +// 76543210 bit numbers +// xxx bitStart = 4, length = 3 +// 00011100 mask byte +// 10101111 original reg value (read) +// 10100011 original reg value & ~mask +// 10101011 masked | original reg value +// +// first read byte, then insert bit values, then write byte: +// otherwise all other bits will be written 0, this may trigger unexpected behaviour +void MPU6000_DMP::SPIwriteBits(byte reg, byte bitStart, byte length, byte data, int ChipSelPin) +{ + byte byte_value = SPIread(reg, m_chipSelect); + byte mask = ((1 << length) - 1) << (bitStart - length + 1); // create mask + data <<= (bitStart - length + 1); // shift data into correct position + data &= mask; // zero all non-important bits in data (just to make sure) + byte_value &= ~(mask); // zero all important bits in existing byte, maintain the rest + byte_value |= data; // combine data with existing byte + SPIwrite(reg, byte_value, ChipSelPin); + + #ifdef DEBUG + //Serial.print(" bits set: "); + //for (byte i = 0; i < (7 - bitStart); i ++) Serial.print("x"); + //for (byte j = 0; j < length; j ++) Serial.print(bitRead(data, bitStart - j)); + //for (byte k = 0; k < (bitStart - length + 1); k ++) Serial.print("x"); + //Serial.println(); + #endif + +} + + +// ############################################################################################## // +// ################################ DMP functions used in dmpInitialize() ####################### // +// ############################################################################################## // +// If you like to know how it works, please read on. Otherwise, just FIRE AND FORGET ;-) + +void MPU6000_DMP::setMemoryBank(byte bank, boolean prefetchEnabled, boolean userBank, int ChipSelPin) +{ + // - the value in 0x6D activates a specific bank in the DMP + // - the value in 0x6E sets the read/write pointer to a specific startaddress within the specified DMP bank + // - register 0x6F is the register from which to read or to which to write the data + // (after each r/w autoincrement address within the specified DMP bank starting from startaddress) + bank = bank & 0x1F; // 0x1F = 00011111 + // bank 0: 0 & 0x1F = 00000000 $ 00011111 = 00000000 + // bank 1: 1 & 0x1F = 00000001 $ 00011111 = 00000001 + // bank 2: 2 & 0x1F = 00000010 $ 00011111 = 00000010 + // bank 3: 3 & 0x1F = 00000011 $ 00011111 = 00000011 + // bank 4: 4 & 0x1F = 00000100 $ 00011111 = 00000100 + // bank 5: 5 & 0x1F = 00000101 $ 00011111 = 00000101 + // bank 6: 6 & 0x1F = 00000110 $ 00011111 = 00000110 + // bank 7: 7 & 0x1F = 00000111 $ 00011111 = 00000111 + // is this to maximize the number of banks to 00011111 is 0x1F = 31 ? + if (userBank) + bank |= 0x20; + if (prefetchEnabled) + bank |= 0x40; + SPIwrite(0x6D, bank, ChipSelPin); +} + +//***********************************************************// +void MPU6000_DMP::setMemoryStartAddress(byte startaddress, int ChipSelPin) +{ + // - the value in 0x6D activates a specific bank in the DMP + // - the value in 0x6E sets the read/write pointer to a specific startaddress within the specified DMP bank + // - register 0x6F is the register from which to read or to which to write the data + // (after each r/w autoincrement address within the specified DMP bank starting from startaddress) + SPIwrite(0x6E, startaddress, ChipSelPin); +} + + +//***********************************************************// +boolean MPU6000_DMP::writeDMPMemory() +{ + // - the value in 0x6D activates a specific bank in the DMP + // - the value in 0x6E sets the read/write pointer to a specific startaddress within the specified DMP bank + // - register 0x6F is the register from which to read or to which to write the data + // (after each r/w autoincrement address within the specified DMP bank starting from startaddress) + + //Serial.print("\tWriting DMP memory.......... "); + + unsigned int i, j; + byte dmp_byte; + + // ### there are 8 DMP banks (numbers 0 to 7) + + // DMP banks 0 - 6 are completely filled with 256 bytes: + for (i = 0; i < 7; i ++) + { + //DEBUG_PRINT("@@@ write bank "); DEBUG_PRINTLN(i); + setMemoryBank(i, false, false, m_chipSelect); // bank number = i + setMemoryStartAddress(0, m_chipSelect); // startaddress = 0 so start writing every DMP bank from the beginning + digitalWrite(m_chipSelect,LOW); + SPI.transfer(0x6F); + + for (j = 0; j < 256; j ++) // max. 256 bytes of data fit into one DMP bank + { + dmp_byte = pgm_read_byte(dmpMemory + (i * 256) + j); + SPI.transfer(dmp_byte); + // #ifdef DEBUG + // if (dmp_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug + // Serial.println(dmp_byte, HEX); + // #endif + } + digitalWrite(m_chipSelect,HIGH); + //DEBUG_PRINTLN(); + } + + // DMP bank 7 gets only 137 bytes: + //DEBUG_PRINTLN("@@@ write bank 7"); + setMemoryBank(7, false, false, m_chipSelect); // bank number = 7 + setMemoryStartAddress(0, m_chipSelect); // startaddress = 0 so start writing also this DMP bank from the beginning + digitalWrite(m_chipSelect,LOW); + SPI.transfer(0x6F); + + for (j = 0; j < 137; j ++) // only 137 bytes of data into DMP bank 7 + { + dmp_byte = pgm_read_byte(dmpMemory + (7 * 256) + j); + SPI.transfer(dmp_byte); + // #ifdef DEBUG + // if (dmp_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug + // Serial.println(dmp_byte, HEX); + // #endif + } + digitalWrite(m_chipSelect,HIGH); + //DEBUG_PRINTLN(); + + //Serial.println("done."); + + return true; // end of writeDMPMemory reached +} + +//***********************************************************// +boolean MPU6000_DMP::verifyDMPMemory() +{ + // - the value in 0x6D activates a specific bank in the DMP + // - the value in 0x6E sets the read/write pointer to a specific startaddress within the specified DMP bank + // - register 0x6F is the register from which to read or to which to write the data + // (after each r/w autoincrement address within the specified DMP bank starting from startaddress) + + //Serial.print("\tVerifying DMP memory.......... "); + + unsigned int i, j; + byte dmp_byte, check_byte; + boolean verification = true; + + // ### there are 8 DMP banks (numbers 0 to 7) + + // DMP banks 0 - 6 are completely read, all 256 bytes: + for (i = 0; i < 7; i ++) + { + //DEBUG_PRINT(">>> read bank "); DEBUG_PRINTLN(i); + setMemoryBank(i, false, false, m_chipSelect); // bank number = i + setMemoryStartAddress(0, m_chipSelect); // startaddress = 0 so start reading every DMP bank from the beginning + digitalWrite(m_chipSelect,LOW); + SPI.transfer(0x6F | 0x80); // 0x6F | 0x80 causes a "1" added as MSB to 0x6F to denote reading from reg i.s.o. writing to it + + for (j = 0; j < 256; j ++) // max. 256 bytes of data fit into one DMP bank + { + check_byte = pgm_read_byte(dmpMemory + (i * 256) + j); + dmp_byte = SPI.transfer(0x00); + if (dmp_byte != check_byte) + { + //Serial.println("$$$ dmpMemory: byte verification error"); + verification = false; + } + // #ifdef DEBUG + // if (dmp_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug + // Serial.println(dmp_byte, HEX); + // #endif + } + digitalWrite(m_chipSelect,HIGH); + //DEBUG_PRINTLN(); + } + + // DMP bank 7 only read first 137 bytes: + //DEBUG_PRINTLN(">>> read bank 7"); + setMemoryBank(7, false, false, m_chipSelect); // bank number = 7 + setMemoryStartAddress(0, m_chipSelect); // startaddress = 0 so start reading also this DMP bank from the beginning + digitalWrite(m_chipSelect,LOW); + SPI.transfer(0x6F | 0x80); // 0x6F | 0x80 causes a "1" added as MSB to 0x6F to denote reading from reg i.s.o. writing to it + + for (j = 0; j < 137; j ++) // only 137 bytes of data into DMP bank 7 + { + check_byte = pgm_read_byte(dmpMemory + (7 * 256) + j); + dmp_byte = SPI.transfer(0x00); + if (dmp_byte != check_byte) + { + //Serial.println("$$$ dmpMemory: byte verification error"); + verification = false; + } + // #ifdef DEBUG + // if (dmp_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug + // Serial.println(dmp_byte, HEX); + // #endif + } + digitalWrite(m_chipSelect,HIGH); + //DEBUG_PRINTLN(); + + // if (verification == true) Serial.println("success!"); + // if (verification == false) Serial.println("FAILED!"); + + return verification; // true if DMP correctly written, false if not +} + +//***********************************************************// +boolean MPU6000_DMP::writeDMPConfig() +{ + byte progBuffer, success, special; + unsigned int i, j; + // config set dmpConfig is a long string of blocks with the following structure: + // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]] + byte bank, offset, length; + + //Serial.print("\tWriting DMP configuration... "); + + for (i = 0; i < MPU6050_DMP_CONFIG_SIZE;) + { + bank = pgm_read_byte(dmpConfig + i++); // pgm_read_byte() is a macro that reads a byte of data stored in a specified address(PROGMEM area) + offset = pgm_read_byte(dmpConfig + i++); + length = pgm_read_byte(dmpConfig + i++); + + if (length > 0) // regular block of data to write + { + //DEBUG_PRINT("!! bank : "); DEBUG_PRINTLNF(bank, HEX); + setMemoryBank(bank, false, false, m_chipSelect); // bank number = bank + //DEBUG_PRINT("!! offset: "); DEBUG_PRINTLNF(offset, HEX); + setMemoryStartAddress(offset, m_chipSelect); // startaddress = offset from the beginning (0) of the bank + //DEBUG_PRINT("!! length: "); DEBUG_PRINTLNF(length, HEX); + + digitalWrite(m_chipSelect,LOW); + SPI.transfer(0x6F); + + for (j = 0; j < length; j++) + { + progBuffer = pgm_read_byte(dmpConfig + i + j); + SPI.transfer(progBuffer); + //DEBUG_PRINTLNF(progBuffer, HEX); + } + + digitalWrite(m_chipSelect,HIGH); + i = i + length; + } + else // length = 0; special instruction to write + { + // NOTE: this kind of behavior (what and when to do certain things) + // is totally undocumented. This code is in here based on observed + // behavior only, and exactly why (or even whether) it has to be here + // is anybody's guess for now. + special = pgm_read_byte(dmpConfig + i++); + //DEBUG_PRINTLN("!! Special command code "); + //DEBUG_PRINTF(special, HEX); + //DEBUG_PRINTLN(" found..."); + if (special == 0x01) + { + // enable DMP-related interrupts (ZeroMotion, FIFOBufferOverflow, DMP) + SPIwrite(0x38, 0x32, m_chipSelect); // write 00110010: ZMOT_EN, FIFO_OFLOW_EN, DMP_INT_EN true + // by the way: this sets all other interrupt enables to false + success = true; + } + else + { + // unknown other special command if this may be needed in the future, but for now this should not happen + success = false; + } + } + } + + //Serial.println("done."); + + return true; +} + +//***********************************************************// +boolean MPU6000_DMP::verifyDMPConfig() +{ + byte check_byte, progBuffer, success, special; + unsigned int i, j; + // config set dmpConfig is a long string of blocks with the following structure: + // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]] + byte bank, offset, length; + boolean verification = true; + + //Serial.print("\tVerifying DMP configuration... "); + + for (i = 0; i < MPU6050_DMP_CONFIG_SIZE;) + { + bank = pgm_read_byte(dmpConfig + i++); // pgm_read_byte() is a macro that reads a byte of data stored in a specified address(PROGMEM area) + offset = pgm_read_byte(dmpConfig + i++); + length = pgm_read_byte(dmpConfig + i++); + + if (length > 0) // regular block of data to read + { + //DEBUG_PRINT("!! bank : "); DEBUG_PRINTLNF(bank, HEX); + setMemoryBank(bank, false, false, m_chipSelect); // bank number = bank + //DEBUG_PRINT("!! offset: "); DEBUG_PRINTLNF(offset, HEX); + setMemoryStartAddress(offset, m_chipSelect); // startaddress = offset from the beginning (0) of the bank + //DEBUG_PRINT("!! length: "); DEBUG_PRINTLNF(length, HEX); + + digitalWrite(m_chipSelect,LOW); + SPI.transfer(0x6F | 0x80); // 0x6F | 0x80 causes a "1" added as MSB to 0x6F to denote reading from reg i.s.o. writing to it + + for (j = 0; j < length; j++) + { + progBuffer = pgm_read_byte(dmpConfig + i + j); + check_byte = SPI.transfer(0x00); + if (progBuffer != check_byte) + { + //DEBUG_PRINTLN("$$$ dmpConfig: byte verification error"); + verification = false; + } + // #ifdef DEBUG + // if (check_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug + // Serial.println(check_byte, HEX); + // #endif + } + + digitalWrite(m_chipSelect,HIGH); + i = i + length; + } + else // length = 0; special instruction to write + { + // NOTE: this kind of behavior (what and when to do certain things) + // is totally undocumented. This code is in here based on observed + // behavior only, and exactly why (or even whether) it has to be here + // is anybody's guess for now. + special = pgm_read_byte(dmpConfig + i++); + //DEBUG_PRINT("!! Special command code "); + //DEBUG_PRINTF(special, HEX); + //DEBUG_PRINTLN(" found..."); + if (special == 0x01) + { + // enable DMP-related interrupts (ZeroMotion, FIFOBufferOverflow, DMP) + check_byte = SPIread(0x38, m_chipSelect); // shoudl read 00110010: ZMOT_EN, FIFO_OFLOW_EN, DMP_INT_EN true + + if (check_byte != 0x32) + { + //DEBUG_PRINTLN("$$$ dmpConfig: byte verification error"); + verification = false; + } + // #ifdef DEBUG + // if (check_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug + // Serial.println(check_byte, HEX); + // #endif + + success = true; + } + else + { + // unknown special command + success = false; + } + } + } + + //if (verification == true) Serial.println("success!"); + //if (verification == false) Serial.println("FAILED!"); + + return verification; // true if DMP correctly written, false if not +} + +//***********************************************************// +// process only one line from dmpUpdates each time writeDMPUpdates() is called +unsigned int MPU6000_DMP::writeDMPUpdates(unsigned int pos, byte update_number) +{ + // pos is the current reading position within dmpUpdates + byte progBuffer, success; + unsigned int j; + // config set dmpUpdates is a long string of blocks with the following structure: + // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]] + byte bank, offset, length; + + //Serial.print("\tWriting DMP update "); Serial.print(update_number); Serial.print("/7 ..... "); + + + bank = pgm_read_byte(dmpUpdates + pos++); // pgm_read_byte() is a macro that reads a byte of data stored in a specified address(PROGMEM area) + offset = pgm_read_byte(dmpUpdates + pos++); + length = pgm_read_byte(dmpUpdates + pos++); + + //DEBUG_PRINT("!! bank : "); DEBUG_PRINTLNF(bank, HEX); + setMemoryBank(bank, false, false, m_chipSelect); // bank number = bank + //DEBUG_PRINT("!! offset: "); DEBUG_PRINTLNF(offset, HEX); + setMemoryStartAddress(offset, m_chipSelect); // startaddress = offset from the beginning (0) of the bank + //DEBUG_PRINT("!! length: "); DEBUG_PRINTLNF(length, HEX); + + digitalWrite(m_chipSelect,LOW); + SPI.transfer(0x6F); + + for (j = 0; j < length; j++) + { + progBuffer = pgm_read_byte(dmpUpdates + pos + j); + SPI.transfer(progBuffer); + //DEBUG_PRINTLNF(progBuffer, HEX); + } + + digitalWrite(m_chipSelect,HIGH); + pos = pos + length; + //DEBUG_PRINT("!! last position written: "); DEBUG_PRINTLN(pos); + + //Serial.println("done."); + + return pos; // return last used position in dmpUpdates: will be starting point for next call! +} + +//***********************************************************// +// process only one line from dmpUpdates each time writeDMPUpdates() is called +unsigned int MPU6000_DMP::verifyDMPUpdates(unsigned int pos_verify, byte update_number) +{ + // pos_verify is the current verifying position within dmpUpdates + byte check_byte, progBuffer, success; + unsigned int j; + // config set dmpUpdates is a long string of blocks with the following structure: + // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]] + byte bank, offset, length; + boolean verification = true; + + //Serial.print("\tVerifying DMP update "); Serial.print(update_number); Serial.print("/7 ..... "); + + bank = pgm_read_byte(dmpUpdates + pos_verify++); // pgm_read_byte() is a macro that reads a byte of data stored in a specified address(PROGMEM area) + offset = pgm_read_byte(dmpUpdates + pos_verify++); + length = pgm_read_byte(dmpUpdates + pos_verify++); + + //DEBUG_PRINT("!! bank : "); DEBUG_PRINTLNF(bank, HEX); + setMemoryBank(bank, false, false, m_chipSelect); // bank number = bank + //DEBUG_PRINT("!! offset: "); DEBUG_PRINTLNF(offset, HEX); + setMemoryStartAddress(offset, m_chipSelect); // startaddress = offset from the beginning (0) of the bank + //DEBUG_PRINT("!! length: "); DEBUG_PRINTLNF(length, HEX); + + digitalWrite(m_chipSelect,LOW); + SPI.transfer(0x6F | 0x80); // 0x6F | 0x80 causes a "1" added as MSB to 0x6F to denote reading from reg i.s.o. writing to it + + for (j = 0; j < length; j++) + { + progBuffer = pgm_read_byte(dmpUpdates + pos_verify + j); + check_byte = SPI.transfer(0x00); + if (progBuffer != check_byte) + { + //DEBUG_PRINTLN("$$$ dmpUpdates: byte verification error"); + verification = false; + } + // #ifdef DEBUG + // if (check_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug + // Serial.println(check_byte, HEX); + // #endif + } + + digitalWrite(m_chipSelect,HIGH); + pos_verify = pos_verify + length; + //DEBUG_PRINT("!! last position verified: "); DEBUG_PRINTLN(pos_verify); + + //if (verification == true) Serial.println("success!"); + //if (verification == false) Serial.println("FAILED!"); + //return verification; // true if DMP correctly written, false if not + + return pos_verify; // return last used position in dmpUpdates: will be starting point for next call! +} + +//***********************************************************// +/** Get current FIFO buffer size. + * This value indicates the number of bytes stored in the FIFO buffer. This + * number is in turn the number of bytes that can be read from the FIFO buffer + * and it is directly proportional to the number of samples available given the + * set of sensor data bound to be stored in the FIFO (defined by register 35 and 36). + * - return: Current FIFO buffer size + */ +unsigned int MPU6000_DMP::getFIFOCount(int ChipSelPin) +{ + // FIFO_COUNT should always be read in high-low order (0x72-0x73) in order to + // guarantee that the most current FIFO Count value is read + byte fifo_H = SPIread(0x72, m_chipSelect); + byte fifo_L = SPIread(0x73, m_chipSelect); + unsigned int two_bytes = (fifo_H << 8) | fifo_L; + return two_bytes; +} + +byte MPU6000_DMP::dmpInitialize() +{ + // Trigger a full device reset. + // A small delay of ~50ms may be desirable after triggering a reset. + + //Serial.println("Resetting MPU6000..."); + SPIwriteBit(0x6B, 7, true, m_chipSelect); // DEVICE_RESET + //digitalWrite(BLUE_LED_PIN, HIGH); // shows start of DMP inititialize + delay(30 + 170); // wait after reset + time to see the LED blink + + // Setting the SLEEP bit in the register puts the device into very low power + // sleep mode. In this mode, only the serial interface and internal registers + // remain active, allowing for a very low standby current. Clearing this bit + // puts the device back into normal mode. To save power, the individual standby + // selections for each of the gyros should be used if any gyro axis is not used + // by the application. + // disable sleep mode + //Serial.println("Disabling sleep mode..."); + SPIwriteBit(0x6B, 6, false, m_chipSelect); // SLEEP + + // get MPU hardware revision + //Serial.println("Selecting user bank 16..."); + setMemoryBank(0x10, true, true, m_chipSelect); + + //Serial.println("Selecting memory byte 6..."); + setMemoryStartAddress(0x06, m_chipSelect); + + //Serial.println("Checking hardware revision..."); + digitalWrite(m_chipSelect,LOW); + SPI.transfer(0x6F | 0x80); + byte hwRevision = SPI.transfer(0x00); + digitalWrite(m_chipSelect,HIGH); + //Serial.println("Revision @ user[16][6] = "); + //Serial.println(hwRevision, HEX); + //Serial.println("Resetting memory bank selection to 0..."); + setMemoryBank(0, false, false, m_chipSelect); + + // check OTP bank valid + //DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); + byte otpValid = SPIreadBit(0x00, 0, m_chipSelect); + //DEBUG_PRINT(F("OTP bank is ")); + //DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); + + // get X/Y/Z gyro offsets + //DEBUG_PRINTLN(F("Reading gyro offset TC values...")); + byte xgOffsetTC = SPIreadBits(0x01, 6, 6, m_chipSelect); // [7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD + byte ygOffsetTC = SPIreadBits(0x02, 6, 6, m_chipSelect); // [7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD + byte zgOffsetTC = SPIreadBits(0x03, 6, 6, m_chipSelect); // [7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD + //DEBUG_PRINT("X gyro offset = "); + //DEBUG_PRINTLN(xgOffsetTC); + //DEBUG_PRINT("Y gyro offset = "); + //DEBUG_PRINTLN(ygOffsetTC); + //DEBUG_PRINT("Z gyro offset = "); + //DEBUG_PRINTLN(zgOffsetTC); + + // load DMP code into memory banks + //DEBUG_PRINT(F("########################### Writing DMP code to MPU memory banks (")); + //DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); + //DEBUG_PRINTLN(F(" bytes)")); + if (writeDMPMemory()) + { + //DEBUG_PRINTLN(F("########################### Success! DMP code written but not verified.")); + + //DEBUG_PRINTLN(F("########################### Verify DMP code...")); + //Serial.println("verifyDMPMemory"); + verifyDMPMemory(); + + //digitalWrite(BLUE_LED_PIN, LOW); // shows end of write DMP memory + //delay(200); // time to see the LED blink + + // write DMP configuration + //DEBUG_PRINT(F("########################### Writing DMP configuration to MPU memory banks (")); + //DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); + //DEBUG_PRINTLN(F(" bytes in config def)")); + if (writeDMPConfig()) + { + //DEBUG_PRINTLN(F("########################### Success! DMP configuration written but not verified.")); + + //DEBUG_PRINTLN(F("########################### Verify DMP configuration...")); + //Serial.println("Verify DMP configuration"); + verifyDMPConfig(); + + //digitalWrite(BLUE_LED_PIN, HIGH); // shows start of write DMP configuration + //delay(200); // time to see the LED blink + + //DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); + SPIwriteBits(0x6B, 2, 3, 0x03, m_chipSelect); // CLKSEL[2:0] = 011 = PLL with Z axis gyroscope reference + + //DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); + SPIwrite(0x38, 0x12, m_chipSelect); // INT_ENABLE = 00010010 = FIFO_OFLOW_EN & DMP_INT_EN + + // register INT_ENABLE 0x38: + // bit 0 DATA_RDY_EN 0x01 + // bit 1 DMP_INT_EN 0x02 (undocumented) - enabling this bit also enables DATA_RDY_EN it seems + // bit 2 UNKNOWN_INT_EN 0x04 (undocumented) + // bit 3 I2C_MST_INT_EN 0x08 + // bit 4 FIFO_OFLOW_EN 0x10 + // bit 5 ZMOT_EN 0x20 (undocumented) + // bit 6 MOT_EN 0x40 + // bit 7 FF_EN 0x80 (undocumented) + + // register INT_STATUS 0x3A: + // bit 0 DATA_RDY_INT 0x01 + // bit 1 DMP_INT 0x02 (undocumented) + // bit 2 UNKNOWN_INT 0x04 (undocumented) + // bit 3 I2C_MST_INT 0x08 + // bit 4 FIFO_OFLOW_INT 0x10 + // bit 5 ZMOT_INT 0x20 (undocumented) + // bit 6 MOT_INT 0x40 + // bit 7 FF_INT 0x80 (undocumented) + + //DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); + //setRate(4); // 1kHz / (1 + 4) = 200 Hz (when DLPF_CFG enabled [1 to 6] - true, see below) + SPIwrite(0x19, 4, m_chipSelect); // SMPLRT_DIV[7:0] = 4 (ok) + + // FSYNC input not connnected on ArduIMU+ V3 + //DEBUG_PRINTLN(F("Disable external frame synchronization...")); + SPIwriteBits(0x1A, 5, 3, 0x00, m_chipSelect); // EXT_SYNC_SET[2:0] = 000 = input disabled + + //DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); + SPIwriteBits(0x1A, 2, 3, 0x03, m_chipSelect); // DLPF_CFG[2:0] = 011 = accel 44 Hz gyro 42 Hz + + //DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); + SPIwriteBits(0x1B, 4, 2, 0x03, m_chipSelect); // FS_SEL[1:0] = 11 = +/- 2000 deg/s + + //DEBUG_PRINTLN(F("Setting accelerometer full scale range to +/- 2 g...")); + SPIwriteBits(0x1C, 4, 2, 0x00, m_chipSelect); + + //DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); + SPIwrite(0x70, 0x03, m_chipSelect); // DMP related register + SPIwrite(0x71, 0x00, m_chipSelect); // DMP related register + + //DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); + SPIwriteBit(0x00, 0, false, m_chipSelect); // [0] OTP_BNK_VLD + + // enabling this part causes misalignment and drift of x, y and z axis + // relative to ArduIMU+ V3/MPU-6000 x, y and z axis + /* + //DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); + SPIwriteBits(0x00, 6, 6, xgOffsetTC, m_chipSelect); + SPIwriteBits(0x01, 6, 6, ygOffsetTC, m_chipSelect); + SPIwriteBits(0x02, 6, 6, zgOffsetTC, m_chipSelect); + */ + + /* + //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); + setXGyroOffset(0); + setYGyroOffset(0); + setZGyroOffset(0); + */ + + //DEBUG_PRINTLN(F("###################### Writing final memory update 1/7 (function unknown)...")); + byte update_number = 1; // holds update number for user information + unsigned int pos = 0; // pos is the current reading position within dmpUpdates; this is the first call; set pos = 0 only once! + pos = writeDMPUpdates(pos, update_number); + unsigned int pos_verify = 0; // pos_verify is the current reading position within dmpUpdates; this is the first call; set pos_verify = 0 only once! + pos_verify = verifyDMPUpdates(pos_verify, update_number); + + //DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); + update_number ++; + pos = writeDMPUpdates(pos, update_number); + pos_verify = verifyDMPUpdates(pos_verify, update_number); + + //DEBUG_PRINTLN(F("Resetting FIFO...")); + //SPIwriteBit(0x6A, 6, false, m_chipSelect); // FIFO_EN = 0 = disable + SPIwriteBit(0x6A, 2, true, m_chipSelect); // FIFO_RESET = 1 = reset (ok) only when FIFO_EN = 0 + //SPIwriteBit(0x6A, 6, true, m_chipSelect); // FIFO_EN = 1 = enable + + // Get current FIFO buffer size. + // This value indicates the number of bytes stored in the FIFO buffer. This + // number is in turn the number of bytes that can be read from the FIFO buffer + // and it is directly proportional to the number of samples available given the + // set of sensor data bound to be stored in the FIFO (register 35 and 36). + //DEBUG_PRINTLN(F("Reading FIFO count...")); + unsigned int fifoCount = getFIFOCount(m_chipSelect); + + // just after FIFO reset so count probably 0 + //DEBUG_PRINT(F("Current FIFO count = ")); + //DEBUG_PRINTLN(fifoCount); + SPIreadBytes(0x74, fifoCount, m_fifoBuffer, m_chipSelect); + + //DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); + SPIwrite(0x1F, 2, m_chipSelect); // MOT_THR[7:0] = 2 + + //DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); + SPIwrite(0x21, 156, m_chipSelect); // detection threshold for Zero Motion interrupt generation + + //DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); + SPIwrite(0x20, 80, m_chipSelect); // duration counter threshold for Motion interrupt generation. The duration counter ticks at 1 kHz, therefore MOT_DUR has a unit of 1 LSB = 1 ms + + //DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); + SPIwrite(0x22, 0, m_chipSelect); // duration counter threshold for Zero Motion interrupt generation. The duration counter ticks at 16 Hz, therefore ZRMOT_DUR has a unit of 1 LSB = 64 ms + + //DEBUG_PRINTLN(F("Resetting FIFO...")); + //SPIwriteBit(0x6A, 6, false, m_chipSelect); // FIFO_EN = 0 = disable + SPIwriteBit(0x6A, 2, true, m_chipSelect); // FIFO_RESET = 1 = reset (ok) only when FIFO_EN = 0 + //SPIwriteBit(0x6A, 6, true, m_chipSelect); // FIFO_EN = 1 = enable + + //DEBUG_PRINTLN(F("Enabling FIFO...")); + SPIwriteBit(0x6A, 6, true, m_chipSelect); // FIFO_EN = 1 = enable + + //DEBUG_PRINTLN(F("Enabling DMP...")); + SPIwriteBit(0x6A, 7, true, m_chipSelect); // USER_CTRL_DMP_EN + + //DEBUG_PRINTLN(F("Resetting DMP...")); + SPIwriteBit(0x6A, 3, true, m_chipSelect); // Reset DMP + + //DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); + update_number ++; + pos = writeDMPUpdates(pos, update_number); + pos_verify = verifyDMPUpdates(pos_verify, update_number); + + //DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); + update_number ++; + pos = writeDMPUpdates(pos, update_number); + pos_verify = verifyDMPUpdates(pos_verify, update_number); + + //DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); + update_number ++; + pos = writeDMPUpdates(pos, update_number); + pos_verify = verifyDMPUpdates(pos_verify, update_number); + + delay(50); // may be used as test just to see if number of FIFO bytes changes + + + //DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); + //Serial.println("Waiting for FIFO count to increment"); + while ((fifoCount = getFIFOCount(m_chipSelect)) < 3); + //Serial.println("Done"); + /* switched off, may crash the sketch (FIFO contents not used here anyway) + // 1st read FIFO + //DEBUG_PRINT(F("Current FIFO count = ")); + //DEBUG_PRINTLN(fifoCount); + //DEBUG_PRINTLN(F("Reading FIFO data...")); + //Serial.println("Reading FIFO data 1st time..."); + SPIreadBytes(0x74, fifoCount, fifoBuffer, m_chipSelect); + */ + + //DEBUG_PRINTLN(F("Reading interrupt status...")); + byte mpuIntStatus = SPIread(0x3A, m_chipSelect); + + //DEBUG_PRINT(F("Current interrupt status = ")); + //DEBUG_PRINTLNF(mpuIntStatus, HEX); + + // Jeff Rowberg's code had a read statement here... I suppose that must be a write statement! + //DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); + //for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + //readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + //DEBUG_PRINTLN(F("Writing final memory update 6/7 (function unknown)...")); + update_number ++; + pos = writeDMPUpdates(pos, update_number); + pos_verify = verifyDMPUpdates(pos_verify, update_number); + + //DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); + //Serial.println("Waiting for FIFO count to increment"); + while ((fifoCount = getFIFOCount(m_chipSelect)) < 3); + //Serial.println("Done"); + + //DEBUG_PRINT(F("Current FIFO count=")); + //DEBUG_PRINTLN(fifoCount); + + /* switched off, may crash the sketch (FIFO contents not used here anyway) + // 2nd read FIFO + //DEBUG_PRINTLN(F("Reading FIFO data...")); + Serial.println("Reading FIFO data 2nd time..."); + SPIreadBytes(0x74, fifoCount, fifoBuffer, m_chipSelect); + */ + + //DEBUG_PRINTLN(F("Reading interrupt status...")); + mpuIntStatus = SPIread(0x3A, m_chipSelect); + + //DEBUG_PRINT(F("Current interrupt status=")); + //DEBUG_PRINTLNF(mpuIntStatus, HEX); + + //DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); + update_number ++; + pos = writeDMPUpdates(pos, update_number); + pos_verify = verifyDMPUpdates(pos_verify, update_number); + + //DEBUG_PRINTLN(F("DMP is good to go! Finally.")); + + //DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); + SPIwriteBit(0x6A, 7, false, m_chipSelect); // USER_CTRL_DMP_EN + + //DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); + //SPIwriteBit(0x6A, 6, false, m_chipSelect); // FIFO_EN = 0 = disable + SPIwriteBit(0x6A, 2, true, m_chipSelect); // FIFO_RESET = 1 = reset (ok) only when FIFO_EN = 0 + //SPIwriteBit(0x6A, 6, true, m_chipSelect); // FIFO_EN = 1 = enable + SPIread(0x3A, m_chipSelect); // reading the register will clear all INT bits + + //Serial.println("Done dmp verified"); + + } + else + { + //DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); + return 2; // configuration block loading failed + } + + } + else + { + //DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); + return 1; // main binary block loading failed + } + + //digitalWrite(BLUE_LED_PIN, LOW); // shows end of write DMP configuration + //delay(200); // time to see the LED blink + + Serial.println("... Digital Motion Processor (DMP) initializing done."); + Serial.println(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"); + return 0; // success +} diff --git a/src/input/APM/MPU6000_HW.h b/src/input/APM/MPU6000_HW.h new file mode 100644 index 0000000..67d4a9e --- /dev/null +++ b/src/input/APM/MPU6000_HW.h @@ -0,0 +1,147 @@ +#include +#include +#include "MPU6000_DMP.h" + +#include "input/Sensor.h" + +//This arduino library is based on the following +//https://storage.ning.com/topology/rest/1.0/file/get/3691047852?profile=original + +//Which according to the source is based on: + +//This sketch is largely based on the excellent (but complicated) sketch from Jeff Rowberg, version +// 6/21/2012, Copyright (c) 2012 J. Rowberg +// https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 + +// The following is the orignal copyright notice from above link->code (github::jrowberg) + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + + +// On the ArduIMU+ V3, output pin D4 on the ATmega328P connects to +// input pin /CS (/Chip Select) of the MPU-6000. +//const int ChipSelPin1 = 53; + +#define MPU6000_DMP_DEFAULT_CS 53 + +//class MPU6000_DMP : public InertialVec +class MPU6000_DMP +{ + +protected: + // MPU control & status variables + boolean m_dmpReady = false; + unsigned int m_packetSize = 42; + unsigned int m_fifoCount; + byte m_fifoBuffer[64]; + uint8_t m_chipSelect=MPU6000_DMP_DEFAULT_CS; + byte m_devStatus=0; + + boolean m_updateReady=false; + + + int m_raw_q_w = 0; + int m_raw_q_x = 0; + int m_raw_q_y = 0; + int m_raw_q_z = 0; + float m_q_w = 0; + float m_q_x = 0; + float m_q_y = 0; + float m_q_z = 0; + int m_AcceX = 0; + int m_AcceY = 0; + int m_AcceZ = 0; + float m_Ax = 0; + float m_Ay = 0; + float m_Az = 0; + int m_GyroX = 0; + int m_GyroY = 0; + int m_GyroZ = 0; + + float m_euler_x = 0; + float m_euler_y = 0; + float m_euler_z = 0; + + uint32_t m_lastUpdateTime=0; + +public: + MPU6000_DMP() { }; + MPU6000_DMP(uint8_t chipSelect) { m_chipSelect = chipSelect; } + + void begin(); + void end(); + Sensor::Status status(); + void calibrate(); + + boolean dmpReady() { return m_dmpReady; } + + boolean updateReady() { return m_updateReady; } + void clearUpdateReady() { m_updateReady = false; } + + uint16_t irqCount(); + uint16_t irqCountClear(); + + uint32_t lastUpdateTime() { return m_lastUpdateTime; } + void setLastUpdateTime(uint32_t time) { m_lastUpdateTime = time; } + + + void update(); + + void getQ(float &q_w, float &q_x, float &q_y, float &q_z) { q_w=m_q_w; q_x=m_q_x; q_y=m_q_y; q_z=m_q_z; } + void getEuler(float &euler_x, float &euler_y, float &euler_z) { euler_x=m_euler_x; euler_y=m_euler_y; euler_z=m_euler_z; } + float getEulerX() { return m_euler_x; } + float getEulerY() { return m_euler_y; } + float getEulerZ() { return m_euler_z; } + + +private: + + byte SPIread(byte reg, int ChipSelPin); + void SPIwrite(byte reg, byte data, int ChipSelPin); + byte SPIreadBit(byte reg, byte bitNum, int ChipSelPin); + void SPIwriteBit(byte reg, byte bitNum, byte databit, int ChipSelPin); + byte SPIreadBits(byte reg, byte bitStart, byte length, int ChipSelPin); + void SPIwriteBits(byte reg, byte bitStart, byte length, byte data, int ChipSelPin); + void SPIreadBytes(byte reg, unsigned int length, byte *data, int ChipSelPin); + + + void setMemoryBank(byte bank, boolean prefetchEnabled, boolean userBank, int ChipSelPin); + void setMemoryStartAddress(byte startaddress, int ChipSelPin); + boolean writeDMPMemory(); + boolean verifyDMPMemory(); + boolean writeDMPConfig(); + boolean verifyDMPConfig(); + unsigned int writeDMPUpdates(unsigned int pos, byte update_number); + unsigned int verifyDMPUpdates(unsigned int pos_verify, byte update_number); + unsigned int getFIFOCount(int ChipSelPin); + byte dmpInitialize(); + + +}; + + + + + + + + diff --git a/src/input/Bumper.h b/src/input/Bumper.h new file mode 100644 index 0000000..c078378 --- /dev/null +++ b/src/input/Bumper.h @@ -0,0 +1,127 @@ +/* Copyright 2021 MINDS-i, INC. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +#ifndef BUMPER_H +#define BUMPER_H +#include "MINDSi.h" + +#define DEFAULT_DEBOUNCE_DELAY 25 +#define BUMPER_BUTTON_LEFT 0 +#define BUMPER_BUTTON_RIGHT 1 + +class bumper +{ +private: + + int8_t m_pin[2]; + int8_t m_buttonState[2]; + int8_t m_buttonEvent[2]; + int8_t m_lastButtonState[2]; + uint32_t m_lastDebounceTime[2]; + uint32_t m_debounceDelayAmount; + +public: + + bumper() + { + m_pin[BUMPER_BUTTON_LEFT]=A5; + m_pin[BUMPER_BUTTON_RIGHT]=A6; + m_buttonState[BUMPER_BUTTON_LEFT] = 0; + m_buttonState[BUMPER_BUTTON_RIGHT] = 0; + m_buttonEvent[BUMPER_BUTTON_LEFT] = 0; + m_buttonEvent[BUMPER_BUTTON_LEFT] = 0; + m_lastButtonState[BUMPER_BUTTON_LEFT] = 0; + m_lastButtonState[BUMPER_BUTTON_RIGHT] = 0; + m_lastDebounceTime[BUMPER_BUTTON_LEFT] = 0; + m_lastDebounceTime[BUMPER_BUTTON_RIGHT] = 0; + + m_debounceDelayAmount = DEFAULT_DEBOUNCE_DELAY; + + pinMode(m_pin[BUMPER_BUTTON_LEFT], INPUT); + pinMode(m_pin[BUMPER_BUTTON_RIGHT], INPUT); + + } + + void begin(uint8_t leftPin, uint8_t rightPin) + { + pinMode(leftPin, INPUT); + pinMode(rightPin, INPUT); + + m_pin[BUMPER_BUTTON_LEFT] = leftPin; + m_pin[BUMPER_BUTTON_RIGHT] = rightPin; + } + + void updateButton(uint8_t pin_id) + { + uint8_t curButtonState; + + if (pin_id > 2) + return; + + //invert state: pressed is logic level 0 + curButtonState = !digitalRead(m_pin[pin_id]); + //Serial.println(curButtonState); + + if (curButtonState != m_lastButtonState[pin_id]) + { + m_lastDebounceTime[pin_id] = millis(); + } + else + { + if (millis() - m_lastDebounceTime[pin_id] > m_debounceDelayAmount) + { + if (curButtonState != m_buttonState[pin_id]) + { + m_buttonState[pin_id] = curButtonState; + m_buttonEvent[pin_id] = 1; + } + } + } + + //save state + m_lastButtonState[pin_id] = curButtonState; + + } + + void update() + { + updateButton(BUMPER_BUTTON_LEFT); + updateButton(BUMPER_BUTTON_RIGHT); + } + + uint8_t buttonEvent(uint8_t pin_id) + { + uint8_t event = m_buttonEvent[pin_id]; + //clear event + m_buttonEvent[pin_id] = 0; + return event; + } + + uint8_t buttonState(uint8_t pin_id) + { + return m_buttonState[pin_id]; + } + + uint8_t leftButtonEvent() { return buttonEvent(BUMPER_BUTTON_LEFT); } + uint8_t rightButtonEvent() { return buttonEvent(BUMPER_BUTTON_RIGHT); } + + uint8_t leftButtonState() { return buttonState(BUMPER_BUTTON_LEFT); } + uint8_t rightButtonState() { return buttonState(BUMPER_BUTTON_RIGHT);} + + + +}; + +#endif diff --git a/src/math/Waypoint.cpp b/src/math/Waypoint.cpp index c6dd598..7dc5f51 100644 --- a/src/math/Waypoint.cpp +++ b/src/math/Waypoint.cpp @@ -2,61 +2,52 @@ #ifndef STAND_ALONE_TEST #include "comms/Protocol.h" -float Waypoint::getAltitude(){ +float Waypoint::getAltitude() +{ return ((double)extra)/((double)Protocol::U16_FIXED_FACTOR); } -float Waypoint::getApproachSpeed(){ + +float Waypoint::getApproachSpeed() +{ return ((double)extra)/((double)Protocol::U16_FIXED_FACTOR); } -Waypoint::Components Waypoint::headingComponents(const Waypoint& target) const { - float aRlat = this->radLatitude(); - float aRlng = this->radLongitude(); - float bRlat = target.radLatitude(); - float bRlng = target.radLongitude(); + + +//This isn't used much and should be updated to not use components (GPS_LOCAL seems better use) +Waypoint::Components Waypoint::headingComponents(const Waypoint& target) const +{ + LOCAL_COORD lc; + calc_haversine(&(this->m_gpsCoord),&(target.m_gpsCoord),&lc); Waypoint::Components result; - result.y = sin(bRlng - aRlng) * cos(bRlat); - result.x = cos(aRlat)*sin(bRlat) - sin(aRlat)*cos(bRlat)*cos(bRlng - aRlng); - return result; + result.y = lc.y; + result.x = lc.x; + return result; } -float Waypoint::headingTo(const Waypoint& target) const { - auto cmps = headingComponents(target); - return toDeg( atan2(cmps.y,cmps.x) ); + +float Waypoint::headingTo(const Waypoint& target) const +{ + LOCAL_COORD lc; + calc_haversine(&m_gpsCoord,&target.m_gpsCoord,&lc); + return lc.heading; } -float Waypoint::distanceTo(const Waypoint& target) const { - float aRlat = this->radLatitude(); - float aRlng = this->radLongitude(); - float bRlat = target.radLatitude(); - float bRlng = target.radLongitude(); - float sinlat = sin((aRlat - bRlat)/2.); - float sinlng = sin((aRlng - bRlng)/2.); - float chord = sinlat*sinlat + sinlng*sinlng*cos(aRlat)*cos(bRlat); - return 2. * Units::EARTH_RAD * atan2( sqrt(chord), sqrt(1.-chord) ); + +float Waypoint::distanceTo(const Waypoint& target) const +{ + LOCAL_COORD lc; + calc_haversine(&m_gpsCoord,&target.m_gpsCoord,&lc); + return lc.distance/METERS_PER_MILE; } -Waypoint -Waypoint::extrapolate(float bearing, float distance) const {//degrees,miles - // for small distances the error from earth's curvature is less than - // the error in the floating point trig, so a quick rectilinear - // approximation behaves better - if(distance < 2.0 /*miles*/){ - float dy = cos(toRad(bearing)) * distance; - float dx = sin(toRad(bearing)) * distance; - float dlat = dy*(360.0 / Units::EARTH_CIRC); - float dlon = dx*(360.0 / (Units::EARTH_CIRC*cos(radLatitude()))); - return Waypoint(degLatitude() +dlat, - degLongitude()+dlon, - Units::DEGREES, - extra); - } - // Full great circle curve algorithm - float rlat, rlng; - float pRlat = radLatitude(); - float pRlng = radLongitude(); - rlat = asin(sin(pRlat)*cos(distance/Units::EARTH_RAD) + - cos(pRlat)*sin(distance/Units::EARTH_RAD)*cos(toRad(bearing))); - rlng = pRlng+atan2( - (sin(toRad(bearing))*sin(distance/Units::EARTH_RAD)*cos(pRlat)), - (cos(distance/Units::EARTH_RAD)-sin(pRlat)*sin(rlat)) - ); - return Waypoint(rlat, rlng, Units::RADIANS, extra); + +//degrees,miles + +Waypoint Waypoint::extrapolate(float bearing, float distance) const +{ + GPS_COORD gc; + LOCAL_COORD lc; + lc.distance=distance*METERS_PER_MILE; + lc.heading=bearing; + new_gps_float(&m_gpsCoord,&lc,&gc); + + return Waypoint(gc); } #endif diff --git a/src/math/Waypoint.h b/src/math/Waypoint.h index 2922185..0d95894 100644 --- a/src/math/Waypoint.h +++ b/src/math/Waypoint.h @@ -1,52 +1,85 @@ #ifndef WAYPOINT_H #define WAYPOINT_H +#include #include "Arduino.h" #include "SpatialMath.h" -#include -class Waypoint{ +#include "floatgps.h" + + +enum GPS_DATA_PRECISION +{ + GPS_DATA_PRECISION_UNKNOWN, + GPS_DATA_PRECISION_LOW, + GPS_DATA_PRECISION_HIGH, +}; + +class Waypoint +{ + public: - float lat, lng; //these are stored in degrees + //float lat, lng; //these are stored in degrees uint16_t extra; //8.8 fixed value used for altitude (air) or speed (ground) - Waypoint(): - lat(0), lng(0), extra(0) {} - Waypoint(float latitude, float longitude): //entry in degrees - lat(latitude), lng(longitude), extra(0) {} - Waypoint(float latitude, float longitude, Units::Rotation rad, uint16_t ex): - lat(latitude), lng(longitude), extra(ex) { - if(rad == Units::RADIANS){ - lat = toDeg(latitude); - lng = toDeg(longitude); - } //if not set to radians, they will stay initialized to degrees - } - void update(float latitude, float longitude){ - lat = latitude; - lng = longitude; - } - void update(float latitude, float longitude, Units::Rotation rad){ - if(rad == Units::RADIANS){ - lat = toDeg(latitude); - lng = toDeg(longitude); - } else { - lat = latitude; - lng = longitude; - } - } - float radLatitude() const { - return toRad(lat); - } - float radLongitude() const { - return toRad(lng); - } - float degLatitude() const { - return lat; + GPS_COORD m_gpsCoord; + uint8_t m_precision = GPS_DATA_PRECISION_UNKNOWN; + + Waypoint() { update(0.0,0.0);} + Waypoint(float latitude, float longitude) { update(latitude,longitude); } + + // Not sure if needed right now + // Waypoint(float latitude, float longitude, Units::Rotation rad, uint16_t ex) + // { float_to_gps_angle(0.0,&this->m_gpsCoord.longitude); float_to_gps_angle(0,&this->m_gpsCoord.latitude); } + // { + // if(rad == Units::RADIANS){ + // lat = toDeg(latitude); + // lng = toDeg(longitude); + // } //if not set to radians, they will stay initialized to degrees + // } + + Waypoint(GPS_COORD gpsCoord) { update(gpsCoord); } + + + + void update(float latitude, float longitude) + { + float_to_gps_angle(longitude,&this->m_gpsCoord.longitude); + float_to_gps_angle(latitude,&this->m_gpsCoord.latitude); + m_precision=GPS_DATA_PRECISION_LOW; } - float degLongitude() const { - return lng; + + // void update(float latitude, float longitude, Units::Rotation rad) + // { + // if(rad == Units::RADIANS){ + // lat = toDeg(latitude); + // lng = toDeg(longitude); + // } else { + // lat = latitude; + // lng = longitude; + // } + // } + + void update(GPS_COORD gpsCoord) + { + m_gpsCoord = gpsCoord; + m_precision=GPS_DATA_PRECISION_HIGH; } + + //*warning* this will return loss of percision. + // These should be removed at some point when no longer used + float radLatitude() const { return toRad(degLatitude()); } + float radLongitude() const { return toRad(degLongitude()); } + float degLatitude() const { return gps_angle_to_float(&m_gpsCoord.latitude); } + float degLongitude() const { return gps_angle_to_float(&m_gpsCoord.longitude); } + + //returns lat/lon without precision loss + GPS_ANGLE angLatitude() const { return m_gpsCoord.latitude; } + GPS_ANGLE angLongitude() const { return m_gpsCoord.longitude; } + struct Components{ float y, x; }; + /** + * *** Should be removed in future: GPS_LOCAL instead *** * Calculate the resulting vector as components for direct travel to * the target waypoint. The resulting vector components are not normalized. * x = North positive, south negative; @@ -78,16 +111,28 @@ class Waypoint{ * @return The resulting GPS location */ Waypoint extrapolate(float bearing, float distance) const; - void setExtra(uint16_t alt){ + + + /** + * Extra is used for both elevation (air) and speed (ground) + */ + void setExtra(uint16_t alt) + { extra = alt; } - uint16_t getExtra() const { + uint16_t getExtra() const + { return extra; } -#ifndef STAND_ALONE_TEST + + uint8_t getGPSDataPrecision() { return m_precision; } + + + #ifndef STAND_ALONE_TEST float getAltitude(); float getApproachSpeed(); -#endif + #endif + }; #endif diff --git a/src/math/floatgps.cpp b/src/math/floatgps.cpp new file mode 100644 index 0000000..87f900c --- /dev/null +++ b/src/math/floatgps.cpp @@ -0,0 +1,189 @@ +#include +#include // for strlen, strchr, strncpy +#include +#include // for toupper +#include "floatgps.h" + +#include "gps_angle.h" +#include "ftoa.h" + + +/***************************************************************************** +FUNCTION: fearth_radius() +Calculate radius of earth in meters from GPS coordinates +6471000 mean radius + +latitude is given in radians and is the geodedic latitude +Earth radius varies by +7000 m to -22000 m which is about +https://planetcalc.com/7721/ +radius at equator (a) = 6378137 m +radius at pole (b) =6356752.3142 m +*****************************************************************************/ +float fearth_radius(float latitude) +{ + float radius_equator = 6379137.f; + float radius_pole = 6356752.3142f; + float rp_sin_lat = radius_pole * sinf(latitude); + float re_cos_lat = radius_equator * cosf(latitude); + float rp_sin_lat_2 = rp_sin_lat * rp_sin_lat; + float re_cos_lat_2 = re_cos_lat * re_cos_lat; + + float denom = re_cos_lat_2 + rp_sin_lat_2; + float numer = radius_equator * radius_equator *re_cos_lat_2 + + radius_pole * radius_pole * rp_sin_lat_2; + + + float radius = sqrtf(numer/denom); + return radius; +} // end fearth_radius + + +/***************************************************************************** +FUNCTION: new_gps_float() +Calculate new longitude using float precision +*****************************************************************************/ +int new_gps_float(const GPS_COORD* gps_start, // [in] starting GPS coordinates + const LOCAL_COORD* local, // [in] distance and heading + GPS_COORD* gps_end) // [out] end GPS coordinates +{ + int err = 0; + float lat_degrees = gps_angle_to_float(&gps_start->latitude); + float long_degrees = gps_angle_to_float(&gps_start->longitude); + //double dlat_degrees = gps_angle_to_double(&gps_start->latitude); + //double dlong_degrees = gps_angle_to_double(&gps_start->longitude); + + //double phi1 = deg_to_rad(dlat_degrees); + //double lambda1 = deg_to_rad(dlong_degrees); + + + float fphi1 = fdeg_to_rad(lat_degrees); + float flambda1 = fdeg_to_rad(long_degrees); + + float theta = fdeg_to_rad(local->heading); + float delta = local->distance/fearth_radius(fphi1); + + // // for testing here... + // float inc_sin_phi = -0.5f*delta*delta*sinf(fphi1)+delta*cosf(theta)*cosf(fphi1); + // double sin_phi2 = sin(phi1) + inc_sin_phi; + // double phi2 = asin(sin_phi2); + // float delta_phi2 = (float)(phi2-phi1); + + //float inc_phi2_precise = delta * cosf(theta); // this is faster, but not quite as precise + float inc_phi2_precise = -0.5f*delta*delta*tanf(fphi1)+delta*cosf(theta); + float inc_phi2_precise_deg = frad_to_deg(inc_phi2_precise); + gps_angle_add_increment(&gps_start->latitude, inc_phi2_precise_deg, &gps_end->latitude); + float fphi2_degrees = gps_angle_to_float(&gps_end->latitude); + float fphi2 = fdeg_to_rad(fphi2_degrees); + float y = sinf(theta)*delta*cosf(fphi1); + float x = cosf(delta) - sinf(fphi1)*sinf(fphi2); + float lambda_inc = atan2f(y,x); + float lambda_inc_degrees = frad_to_deg(lambda_inc); + gps_angle_add_increment(&gps_start->longitude, lambda_inc_degrees, &gps_end->longitude); + + return err; +} // end new_gps_float + +/****************************************************************************** +FUNCTION: calc_delta_gps +determine the difference in latitude and longitude for two GPS points. +This assumes that the difference is small and can be represented as a +float. This picks the shortest distance. +These differences are in range -180 < delta <= 180 +******************************************************************************/ +int calc_delta_gps(const GPS_COORD* gps1, // starting GPS + const GPS_COORD* gps2, // ending GPS + DELTA_GPS* delta_gps) // difference in degrees (float) + +{ + int err = 0; + + delta_gps->latitude = gps_angle_diff(&gps1->latitude, &gps2->latitude); + delta_gps->longitude = gps_angle_diff(&gps1->longitude, &gps2->longitude); + return err; +} // end calc_delta_gps + +/****************************************************************************** +FUNCTION: calc_haversine() +Calculate the distance and heading from two GPS coordinates using the Haversine +method +This uses the small angle approximation for delta phi and delta lambda +******************************************************************************/ +int calc_haversine(const GPS_COORD* gps_start, // starting GPS (degrees) + const GPS_COORD* gps_end, // ending GPS (degrees) + LOCAL_COORD* local) // [out] local coordinates relative to start +{ + int err = 0; + DELTA_GPS delta_gps; +// converts two double gps to single float differences + err = calc_delta_gps(gps_start, gps_end, &delta_gps); + float delta_phi = fdeg_to_rad(delta_gps.latitude); + float delta_lambda = fdeg_to_rad(delta_gps.longitude); + float fphi1_deg = gps_angle_to_float(&gps_start->latitude); + float fphi1 = fdeg_to_rad(fphi1_deg); + float flambda1_deg = gps_angle_to_float(&gps_start->longitude); + float flambda1 = fdeg_to_rad(flambda1_deg); + float fphi2_deg = gps_angle_to_float(&gps_end->latitude); + float fphi2 = fdeg_to_rad(fphi2_deg); + float flambda2_deg = gps_angle_to_float(&gps_end->longitude); + float flambda2 = fdeg_to_rad(flambda2_deg); + + //float sin_delta_phi = sinf(delta_phi/2.f); + float sin_delta_phi = delta_phi/2.f; // small angle sinf approximation + sin_delta_phi *= sin_delta_phi; + //float sin_delta_lambda = sinf(delta_lambda/2.f); + float sin_delta_lambda = delta_lambda/2.f; + sin_delta_lambda *= sin_delta_lambda; + float a = sin_delta_phi + cosf(fphi1)*cos(fphi2)*sin_delta_lambda; + //float c = 2.f * atan2f(sqrtf(a), sqrtf(1-a)); + float c = 2.f * sqrtf(a); // small angle approx + local->distance = c * fearth_radius(fphi1); + + //float y = sinf(delta_lambda) * cosf(fphi2); + //float x = cosf(fphi1) * sinf(fphi2) - sinf(fphi1) * cosf(fphi2) * cosf(delta_phi); + float y = delta_lambda * cosf(fphi2); // small angle approx + //float x = cosf(fphi1) * sinf(fphi2) - sinf(fphi1) * cosf(fphi2); // small angle approx + //float x = sin(fphi2 - fphi1); // another small angle approx + float x = delta_phi; // yet another small angle approx + float theta = atan2f(y,x); // theta is not necessarily a small angle so keep this + float theta_deg = frad_to_deg(theta); + theta_deg += 360.f; + if (theta_deg >= 360.f) + { + theta_deg -= 360.f; + } + + local->heading = theta_deg; + local->x = x; + local->y = y; + + return err; +} // end calc_haversine + +/****************************************************************************** +FUNCTION gps_coord_to_str() +Convert both latitude and longitude to strings +******************************************************************************/ +int gps_coord_to_str(GPS_COORD* gps, // coordinates to convert + char* str, // place to put string + int len, // length of string + int num_dec, // number of digits after decimal place + const char* fmt) // one of "DD", "DMM", or "DMS" +{ + int err = 0; + int elat = 0; + int elon = 0; + elat = lonlat_to_str(&gps->latitude, str, len, num_dec, fmt); + if (!elat) + { + strcat(str, ","); + int lat_len = strlen(str); + elon = lonlat_to_str(&gps->longitude, str+lat_len, len-lat_len, num_dec, fmt); + } + if (elon || elat) + { + err = GPS_ANGLE_ERR_BAD_CONVERSION; + } + return err; +} // end gps_coord_to_str + + diff --git a/src/math/floatgps.h b/src/math/floatgps.h new file mode 100644 index 0000000..66ba3cd --- /dev/null +++ b/src/math/floatgps.h @@ -0,0 +1,60 @@ +/***************************************************************************** +FILE: floatgps.h +This defines structures and functions for handling GPS coordinates using +floats where possible instead of double +*****************************************************************************/ +#if !defined (FLOATGPS_H) +#define FLOATGPS_H + +#include "gps_angle.h" + +#define METERS_PER_MILE 1609.34 + +typedef struct +{ + GPS_ANGLE latitude; // latitude (degrees) + GPS_ANGLE longitude; // longitude (degrees) +} GPS_COORD; + +typedef struct +{ + float distance; // distance meters + float heading; // degrees east on north + float x; //X component of distance and heading + float y; //y componet of distance and heading +} LOCAL_COORD; + +typedef struct +{ + float latitude; // degrees difference in latitude + float longitude; // degress difference in longitude +} DELTA_GPS; + + +float fearth_radius(float latitude); + + + +// int new_gps_exact(GPS_COORD* gps_start, // [in] starting GPS coordinates +// LOCAL_COORD* local, // [in] distance and heading +// GPS_COORD* gps_end); // [out] end GPS coordinates + +int new_gps_float(const GPS_COORD* gps_start, // [in] starting GPS coordinates + const LOCAL_COORD* local, // [in] distance and heading + GPS_COORD* gps_end); // [out] end GPS coordinates + +int calc_delta_gps(const GPS_COORD* gps1, // [in] starting GPS + const GPS_COORD* gps2, // [in] ending GPS + DELTA_GPS* delta_gps); // [out] difference in degrees (float) + +int calc_haversine(const GPS_COORD* gps_start, // [in] starting GPS (degrees) + const GPS_COORD* gps_end, // [in] ending GPS (degrees) + LOCAL_COORD* local); // [out] local coordinates relative to start +int gps_coord_to_str(GPS_COORD* gps, // coordinates to convert + char* str, // place to put string + int len, // length of string + int num_dec, // number of digits after decimal place + const char* fmt); // one of "DD", "DMM", or "DMS" + +#endif + diff --git a/src/math/ftoa.cpp b/src/math/ftoa.cpp new file mode 100644 index 0000000..5cc04b4 --- /dev/null +++ b/src/math/ftoa.cpp @@ -0,0 +1,184 @@ +/****************************************************************************** +FILE: ftoa.c +functions to help with converting float to ascii string +******************************************************************************/ +#include +#include +#include "ftoa.h" + +#define DEBUG_FTOA_MINUTES 0 + +/****************************************************************************** +FUNCTION: ftoa_frac() +Convert float to ascii string + +Multiply by 10, assign to unsigned int store in digit +subtract result +do until number of digits is determined +The remainder at the end allows rounding +Rounding will be done on the string. + +This assumes a positive or zero float between 0 <= x < 1.0 +result will be ".ddddddd" with precision digits + +******************************************************************************/ +int ftoa_frac(float x, //[in] numberto convert + char* buf, // [in] place to put ASCII + int len, // [in] length of buffer including null + int precision) // [in] number of digits after decimal +{ + int err = 0; + float y = x; + int n; + if (len < precision + 2) // length plus decimal and null + { + err = FTOA_ERR_TOO_LONG; + return err; + } + if (x < 0.f || x >= 1.0f) + { + err = FTOA_ERR_BAD_VALUE; + } + char* ptr = buf; + *ptr++ = '.'; + for(n = 0; n < precision; n++) + { + unsigned int temp; + y *= 10.f; + temp = (unsigned int) y; +#if DEBUG_FTOA_MINUTES + printf("temp = %d\n", temp); +#endif + *ptr++ = '0' + temp; + y -= (float)temp; + } + *ptr++ = '\0'; +#if DEBUG_FTOA_MINUTES + printf("ftoa_frac buf = %s\n", buf); +#endif + if (y >= 0.5f) + { + err = ftoa_roundup(buf, precision); +#if DEBUG_FTOA_MINUTES + if (err != 0) + { + printf("ftoa_roundup returned %d\n", err); + } + else + { + printf("rounded up to %s\n", buf); + } +#endif + } + + return err; + +} // end ftoa_frac + +/****************************************************************************** +FUNCTION: ftoa_roundup() +roundup the ASCIi string by adding 1 and carrying through the digits +input is a string starting with . +len is index of first digit to start. Work backwords until no carry or . +or beginning of buffer +Error if carry is beyond the first character +In this case the value rounds to 1.0 +The calling function should check for this and needs to add 1 to integerpart + +******************************************************************************/ +int ftoa_roundup(char* buf, // [in,out] pointer to buffer with number to round + int start_index) // [in] index to start with + +{ + int err = 0; + int i; + int carry = 0; + int end_i = 1; + if (buf[0] != '.') + { + end_i = 0; + } + for (i = start_index; i >= end_i; i--) + { + char digit = buf[i]; + if (digit < '0' || digit > '9') + { + err = FTOA_ERR_BAD_CHAR; + return err; + } + if (digit == '9') + { + buf[i] = '0'; + carry = 1; + } + else + { + buf[i] += 1; + carry = 0; + break; // done + } + } // end for(i... + if (carry) + { + err = FTOA_ERR_ROUND; + } + return err; +} //end ftoa_roundup + +/****************************************************************************** +FUNCTION: ftoa_minutes_to_deg_str() +convert decimal minutes string to degree decimal string. +The minutes will be less than 60 to give a fraction less than 1. +Assume a positive number. +This converts to 32-bit unsigned integers and performs the operation on this +number to give 26 bits of precision for result +******************************************************************************/ +int ftoa_minutes_to_deg_str(int abs_minutes, // 0-59 for number of minutes + float abs_frac, // fraction of minute + char* buf, // place to put string + int buf_len) // length of buffer including null character +{ + int err = 0; + const unsigned long uscale = 10000000UL; // 10^7 + char* ptr = buf; + float scale = (float) (uscale); // 10^7 + unsigned long lval = ((unsigned long) abs_minutes) * uscale; + unsigned long lfrac = (unsigned long)(abs_frac * scale); +#if DEBUG_FTOA_MINUTES + printf(" abs_minutes = %d, abs_frac = %.9f\n", abs_minutes, abs_frac); + printf(" lval = %lu, lfrac = %lu\n", lval, lfrac); +#endif + if (buf_len < (7+2)) // 7 digits . and null + { + return FTOA_ERR_TOO_LONG; + } + *ptr++ = '.'; + lval += lfrac; + lval /= 6UL; // divide by 60 and multiply by 10 to shift to left +#if DEBUG_FTOA_MINUTES + printf("starting lval = %lu\n", lval); +#endif + int i; + for (i = 0; i < 7; i++) + { + unsigned long tval = lval/uscale; +#if DEBUG_FTOA_MINUTES + printf("tval = %lu\n", tval); +#endif + lval -= tval*uscale; + lval *= 10UL; // shift digits to left +#if DEBUG_FTOA_MINUTES + printf("next lval = %lu\n", lval); +#endif + *ptr++ = '0' + tval; + } + *ptr = '\0'; +#if DEBUG_FTOA_MINUTES + printf("minutes_to_deg_str: returns %s\n", buf); +#endif + return err; + + + +} // end ftoa_minutes_to_deg_str + diff --git a/src/math/ftoa.h b/src/math/ftoa.h new file mode 100644 index 0000000..685f1a7 --- /dev/null +++ b/src/math/ftoa.h @@ -0,0 +1,25 @@ +/****************************************************************************** +FILE: ftoa.h +functions to help with converting float to ascii string +******************************************************************************/ +#if !defined(FTOA_H) +#define FTOA_H + +#define FTOA_ERR_BAD_VALUE (-1) +#define FTOA_ERR_TOO_LONG (-2) +#define FTOA_ERR_ROUND (-3) +#define FTOA_ERR_BAD_CHAR (-4) + +int ftoa_frac(float x, //[in] numberto convert + char* buf, // [in,out] place to put ASCII + int len, // [in] length of buffer including null + int precision); // [in] number of digits after decimal +int ftoa_roundup(char* buf, // [in,out] pointer to buffer with number to round + int start_index); //[in] index to start with + +int ftoa_minutes_to_deg_str(int abs_minutes, // 0-59 for number of minutes + float abs_frac, // fraction of minute + char* buf, // place to put string + int buf_len); // length of buffer including null character +#endif + diff --git a/src/math/gps_angle.cpp b/src/math/gps_angle.cpp new file mode 100644 index 0000000..117236b --- /dev/null +++ b/src/math/gps_angle.cpp @@ -0,0 +1,393 @@ +/****************************************************************************** +FILE: gps_angle.c +Handles high precision angles using float +******************************************************************************/ + +#include +#include // for strlen, strchr, strncpy +#include +#include +#include // for toupper +//#include "global_defs.h" // DEBUG_PRINT and DEBUG_DOUBLE +#include "floatgps.h" +#include "ftoa.h" + + +#define MIN_IN_180 10800 +#define MIN_IN_360 21600 + +/****************************************************************************** +FUNCTION: gps_angle_to_float() +Converts GPS_ANGLE value to float degrees +Loses some precision +Used to calculate sin and cos when precision is not critical +******************************************************************************/ +float gps_angle_to_float(const GPS_ANGLE* angle) +{ + float result; + result = (float)angle->minutes + angle->frac; + result /= 60.f; + return result; +} // end gps_angle_to_float + +/****************************************************************************** +FUNCTION: float_to_gps_angle() +convert a float degrees to GPS_ANGLE +******************************************************************************/ +void float_to_gps_angle(float degrees, GPS_ANGLE* gps) +{ + float dminutes = degrees * 60.; + double frac = fmod(dminutes, 1.); +#if DEBUG_PRINT + printf("float_to_gps_angle: degrees = %9e\n", degrees); + printf("float_to_gps_angle: dminutes = %.9e\n", dminutes); + printf("float_to_gps_angle: frac = %.9e\n", frac); +#endif + gps->minutes = (int)(dminutes - frac); + gps->frac = (float)frac; +#if DEBUG_PRINT + print_gps_angle("float_to_gps_angle", gps); +#endif + gps_angle_normalize(gps); +#if DEBUG_PRINT + print_gps_angle("after normalize", gps); +#endif +} //end float_to_gps_angle + +/****************************************************************************** +FUNCTION: gps_angle_normalize() +checks the minutes and frac parts of the angle and makes the fractional part +be between -1.0 < frac < 1.0 +For LONGITUDE, makes the angle -180 < angle <= 180. +******************************************************************************/ +void gps_angle_normalize(GPS_ANGLE* angle) +{ + int minutes = angle->minutes; + float frac = angle->frac; + float r = fmod(frac, 1.f); // fractional part with sign + float i = frac - r; // integral part with sign + minutes += (int)i; + frac = r; + // if signs are different, then adjust + if (frac != 0) + { + if (frac > 0.f && minutes < 0.f) + { + minutes += 1.f; + frac -= 1.f; + } + else if (frac < 0.f && minutes > 0.f) + { + minutes -= 1.f; + frac += 1.f; + } + } + angle->minutes = minutes; + angle->frac = frac; + if (angle->type == LONGITUDE_TYPE) + { + if (angle->minutes > MIN_IN_180) + { + angle->minutes -= MIN_IN_360; + } + else if (angle->minutes <= -MIN_IN_180) + { + angle->minutes += MIN_IN_360; + } + } +} // end gps_angle_normalize + +/****************************************************************************** +FUNCTION: gps_angle_negate() +change sign of angle +******************************************************************************/ +void gps_angle_negate(GPS_ANGLE* angle) +{ + angle->minutes = -angle->minutes; + angle->frac = -angle->frac; +}// end gps_angle_negate + +/****************************************************************************** +FUNCTION: gps_angle_apply_sign() +change sign of angle +******************************************************************************/ +void gps_angle_apply_sign(GPS_ANGLE* angle, char dir) +{ + char c = toupper(dir); + if (angle->type == LATITUDE_TYPE) + { + if (c == 'S') + { + gps_angle_negate(angle); + } + } + else if (angle->type == LONGITUDE_TYPE) + { + if (c == 'W') + { + gps_angle_negate(angle); + } + } +} //end gps_angle_apply_sign + +/****************************************************************************** +FUNCTION: gps_angle_diff() +subtracts two GPS_ANGLES and returns difference as float degrees +latitude is always monotonic, so a simple subtraction will always work. +we pick the shortest distance and know that the range is +-180 < longitude <= 180 +if the difference is greater than 180 degrees, then go the other way. +180 * 60 = 1080 minutes in 180 degrees +2160 is number of minutes in 360 degrees +This uses the type of the first angle. Assumes both are the same +******************************************************************************/ +float gps_angle_diff(const GPS_ANGLE* gps1, const GPS_ANGLE* gps2) +{ + float result; + GPS_ANGLE diff; + diff.type = gps1->type; + diff.minutes = gps2->minutes - gps1->minutes; + diff.frac = gps2->frac - gps1->frac; + gps_angle_normalize(&diff); + result = (float)diff.minutes + diff.frac; + result /= 60.f; // convert to degrees + return result; +} // end gps_angle_diff + +/****************************************************************************** +FUNCTION: gps_angle_add_increment +Adds a float in degrees to GPS_ANGLE +returns error if |lat| >= 90 +******************************************************************************/ +int gps_angle_add_increment(const GPS_ANGLE* gps, // [in] gps coordinate + float diff, // [in] small increment in degrees + GPS_ANGLE* result) // [out] gps + increment +{ + int err = 0; + *result = *gps; // copy input to output including type + float diff_minutes = diff * 60; // convert to minutes + result->frac = gps->frac + diff_minutes; + gps_angle_normalize(result); + return err; +} // end gps_angle_add_increment + +/***************************************************************************** +FUNCTION: get_lonlat() +convert string to gps_angle +string has DDDMM.MMMMMM +DD or DDDis degrees , MM.MMMMM is minutes +Convert ot DDDMM minutes and .MMMMMM for fractional part +*****************************************************************************/ +int get_lonlat(char* str, GPS_ANGLE* lonlat) +{ + int err = 0; + char buf[20]; + char* end_ptr; + size_t len = strlen(str); + if (len >= sizeof(buf)-1) + { + return -1; + } + strcpy(buf, str); + char* p = strchr(buf, '.'); + int first_part; + float second_part; + + if (p == NULL) + { + first_part = strtod(buf, &end_ptr); + second_part = 0.f; + } + else + { + *p = 0; // replace . with 0 + first_part = atoi(buf); // convert as integer + *p = '.'; // replace . + second_part = strtod(p, &end_ptr); + } + int degrees = first_part / 100; + int minutes = first_part - degrees * 100; + lonlat->minutes = minutes + degrees * 60; + lonlat->frac = second_part; + gps_angle_normalize(lonlat); + return err; + +}// end get_lonlat + + +/***************************************************************************** +FUNCTION: get_longitude() +convert longitude string to gps_angle +string has DDDMM.MMMMMM +DD is degrees longitude, MM.MMMMM is minutes +Convert ot DDDMM minutes and .MMMMMM for fractional part +set LONGITUDE_TYP +*****************************************************************************/ +int get_longitude(char* str, GPS_ANGLE* longitude) +{ + int err = 0; + longitude->type = LONGITUDE_TYPE; + err = get_lonlat(str, longitude); + return err; +} // end get longitude + +/***************************************************************************** +FUNCTION: get_latitude() +convert latitude string to gps_angle +string has DDMM.MMMMMM +DD is degrees latitude, MM.MMMMM is minutes +Convert ot DDMM minutes and .MMMMMM for fractional part +set LATITUDE_TYPE +*****************************************************************************/ +int get_latitude(char* str, GPS_ANGLE* latitude) +{ + int err = 0; + latitude->type = LATITUDE_TYPE; + err = get_lonlat(str, latitude); + return err; +} //end get_latitude + +/****************************************************************************** +FUNCTION: lonlat_to_str() +convert longitutude or latitude from GPS_ANGLE to string +latitude: DDMM.MMMMM N/S +longitude: DDDMM.MMMMM E/W + +return 0 if ok +possible errors, +GPS_ANGLE_ERR_TOO_LONG - buffer not long enough +GPS_ANGLE_ERR_RANGE - input angle not in proper range +DD = decimal degrees DD.DDDDDDDD (signed) +DMM = decimal degrees and decimal minutes DD MM.MMMMMMMM (space between, DD has sign) +DMS = degrees, minutes, and seconds direction, space betwen lat and long +******************************************************************************/ +int lonlat_to_str(const GPS_ANGLE* lonlat, // [in] angle to convert + char* str, // [in] buffer to place string + int len, // [in] length of buffer including null + int num_dec, // [in] number of digits after decimal place + const char* fmt) // [in] one of "DD", "DMM", or "DMS" +{ + int err = 0; + if (lonlat->type != LONGITUDE_TYPE && lonlat->type != LATITUDE_TYPE) + { + return GPS_ANGLE_ERR_BAD_TYPE; + } + char dir; + char delim = ','; + int neg = 0; + float abs_frac = lonlat->frac; + int abs_minutes = lonlat->minutes; + int deg_digits; + if (abs_minutes < 0) + { + abs_minutes = -abs_minutes; + abs_frac = -abs_frac; + neg = 1; + } + int degrees = abs_minutes / 60; // truncate + abs_minutes -= (degrees * 60); + float angle = gps_angle_to_float(lonlat); + if (lonlat->type == LONGITUDE_TYPE) + { + // DDDMM.MMMMM,N/M + int max_len = num_dec + 9; + if (len < max_len) + { + return GPS_ANGLE_ERR_TOO_LONG; + } + if (angle >180.f || angle <= -180.f) + { + return GPS_ANGLE_ERR_BAD_RANGE; + } + if (neg) + { + dir = 'W'; + } + else + { + dir = 'E'; + } + deg_digits = 3; + } + else // must be LATITUDE_TYPE + { + // DDMM.MMMMM,N/S + int max_len = num_dec + 8; + if (len < max_len) + { + return GPS_ANGLE_ERR_TOO_LONG; + } + if (angle > 90.f || angle < -90.f) + { + return GPS_ANGLE_ERR_BAD_RANGE; + } + if (neg) + { + dir = 'S'; + } + else + { + dir = 'N'; + } + deg_digits = 2; + } + // angles are in range and the buffer is big enough. + // create a format string + if (strcmp(fmt, "DMM") == 0) + { + char fmt_deg[10]; + char* ptr = str; + ptr += sprintf(ptr, "%d", degrees); // don't print leading zero + ptr += sprintf(ptr, "%02d", abs_minutes); + err = ftoa_frac(abs_frac, ptr, len - (ptr-str), num_dec); + if (ptr - str > len) + { + err = GPS_ANGLE_ERR_TOO_LONG; + } + } // end if "DMM" + else if (strcmp(fmt, "DMS") == 0) + { + char* ptr = str; + ptr += sprintf(ptr, "DMS"); + }// end if DMS + else if (strcmp(fmt, "DD") == 0) + { + char* ptr = str; + if (neg) + { + ptr += sprintf(ptr, "-"); + } + ptr += sprintf(ptr, "%d", degrees); + err = ftoa_minutes_to_deg_str(abs_minutes, abs_frac, ptr, len - (ptr-str)); +#if DEBUG_PRINT + if (err != 0) + { + printf("ftoa_minutes_to_deg_str return err = %d\n", err); + } +#endif + } // end if "DD" + return err; +} // end lonlat_to_str + +/****************************************************************************** +FUNCTION: fdeg_to_rad() +Convert gps coordinates in degrees to normalized radians using float + +******************************************************************************/ +float fdeg_to_rad(float degrees) +{ + float radians = (float)M_PI * degrees / 180.f; + return radians; +} + +/****************************************************************************** +FUNCTION: frad_to_deg() +Convert radians to degrees with float type + +******************************************************************************/ +float frad_to_deg(float radians) +{ + float deg = 180.f* radians / M_PI; + return deg; +} + diff --git a/src/math/gps_angle.h b/src/math/gps_angle.h new file mode 100644 index 0000000..1b22750 --- /dev/null +++ b/src/math/gps_angle.h @@ -0,0 +1,49 @@ +/***************************************************************************** +FILE: gps_angle.h +This defines structures and functions for handling precision angles with +floats +*****************************************************************************/ +#if !defined (GPS_ANGLE_H) +#define GPS_ANGLE_H + +#include +#define LATITUDE_TYPE 0 // -90 < lat < 90, error if not in range +#define LONGITUDE_TYPE 1 // -180 < long <= 180, modulo 360 +#define GPS_ANGLE_ERR_TOO_LONG (-1) +#define GPS_ANGLE_ERR_BAD_RANGE (-2) +#define GPS_ANGLE_ERR_BAD_TYPE (-3) +#define GPS_ANGLE_ERR_BAD_CONVERSION (-4) +typedef struct +{ + int minutes; // minutes of arc + float frac; // fraction of minute of arc + char type; // LATITUDE_TYPE or LONGITUDE_TYPE +} GPS_ANGLE; + +int get_lonlat(char* str, GPS_ANGLE* lonlat); // convert string to GPS_ANGLE +int lonlat_to_str(const GPS_ANGLE* lonlat, // [in] angle to convert + char* str, // [in] buffer to place string + int len, // [in] length of buffer including null + int num_dec, // [in] number of digits after decimal place + const char* fmt); // [in] one of "DD", "DMM", or "DMS" +int get_longitude(char* str, GPS_ANGLE* longitude); // convert string longitude to GPS_ANGLE +int get_latitude(char* str, GPS_ANGLE* latitude); // convert string latitude to GPS_ANGLE + +float gps_angle_to_float(const GPS_ANGLE* angle); // angle in degrees, loses some precision +void float_to_gps_angle(float degrees, GPS_ANGLE* gps); // loses precision +float gps_angle_diff(const GPS_ANGLE* gps1, const GPS_ANGLE* gps2); // difference in degrees (gps2 - gps1) +int gps_angle_add_increment(const GPS_ANGLE* gps, // [in] input gps coordinate + float inc, // [in] small increment in degrees + GPS_ANGLE* result); // [out] gps + increment +void gps_angle_normalize(GPS_ANGLE* angle); // adjust frac and minutes so -1.f < frac < 1.f +void gps_angle_negate(GPS_ANGLE* angle); // change sign of GPS_ANGLE +void gps_angle_apply_sign(GPS_ANGLE* angle, char dir); // apply E|W to longitude, N|S to latitude + +// rad and degree conversions +float fdeg_to_rad(float degrees); +float frad_to_deg(float radians); + +#endif + + + diff --git a/src/math/gps_print.cpp b/src/math/gps_print.cpp new file mode 100644 index 0000000..e630a6e --- /dev/null +++ b/src/math/gps_print.cpp @@ -0,0 +1,98 @@ +/****************************************************************************** +FiLE: gps_print.c +Functions for debug printing +******************************************************************************/ + + +#include "gps_print.h" +#include "floatgps.h" +#include "gps_angle.h" + +// some debugging functions +void print_gps(HardwareSerial *outputSerial, const char* title, GPS_COORD* gps) +{ + outputSerial->print("GPS: "); + outputSerial->println(title); + + outputSerial->print("latitude: Minutes = "); + outputSerial->print(gps->latitude.minutes); + outputSerial->print(", frac = "); + outputSerial->print(gps->latitude.frac,9); + outputSerial->print(", Type = "); + outputSerial->println(gps->latitude.type,9); + + + outputSerial->print("longitude: Minutes = "); + outputSerial->print(gps->longitude.minutes); + outputSerial->print(", frac = "); + outputSerial->print(gps->longitude.frac,9); + outputSerial->print(", Type = "); + outputSerial->println(gps->longitude.type,9); + + + // printf(" latitude: Minutes = %d, frac = %.9e\n", + // gps->latitude.minutes, gps->latitude.frac); + // printf(" longitude: Minutes = %d, frac = %.9e\n", + // gps->longitude.minutes, gps->longitude.frac); + // printf("latitude type = %d, longitude type = %d\n", + // gps->latitude.type, gps->longitude.type); +} + +void print_local(HardwareSerial *outputSerial, char* title, LOCAL_COORD* local) +{ + outputSerial->print("LOCAL: "); + outputSerial->println(title); + outputSerial->print(" distance = "); + outputSerial->print(local->distance,9); + outputSerial->println("m"); + outputSerial->print(" heading = "); + outputSerial->print(local->heading,9); + outputSerial->println("degrees"); + + //printf("\nLOCAL: %s\n", title); + //printf(" distance = %.9e m\n", local->distance); + //printf(" heading = %.9e degrees\n", local->heading); +} + +void print_delta(HardwareSerial *outputSerial, char* title, DELTA_GPS* delta_gps) +{ + outputSerial->print("DELTA_GPS: "); + outputSerial->println(title); + outputSerial->print(" delta lat = "); + outputSerial->print(delta_gps->latitude,9); + outputSerial->println("degrees"); + outputSerial->print(" delta long = "); + outputSerial->print(delta_gps->longitude,9); + outputSerial->println("degrees"); + + + // printf("\nDELTA GPS: %s\n", title); + // printf(" delta lat = %.9e degrees\n", delta_gps->latitude); + // printf(" delta long = %.9e degrees\n", delta_gps->longitude); +} + +/****************************************************************************** +FUNCTION: print_gps_angle() +debug print routine for GPS_ANGLE +Displays title on first line +Displays values following +******************************************************************************/ +void print_gps_angle(HardwareSerial *outputSerial, char* title, GPS_ANGLE* angle) +{ + outputSerial->print("GPS_ANGLE: "); + outputSerial->println(title); + outputSerial->print(" type = "); + outputSerial->println(angle->type); + outputSerial->print(" minutes = "); + outputSerial->println(angle->minutes); + outputSerial->print(" frac = "); + outputSerial->println(angle->frac,9); + + // printf("\nGPS_ANGLE: %s\n", title); + // printf(" type = %d, minutes = %d, frac = %.9e\n", + // angle->type, angle->minutes, angle->frac); +} + + + + diff --git a/src/math/gps_print.h b/src/math/gps_print.h new file mode 100644 index 0000000..8b8caf2 --- /dev/null +++ b/src/math/gps_print.h @@ -0,0 +1,21 @@ +/****************************************************************************** +FILE: gps_print.h +auxilliarly files to print things +******************************************************************************/ +#ifndef GPS_PRINT_H +#define GPS_PRINT_H + +#include "Arduino.h" + +#include "floatgps.h" +#include "gps_angle.h" + +// some debugging functions +void print_gps(HardwareSerial *outputSerial, const char* title, GPS_COORD* gps); +void print_local(HardwareSerial *outputSerial, char* title, LOCAL_COORD* local); +void print_delta(HardwareSerial *outputSerial, char* title, DELTA_GPS* delta_gps); + +void print_gps_angle(HardwareSerial *outputSerial, char* title, GPS_ANGLE* angle); // debug print of GPS_ANGLE + +#endif + diff --git a/src/storage/EEPROMstorage.h b/src/storage/EEPROMstorage.h index 63c0861..7ef8219 100644 --- a/src/storage/EEPROMstorage.h +++ b/src/storage/EEPROMstorage.h @@ -11,14 +11,17 @@ should call callback immediatly upon attachment */ -class eeStorage : public Storage { +class eeStorage : public Storage +{ private: static eeStorage* m_instance; void (*callback[NUM_STORED_RECORDS])(EE_STORAGE_TYPE); eeStorage(); public: - static eeStorage* getInstance(){ - if(m_instance == NULL) m_instance = new eeStorage(); + static eeStorage* getInstance() + { + if(m_instance == NULL) + m_instance = new eeStorage(); return m_instance; } void attachCallback(uint8_t dataNum, void (*call)(EE_STORAGE_TYPE)); @@ -27,25 +30,35 @@ class eeStorage : public Storage { }; eeStorage* eeStorage::m_instance = NULL; -eeStorage::eeStorage(){ +eeStorage::eeStorage() +{ eeprom::setup(); - for(int i=0; i= NUM_STORED_RECORDS) return; + +void eeStorage::attachCallback(uint8_t dataNum, void (*call)(EE_STORAGE_TYPE)) +{ + if(dataNum >= NUM_STORED_RECORDS) + return; callback[dataNum] = call; - if(call != NULL) call(getRecord(dataNum)); + if(call != NULL) + call(getRecord(dataNum)); } -void -eeStorage::updateRecord(uint8_t dataNum, EE_STORAGE_TYPE value){ - if(dataNum >= NUM_STORED_RECORDS) return; + +void eeStorage::updateRecord(uint8_t dataNum, EE_STORAGE_TYPE value) +{ + if(dataNum >= NUM_STORED_RECORDS) + return; eeprom::writeFloat(EEaddrStart+4*dataNum, value); - if(callback[dataNum] != NULL) callback[dataNum](value); + if(callback[dataNum] != NULL) + callback[dataNum](value); } -EE_STORAGE_TYPE -eeStorage::getRecord(uint8_t dataNum){ - if(dataNum >= NUM_STORED_RECORDS) return 0.f; + +EE_STORAGE_TYPE eeStorage::getRecord(uint8_t dataNum) +{ + if(dataNum >= NUM_STORED_RECORDS) + return 0.f; return eeprom::readFloat(EEaddrStart+4*dataNum); } diff --git a/src/util/PIDcontroller.h b/src/util/PIDcontroller.h index 11e97ba..239221a 100644 --- a/src/util/PIDcontroller.h +++ b/src/util/PIDcontroller.h @@ -18,7 +18,9 @@ class PIDcontroller{ void clearAccumulator(){ pid.clearAccumulator(); } void train(float out){ pid.train(out); } void set(float input){ pid.set(input); } + float get() { return pid.get(); } void stop() { pid.stop(); } + boolean isStopped() { pid.isStopped(); } float update(float current){ uint32_t time = micros(); diff --git a/src/util/PIDexternaltime.h b/src/util/PIDexternaltime.h index 44ae156..592f112 100644 --- a/src/util/PIDexternaltime.h +++ b/src/util/PIDexternaltime.h @@ -27,6 +27,12 @@ class PIDexternaltime{ setPoint = input; stopped = false; } + + float get() + { + return setPoint; + } + /** * Stop the pid controller. This clears the accumulator. * It will output 0 until the next time `set` is called @@ -35,6 +41,12 @@ class PIDexternaltime{ clearAccumulator(); stopped = true; } + + /** + * Return if pid is stopped + */ + boolean isStopped() { return stopped; } + /** * Update the PID controller * current - the current process value