Agra-GPS
/
FreePilot_V2-3
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: FreePilot PinDetect mbed-src
Fork of
FreePilot_V2-2
by 
Agra-GPS
Diff: main.cpp
- Revision:
- 35:f9caeb8ca31e
- Parent:
- 34:c2bc9f9be7ff
- Child:
- 36:8e84b5ade03e
--- a/main.cpp Fri Feb 13 17:22:53 2015 +0000
+++ b/main.cpp Sat Feb 21 13:47:37 2015 +0000
@@ -10,17 +10,16 @@
#include "atoh.h"
#include "checksum.h"
#include <string.h>
-//#include "autosteer.h"
-//#include "conversion.h"
+
#define dot(u,v) ((u).GetX() * (v).GetX()+ (u).GetY() * (v).GetY())
#define norm(v) sqrt(dot(v,v)) // norm = length of vector
#define d(u,v) norm(point_sub(u,v)) // distance = norm of difference
-
+
char *version="FreePilot V2.11 Jtan 20, 2015\r\n";
long lastsend_version=0;
Timer vTimer; //this timer is int based! Max is 30 minutes
-
+
int checksumm;
double distance_from_line;
double cm_per_deg_lon;
@@ -33,36 +32,36 @@
Ticker gyroscopeTicker;
Ticker filterTicker;
Ticker angle_print;
-
+
//Motor
PinDetect motor_switch(p16); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
DigitalOut enable_motor(p7);
-
+
PwmOut pwm1(p22);
-PwmOut pwm2(p21);
-
+PwmOut pwm2(p23);
+
//equipment switches
PinDetect boom1(p20,PullUp); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
PinDetect boom2(p19); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
PinDetect boom3(p18); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
PinDetect boom4(p17); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
-
+
char boom18; //1 byte
char lastboom18; //1 byte
-char boomstate[8]={'$','F','B','S',0,13,10,0 };
-
- double filterg = 100;
+char boomstate[8]= {'$','F','B','S',0,13,10,0 };
+
+double filterg = 100;
Point position;
Point looked_ahead;
Point line_start;
Point line_end;
Point tilt_compensated_position;
Point yaw_compensated_position;
-
+
extern int gyro_pos;
-
+
double distance_to_line;
-
+
//FreePilot variables
int timer_enabled;
double motorspeed;
@@ -76,17 +75,17 @@
Timer motTimer; //this timer is int based! Max is 30 minutes
Timer btTimer; //measure time for Bluetooth communication
long lastgetBT=0;
-
+
int msg2_changed = 1;
char* buffer;
double meter_lat = 0;
double meter_lon = 0;
-
+
char msg[256]; //GPS line buffer
char msg2[256];//PC line buffer
int printing;
int num_of_gps_sats;
-
+
double decimal_lon;
float longitude;
float latitude;
@@ -113,7 +112,7 @@
double velocity; // speed in knot
int connect_time = 10000; //variable to change the time that the serial output all the strings in order to verify if the command was right.
int connecting = 0; //are we still in phase of connecting? based on the connect_time value.
-
+
int angle_send = 0;
int correct_rmc = 1;
double m_lat = 0;
@@ -126,135 +125,125 @@
int active_AB = 0;
double compensation_vector;
char output[256];
-
+
double yaw;
double pitch;
double roll;
-
+
double a_x;
double a_y;
double a_z;
double w_x;
double w_y;
double w_z;
-
+
int readings[3];
-
+
double Freepilot_lat;
double Freepilot_lon;
double Freepilot_lat1;
double Freepilot_lon1;
double Freepilot_bearing;
-
+
volatile bool newline_detected = false;
-
+
Point point_add(Point a, Point b)
{
return Point(a.GetX() + b.GetX(), a.GetY() + b.GetY());
}
-
+
Point point_sub(Point a , Point b)
{
return Point(a.GetX() - b.GetX(), a.GetY() - b.GetY());
}
-
+
double get_yaw()
{
- double yaw_angle = imuFilter.getYaw() * -1 ; // *-1 to reverse polarity as gyro is up-side-down
+ double yaw_angle = imuFilter.getYaw() * -1 ; // *-1 to reverse polarity as gyro is up-side-down
return yaw_angle;
}
-
+
void update_motor()
{
-
+
}
-
+
double get_roll()
{
double roll_angle = 0;
- if ( gyro_pos == 0 )
- {
+ if ( gyro_pos == 0 ) {
roll_angle = imuFilter.getRoll();
- }
- else if ( gyro_pos == 1 )
- {
+ } else if ( gyro_pos == 1 ) {
roll_angle = imuFilter.getRoll() * -1;
- }
- else if( gyro_pos == 2 )
- {
+ } else if( gyro_pos == 2 ) {
roll_angle = imuFilter.getPitch() * -1;
- }
- else if ( gyro_pos == 3 )
- {
+ } else if ( gyro_pos == 3 ) {
roll_angle = imuFilter.getPitch();
}
return roll_angle;
}
-
+
double get_pitch()
{
double pitch_angle = 0;
- if ( gyro_pos == 0 )
- {
- pitch_angle = imuFilter.getPitch();
- }
- else if ( gyro_pos == 1 )
- {
+ if ( gyro_pos == 0 ) {
+ pitch_angle = imuFilter.getPitch();
+ } else if ( gyro_pos == 1 ) {
pitch_angle = imuFilter.getPitch() * -1;
- }
- else if( gyro_pos == 2 )
- {
+ } else if( gyro_pos == 2 ) {
pitch_angle = imuFilter.getRoll();
- }
- else if ( gyro_pos == 3 )
- {
- pitch_angle = imuFilter.getRoll() * -1;
- }
+ } else if ( gyro_pos == 3 ) {
+ pitch_angle = imuFilter.getRoll() * -1;
+ }
return pitch_angle;
}
-
+
double dist_Point_to_Line( Point P, Point line_start, Point line_end)
{
- Point v = point_sub(line_end,line_start);
- Point w = point_sub(P,line_start);
-
- double c1 = dot(w,v);
- double c2 = dot(v,v);
- double b = c1 / c2;
-
- Point resulting(b * v.GetX(),b*v.GetY());
- Point Pb = point_add(line_start, resulting);
- return d(P, Pb);
+ Point v = point_sub(line_end,line_start);
+ Point w = point_sub(P,line_start);
+
+ double c1 = dot(w,v);
+ double c2 = dot(v,v);
+ double b = c1 / c2;
+
+ Point resulting(b * v.GetX(),b*v.GetY());
+ Point Pb = point_add(line_start, resulting);
+ return d(P, Pb);
}
-
+
double lat_to_deg(char *s, char north_south)
{
int deg, min, sec;
double fsec, val;
-
+
deg = ( (s[0] - '0') * 10) + s[1] - '0';
min = ( (s[2] - '0') * 10) + s[3] - '0';
sec = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0'));
fsec = (double)((double)sec /10000.0);
val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
- if (north_south == 'S')
- {
+ if (north_south == 'S') {
val *= -1.0;
}
return val;
}
-
+
// isLeft(): test if a point is Left|On|Right of an infinite 2D line.
// Input: three points P0, P1, and P2
// Return: >0 for P2 left of the line through P0 to P1
// =0 for P2 on the line
// <0 for P2 right of the line
-double isLeft( Point P0, Point P1, Point P2 )
+int isLeft( Point P0, Point P1, Point P2 )
{
- return ( (P1.GetY() - P0.GetY()) * (P2.GetX() - P0.GetX())
- - (P2.GetY() - P0.GetY()) * (P1.GetX() - P0.GetX()));
+ double isleft = ( (P1.GetY() - P0.GetY()) * (P2.GetX() - P0.GetX()) - (P2.GetY() - P0.GetY()) * (P1.GetX() - P0.GetX()));
+ if ( isleft > 0 ) {
+ isleft = 1;
+ } else {
+ isleft = -1;
+ }
+ return (int)isleft;
}
-
+
double lon_to_deg(char *s, char east_west)
{
int deg, min, sec;
@@ -264,13 +253,12 @@
sec = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0'));
fsec = (double)((double)sec /10000.0);
val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
- if (east_west == 'W')
- {
+ if (east_west == 'W') {
val *= -1.0;
}
return val;
}
-
+
void nmea_gga(char *s)
{
char *token;
@@ -282,12 +270,10 @@
char *qual = (char *)NULL;
char *altitude = (char *)NULL;
char *sats = (char *)NULL;
-
+
token = strtok(s, ",");
- while (token)
- {
- switch (token_counter)
- {
+ while (token) {
+ switch (token_counter) {
case 2:
latitude = token;
break;
@@ -313,20 +299,23 @@
token = strtok((char *)NULL, ",");
token_counter++;
}
-
- if (latitude && longitude && altitude && sats)
- {
+
+ if (latitude && longitude && altitude && sats) {
decimal_latitude = lat_to_deg(latitude, lat_dir[0]);
decimal_lon = lon_to_deg(longitude, lon_dir[0]);
num_of_gps_sats = atoi(sats);
gps_satellite_quality = atoi(qual);
- }
- else
- {
+ } else {
gps_satellite_quality = 0;
}
}
-
+
+void autosteer_done()
+{
+ //kill the autosteer once the timeout reech
+ enable_motor = 0;
+}
+
//from farmerGPS code
void get_latlon_byangle(double lat1, double lon1, double distance,double angle, double &lon2, double &lat2)
{
@@ -340,151 +329,19 @@
ydist = sinr * distance;
lat2 = lat1 + (ydist / (69.09 * -1609.344));
lon2 = lon1 - (xdist / (69.09 * 1609.344 * cos(lat1/57.295779513)));
- // return;
+ // return;
}
-
+
Point compensation;
-
+
void yaw_compensate()
{
yaw = get_yaw();
}
-
- float mX[2];
-float mY[2];
-float mZ[2];
-long _speedArr[10];
-long _delSpeed[2];
-//==================
-
-//Filters===========
-float BA;
-float BB;
-float AA;
-//==================
-double _dist = 0;
-double m_PI = 3.14159265359;
-double _lookA;
-double j_scale;
-double j_fGain;
-
-bool _acq = false;
-
-int autoGuide_A(double _gSpeed, double _distErr, int _acq,double j_phaseAdv, double j_tCenter, double lookA, double _scale, double _j_fGain)
-{
- float T1T2r = (1 + sin(j_phaseAdv * m_PI / 180)) / (1 - sin(j_phaseAdv * m_PI / 180));
- float T1 = sqrt(T1T2r) * j_tCenter;
- float T2 = T1 / T1T2r;
-
- float K = (1 + 2 * T1 / 0.2);
- float L = (1 - 2 * T1 / 0.2);
- float M = (1 + 2 * T2 / 0.2);
- float N = (1 - 2 * T2 / 0.2);
-
- BA = K / M;
- BB = L / M;
- AA = N / M;
-
- _lookA = lookA;
- j_scale = _scale;
- j_fGain = _j_fGain;
-
- //int send = 0;
- mX[1] = mX[0];
- mX[0] = _distErr * j_fGain;
- mY[1] = mY[0];
- mY[0] = -AA * mY[1] + BA * mX[0] + BB * mX[1];
- float y = mY[0] * j_scale;
- y = y * (1 + _gSpeed / 100);
- y = 127 + y;
- if(_acq && y > 117 && y < 137)
- {
- if(_distErr > 0 )
- {
- y += 10;
- }
- else
- {
- y -= 10;
- }
- }
-
- if(y > 255)
- {
- y = 255;
- }
- if(y < 0)
- {
- y = 0;
- }
- return y;//j_guidance = y;
-}
-
-/*int LookAhead( double _gSpeed, double _distErr, int _LR, double _headErr)
-{
- double temp = _distErr;
- temp = abs(temp);
- _headErr = 90-_headErr;
- // double _headErr2 = 90 - _headErr;
- //=========================================
-
- double _abDist = _distErr;
- _acq = false;
- temp = abs(_abDist);
- if(temp > 0.2)
- {
- _acq = true;
- }
- if(_acq)
- {
- if(_abDist < 0.15 && _abDist > -0.15)// && _headErr2 > 89.2)
- {
- _acq = false;
- }
- }
- //set_mode(_headErr,_distErr);
- double ld = 0; //xvalue of look ahead point
- double dist = 0;
- double val = 0;
- dist = _gSpeed * _lookA; //Speed * look ahead time.
- val = (_headErr / 180) * m_PI;
- val = cos(val);
- ld = dist * val;
- if(_LR == 0)
- {
- _distErr = _distErr - ld;
- }
- else if(_LR == 1)
- {
- _distErr = _distErr + ld;
- }
- return autoGuide_A(_gSpeed,_distErr,_acq);
-}
-*/
-void SetFilter(double j_phaseAdv, double j_tCenter, double lookA, double _scale, double _j_fGain)
-{
- float T1T2r = (1 + sin(j_phaseAdv * m_PI / 180)) / (1 - sin(j_phaseAdv * m_PI / 180));
- float T1 = sqrt(T1T2r) * j_tCenter;
- float T2 = T1 / T1T2r;
-
- float K = (1 + 2 * T1 / 0.2);
- float L = (1 - 2 * T1 / 0.2);
- float M = (1 + 2 * T2 / 0.2);
- float N = (1 - 2 * T2 / 0.2);
-
- BA = K / M;
- BB = L / M;
- AA = N / M;
-
- _lookA = lookA;
- j_scale = _scale;
- j_fGain = _j_fGain;
-}
-
- void pitch_and_roll(double real_bearing)
+void pitch_and_roll(double real_bearing)
{
pitch = get_pitch();
roll = get_roll();
@@ -498,11 +355,9 @@
int token_counter = 0;
char *height = (char *)NULL;
token = strtok(height_string, ",");
- while (token)
- {
-
- switch (token_counter)
- {
+ while (token) {
+
+ switch (token_counter) {
case 1:
height = token;
break;
@@ -510,17 +365,15 @@
token = strtok((char *)NULL, ",");
token_counter++;
}
- if ( height )
- {
+ if ( height ) {
antennaheight = atof(height);
Config_Save();
}
}
-
+char dms[128];
char* To_DMS(double dec_deg)
{
- char dms[128];
dec_deg = abs(dec_deg);
int d = (int)(dec_deg);
sprintf(dms,"%0.2i\0",d);
@@ -533,9 +386,30 @@
return dms;
}
+
+/*string To_DMS_lon(double dec_deg)
+{
+ dms = "";
+ dec_deg = abs(dec_deg);
+ int d = (int)(dec_deg);
+ sprintf(dms2,"%i",d);
+ dms = string(dms2);
+ double m = (double)(((double)dec_deg - (double)d) * 60.0);
+ sprintf(dms2,"%0.9f",m);
+ if ( m < 10 ) {
+ dms += ("00" + string(dms2));
+ } else if ( m < 100 ) {
+ dms += ("0" + string(dms2));
+ } else {
+ dms += string(dms2);
+ }
+ //sprintf(dms,"%03d%09.7f\0",d,m);
+ return dms;
+}*/
+
+
char* To_DMS_lon(double dec_deg)
{
- char dms[128];
dec_deg = abs(dec_deg);
int d = (int)(dec_deg);
sprintf(dms,"%0.3i\0",d);
@@ -548,10 +422,109 @@
return dms;
}
- char rmc_cpy[256];
+//sets pwm1 and pwm2 and enable_motor
+void process_ASTEER(char* asteer)
+{
+ char *token;
+ int token_counter = 0;
+ char *asteer_speed = (char *)NULL;
+ char *asteer_time = (char *)NULL;
+ token = strtok(asteer, ",");
+ while (token) {
+ switch (token_counter) {
+ case 1:
+ asteer_speed = token;
+ break;
+ case 2:
+ asteer_time = token;
+ break;
+ }
+ token = strtok((char *)NULL, ",");
+ token_counter++;
+ }
+ if ( asteer_speed && asteer_time ) {
+ motorspeed = atof(asteer_speed);
+ enable_time = atof(asteer_time);
+ autosteer_timeout.attach_us(autosteer_done,(double)enable_time * (double)1000.0);
+ if ( motorspeed > 127.0 ) {
+ pwm2_speed = 0.0;
+ pwm1_speed = ((double)motorspeed - (double)127.0) / 127.0;
+ enable_motor = 1;
+ } else if ( motorspeed < 127.0 ) {
+ pwm2_speed = ( ((double) 127-(double)motorspeed) / 127.0 );
+ pwm1_speed = 0.0;
+ enable_motor = 1;
+ } else {
+ pwm1_speed = 0;
+ pwm2_speed = 0;
+ enable_motor = 0;
+ }
+ // if(Authenticated)
+ // {
+ pwm1 = pwm1_speed;
+ pwm2 = pwm2_speed;
+ // }
+ //else
+ //{
+ // sprintf(output,"$NOT AUTHENTICATED TO STEER %f %d %f %f",motorspeed,enable_time,pwm1_speed,pwm2_speed);
+ // pc.puts(output);
+ // bluetooth.puts(output);
+ // }
+ }
+}
+
+//gets the motor value between -255 and 255 (- means left, positive means right)
+//distance in meters, always positive
+//angle in degrees
+//Points in lat/lon format
+int freepilot(Point current, Point target, double heading_err, double dist_line, double scale, double filter_g, double phase_adv)//dist in meters
+{
+ double error = 0;
+ // int motor_pwm = 0;
+
+ int position_sign = isLeft( line_start, line_end, current);
+ int forward_sign = isLeft(line_start,line_end, target);
+
+ dist_line = dist_line * filter_g;
+ heading_err = heading_err * phase_adv;
+ if ( heading_err > 45 ) {
+ heading_err = 45;
+ }
+
+ if ( position_sign == forward_sign ) {
+ error = dist_line + heading_err;
+ } else {
+ error = dist_line - heading_err;
+ }
+
+ if (position_sign == forward_sign && forward_sign == -1 ) {
+ error = error * -1;
+ } else if (position_sign == forward_sign && forward_sign == 1 ) {
+ error = error;
+ } else if(position_sign != forward_sign && forward_sign == 1) {
+ error = (error / 2);
+ } else if ( position_sign != forward_sign && forward_sign == -1) {
+ error = (error / 2) * -1;
+ }
+
+ error = error * scale;
+
+ if ( error > 255 ) {
+ error = 255;
+ } else if ( error < -255 ) {
+ error = -255;
+ }
+
+ error = error + 255;
+ error = (int)(error / 2);
+
+ return (int)error;
+}
+
+char rmc_cpy[256];
void nmea_rmc(char *s)
{
- // strcpy(rmc_cpy, s);
+ // strcpy(rmc_cpy, s);
char *token;
int token_counter = 0;
char *time = (char *)NULL;
@@ -560,18 +533,16 @@
char *vel = (char *)NULL;
char *trk = (char *)NULL;
char *magv = (char *)NULL;
- char *magd = (char *)NULL;
+ // char *magd = (char *)NULL;
char *latitude = (char *)NULL;
char *longitude = (char *)NULL;
char *lat_dir = (char *)NULL;
char *lon_dir = (char *)NULL;
- char magdd = 'E';
-
+ // char magdd = 'E';
+
token = strtok(s, ",*");
- while (token)
- {
- switch (token_counter)
- {
+ while (token) {
+ switch (token_counter) {
case 1:
time = token;
break;
@@ -606,8 +577,7 @@
token = strtok((char *)NULL, ",");
token_counter++;
}
- if (stat && date && time)
- {
+ if (stat && date && time) {
hour = (char)((time[0] - '0') * 10) + (time[1] - '0');
minute = (char)((time[2] - '0') * 10) + (time[3] - '0');
second = (char)((time[4] - '0') * 10) + (time[5] - '0');
@@ -620,46 +590,36 @@
speed_m_s = speed_km * 3600.0 / 1000.0;
track = atof(trk);
magvar = atof(magv);
- // magvar_dir = magd[0];
+ // magvar_dir = magd[0];
}
- position.SetX(lat_to_deg(latitude, lat_dir[0]));
- position.SetY(lon_to_deg(longitude, lon_dir[0]));
- cm_per_deg_lat = 11054000;
- cm_per_deg_lon = 11132000 * cos(decimal_latitude);
-
- pitch_and_roll((track-90)*-1);// as to be real ebaring
-
- compensation.SetY(compensation.GetY() / cm_per_deg_lon);
- compensation.SetX(compensation.GetX() / cm_per_deg_lat);
-
- position = point_add(position,compensation);
-
- double lookaheaddistance = lookaheadtime * speed_m_s;
- get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,(track-90)*-1,look_ahead_lon,look_ahead_lat);
- looked_ahead.SetX(look_ahead_lat);
- looked_ahead.SetY(look_ahead_lon);
- double filtering = sqrt(111111.0*111111.0 + 111111.0 * cos(decimal_latitude/57.295779513)*111111.0 * cos(decimal_latitude/57.295779513));
- distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end);
- double sign = isLeft( line_start, line_end, looked_ahead);
-if ( sign < 0 )
-{
- distance_to_line = distance_to_line;
- sign = 0;
-} else if ( sign > 0 )
-{
- distance_to_line = -distance_to_line;
- sign = 1;
-}
-
- double ErrAngle = asin(abs(distance_to_line * filtering)/(sqrt(lookaheaddistance*lookaheaddistance + abs(distance_to_line * filtering*distance_to_line * filtering))))*57.295779513;
-
- //= acos( 1.0/((0.5 * abs(distance_to_line)/(lookaheaddistance * filtering) )))*57.295779513;
- // int LookAhead(double _gSpeed, double _distErr, int _LR, double _headErr);
- // SetFilter(phaseadv, tcenter, lookaheadtime, scale,filterg);
- // int motor_val = LookAhead( speed_m_s, abs(distance_to_line), sign, ErrAngle);
- int motor_val = autoGuide_A(speed_m_s, abs(distance_to_line),false,phaseadv, tcenter, lookaheadtime, scale,filterg);
- // pc.printf("$MOTORSPEED,%i ErrAngle:%f\r\n",motor_val,ErrAngle);
- // sprintf(rebuilt_rmc,"$GPRMC,%s,%s,%f,%s,%f,%s,%s,%s,%s,%s,%s*\r\n",time,stat,position.GetX(),lat_dir,position.GetY(), lon_dir,vel,trk,date,magv,magd);
+ position.SetX(lat_to_deg(latitude, lat_dir[0]));
+ position.SetY(lon_to_deg(longitude, lon_dir[0]));
+ cm_per_deg_lat = 11054000;
+ cm_per_deg_lon = 11132000 * cos(decimal_latitude);
+
+ pitch_and_roll((track-90)*-1);// as to be real ebaring
+
+ compensation.SetY(compensation.GetY() / cm_per_deg_lon);
+ compensation.SetX(compensation.GetX() / cm_per_deg_lat);
+
+ position = point_add(position,compensation);
+
+ double lookaheaddistance = lookaheadtime * speed_m_s;
+ get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,(track-90)*-1,look_ahead_lon,look_ahead_lat);
+ looked_ahead.SetX(look_ahead_lat);
+ looked_ahead.SetY(look_ahead_lon);
+ double filtering = sqrt(111111.0*111111.0 + 111111.0 * cos(decimal_latitude/57.295779513)*111111.0 * cos(decimal_latitude/57.295779513));
+ distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end);
+
+ double ErrAngle = asin(abs(distance_to_line * filtering)/(sqrt(lookaheaddistance*lookaheaddistance + abs(distance_to_line * filtering*distance_to_line * filtering))))*57.295779513;
+
+int val = freepilot(position, looked_ahead, ErrAngle, abs(distance_to_line*filtering), scale, filterg,phaseadv);//dist in meters
+
+char command[128];
+sprintf(command,"$ASTEER,%i,%i\r\n",val,200);
+pc.puts(command);
+process_ASTEER(command);
+
string rmc = "";
if(sizeof(time) > 0) {
rmc = "$GPRMC,";
@@ -711,12 +671,13 @@
bluetooth.puts(output);
//pc.puts(output);
- sprintf(output,"$DIST_TO_LINE: % .12f %i %f\r\n",distance_to_line * filtering,motor_val,ErrAngle);
+ sprintf(output,"$DIST_TO_LINE: % .12f\r\n",distance_to_line * filtering);//,motor_val,ErrAngle);
pc.puts(output);
}
-
+
void process_FGPSAB(char* ab)
{
+ //pc.puts(ab);
char *token;
int token_counter = 0;
char *line_lat = (char *)NULL;
@@ -725,10 +686,8 @@
char *line_lon1 = (char *)NULL;
char *bearing = (char *)NULL;
token = strtok(ab, ",");
- while (token)
- {
- switch (token_counter)
- {
+ while (token) {
+ switch (token_counter) {
case 1:
line_lat = token;
break;
@@ -758,17 +717,13 @@
line_end.SetX(Freepilot_lat1);
line_end.SetY(Freepilot_lon1);
active_AB = 1;
-
+
sprintf(output, "$ABLINE:%f , %f, %f, %f\r\n",line_start.GetX(),line_start.GetY(),line_end.GetX(),line_end.GetY());
pc.puts(output);
}
-
-void autosteer_done()
-{
- //kill the autosteer once the timeout reech
- enable_motor = 0;
-}
-
+
+
+
void process_FGPSAUTO(char* FGPSAUTO)
{
//pc.puts(FGPSAUTO);
@@ -781,14 +736,12 @@
char *avg = (char *)NULL;
char *_kp = (char *)NULL;
char *_ki = (char *)NULL;
- char *fg = (char *)NULL;
+ char *fg = (char *)NULL;
char *_kd = (char *)NULL;
token = strtok(FGPSAUTO, ",");
- while (token)
- {
- switch (token_counter)
- {
+ while (token) {
+ switch (token_counter) {
case 1:
phase = token;
break;
@@ -820,146 +773,71 @@
token = strtok((char *)NULL, ",");
token_counter++;
}
- if ( _kp && _ki && _kd )
- {
+ if ( _kp && _ki && _kd ) {
kp = atof(_kp);
ki = atof(_ki);
kd = atof(_kd);
}
- if ( phase && center && scl && avg && ahead )
- {
+ if ( phase && center && scl && avg && ahead ) {
lookaheadtime = atof(ahead);
scale = atof(scl);
phaseadv = atof(phase);
avgpos = atof(avg);
tcenter = atof(center);
filterg = atof(fg);
- // / sprintf(output, "$SETTINGS:%f\r\n", lookaheadtime);
- // pc.puts(output);
- SetFilter(phaseadv, tcenter, lookaheadtime, scale,filterg);
+ // / sprintf(output, "$SETTINGS:%f\r\n", lookaheadtime);
+ // pc.puts(output);
+ //SetFilter(phaseadv, tcenter, lookaheadtime, scale,filterg);
}
}
-
-//sets pwm1 and pwm2 and enable_motor
-void process_ASTEER(char* asteer)
-{
- char *token;
- int token_counter = 0;
- char *asteer_speed = (char *)NULL;
- char *asteer_time = (char *)NULL;
- token = strtok(asteer, ",");
- while (token)
- {
- switch (token_counter)
- {
- case 1:
- asteer_speed = token;
- break;
- case 2:
- asteer_time = token;
- break;
- }
- token = strtok((char *)NULL, ",");
- token_counter++;
- }
- if ( asteer_speed && asteer_time )
- {
- motorspeed = atof(asteer_speed);
- enable_time = atof(asteer_time);
- autosteer_timeout.attach_us(autosteer_done,(double)enable_time * (double)1000.0);
- if ( motorspeed > 127.0 )
- {
- pwm2_speed = 0.0;
- pwm1_speed = ((double)motorspeed - (double)127.0) / 127.0;
- enable_motor = 1;
- }
- else if ( motorspeed < 127.0 )
- {
- pwm2_speed = ( ((double) 127-(double)motorspeed) / 127.0 );
- pwm1_speed = 0.0;
- enable_motor = 1;
- }
- else
- {
- pwm1_speed = 0;
- pwm2_speed = 0;
- enable_motor = 0;
- }
- // if(Authenticated)
- // {
- pwm1 = pwm1_speed;
- pwm2 = pwm2_speed;
- // }
- //else
- //{
- // sprintf(output,"$NOT AUTHENTICATED TO STEER %f %d %f %f",motorspeed,enable_time,pwm1_speed,pwm2_speed);
- // pc.puts(output);
- // bluetooth.puts(output);
- // }
- }
-}
-
+
+
+
void pc_analyse(char* pc_string)
{
- // pc.puts(pc_string);
- if (!strncmp(pc_string, "$ASTEER", 7))
- {
+ // pc.puts(pc_string);
+ if (!strncmp(pc_string, "$ASTEER", 7)) {
process_ASTEER(pc_string);
- }
- else if (!strncmp(pc_string, "$BANY",5))
- {
+ } else if (!strncmp(pc_string, "$BANY",5)) {
_ID = Config_GetID();
Config_Save();
- }
- else if (!strncmp(pc_string, "$GPSBAUD",8))
- {
+ } else if (!strncmp(pc_string, "$GPSBAUD",8)) {
process_GPSBAUD(pc_string);
Config_Save();
sprintf(output,"%s %d\r\n",pc_string,gps_baud);
pc.puts(output);
- }
- else if (!strncmp(pc_string, "$FGPSAUTO",9))
- {
+ } else if (!strncmp(pc_string, "$FGPSAUTO",9)) {
process_FGPSAUTO(pc_string);
sprintf(output,"%s\r\n",pc_string);
bluetooth.puts(output);
Config_Save();
- }
- else if (!strncmp(pc_string, "$FGPS,",6))
- {
- int i=5;
- char c=pc_string[i];
- while (c!=0)
- {
- i++;
- if (i>255) break; //protect msg buffer!
- c=pc_string[i];
- gps.putc(c);
- pc.putc(c);
- }
- }
-
- else if (!strncmp(pc_string, "$GPSHEIGHT",10))
- {
+ } else if (!strncmp(pc_string, "$FGPS,",6)) {
+ int i=5;
+ char c=pc_string[i];
+ while (c!=0) {
+ i++;
+ if (i>255) break; //protect msg buffer!
+ c=pc_string[i];
+ gps.putc(c);
+ pc.putc(c);
+ }
+ }
+
+ else if (!strncmp(pc_string, "$GPSHEIGHT",10)) {
process_GPSHEIGHT(pc_string);
sprintf(output,"%s\r\n",pc_string);
bluetooth.puts(output);
Config_Save();
- }
+ }
- else if (!strncmp(pc_string, "$FGPSAB",7))
- {
+ else if (!strncmp(pc_string, "$FGPSAB",7)) {
process_FGPSAB(pc_string);
- }
- else if (!strncmp(pc_string, "$CALIBRATEACCEL",15))
- {
+ } else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) {
calibrateGyroscope();
calibrateAccelerometer();
Config_Save();
- }
- else if (!strncmp(pc_string, "$POSITION",9))
- {
-
+ } else if (!strncmp(pc_string, "$POSITION",9)) {
+
char* pointer;
char* Data[5];
int index = 0;
@@ -967,66 +845,56 @@
pointer = strtok(pc_string, ",");
if(pointer == NULL)
Data[0] = pc_string;
- while(pointer != NULL)
- {
+ while(pointer != NULL) {
Data[index] = pointer;
pointer = strtok(NULL, ",");
index++;
}
- //int temp_pos =
+ //int temp_pos =
gyro_pos = atoi(Data[1]);
pc.printf("POSITION=%i\r\n",gyro_pos);//("POSITION=");
Config_Save();
- }
- else
- {
+ } else {
}
}
-
+
void gps_analyse(char* gps_string)
{
- // pc.puts(gps_string);
+ // pc.puts(gps_string);
//bluetooth.puts(gps_string);
- if (!strncmp(gps_string, "$GPRMC", 6))
- {
- // pc.puts(gps_string);
+ if (!strncmp(gps_string, "$GPRMC", 6)) {
+ // pc.puts(gps_string);
nmea_rmc(gps_string); //analyse and decompose the rmc string
}
}
-
+
int i2 = 0;
bool end2 = false;
bool start2 = false;
-
+
bool getline2()
{
int gotstring=false;
- while (1)
- {
- if( !bluetooth.readable() )
- {
+ while (1) {
+ if( !bluetooth.readable() ) {
break;
}
char c = bluetooth.getc();
- if (c == 36 )
- {
+ if (c == 36 ) {
start2=true;
end2 = false;
i2 = 0;
}
- if ((start2) && (c == 10))
- {
+ if ((start2) && (c == 10)) {
end2=true;
start2 = false;
}
- if (start2)
- {
+ if (start2) {
msg2[i2]=c;
i2++;
if (i2>255) break; //protect msg buffer!
}
- if (end2)
- {
+ if (end2) {
msg2[i2]=c;
msg2[i2+1] = 0;
start2 = false;
@@ -1038,40 +906,38 @@
}
return gotstring;
}
-
-
+
+
int i=0;
bool start=false;
bool end=false;
-
+
bool getline(bool forward)
{
- while (1)
- {
- if( !gps.readable() )
- {
+ while (1) {
+ if( !gps.readable() ) {
break;
}
char c = gps.getc();
- if (forward) //simply forward all to Bluetooth
- {
- bluetooth.putc(c);
+ if (forward) { //simply forward all to Bluetooth
+ bluetooth.putc(c);
}
- if (c == 36 ){start=true;end = false; i = 0;}
- if ((start) && (c == 10))
- {
- end=true;
+ if (c == 36 ) {
+ start=true;
+ end = false;
+ i = 0;
+ }
+ if ((start) && (c == 10)) {
+ end=true;
start = false;
}
- if (start)
- {
+ if (start) {
msg[i]=c;
i++;
if (i>255) break; //protect msg buffer!
}
- if (end)
- {
- msg[i]=c;
+ if (end) {
+ msg[i]=c;
msg[i+1] = 0;
i=0;
start = false;
@@ -1081,76 +947,76 @@
}
return end;
}
-
+
void keyPressedHeld( void )
{
motor_enable_state = "$ENABLE,1\r\n";
motor_enable = 1;
ledGREEN=1; //show green for being ready to steer
}
-
+
void keyReleasedHeld( void )
{
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
motor_enable_state = "$ENABLE,1\r\n";
motor_enable = 0;
- ledGREEN=0;
+ ledGREEN=0;
}
-
+
void boom1PressedHeld( void )
{
- // ledGREEN=1;
- boom18=boom18 & 0xFE;
+ // ledGREEN=1;
+ boom18=boom18 & 0xFE;
}
-
+
void boom1ReleasedHeld( void )
{
- //ledGREEN=0;
- boom18=boom18 | 0x01;
+ //ledGREEN=0;
+ boom18=boom18 | 0x01;
}
-
+
void boom2PressedHeld( void )
-{
- boom18=boom18 & 0xFD;
+{
+ boom18=boom18 & 0xFD;
}
-
+
void boom2ReleasedHeld( void )
{
- boom18=boom18 | 0x02;
+ boom18=boom18 | 0x02;
}
void boom3PressedHeld( void )
{
- boom18=boom18 & 0xFB;
+ boom18=boom18 & 0xFB;
}
-
+
void boom3ReleasedHeld( void )
{
- boom18=boom18 | 0x04;
+ boom18=boom18 | 0x04;
}
-
+
void boom4PressedHeld( void )
{
boom18=boom18 & 0xF7;
}
-
+
void boom4ReleasedHeld( void )
{
boom18=boom18 | 0x08;
}
-
+
void toprint()
{
angle_send = 1;
}
-
-
+
+
int main()
{
- int x=0;
+ //int x=0;
bluetooth.baud(115200);
gps.baud(38400);
pc.baud(38400);
-
+
//JH prepare and send version info
vTimer.start();
vTimer.reset();
@@ -1158,22 +1024,22 @@
motTimer.reset();
lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s
motor_enable_state = "$ENABLE,1\r\n";
- initializeAccelerometer();
+ initializeAccelerometer();
calibrateAccelerometer();
initializeGyroscope();
calibrateGyroscope();
btTimer.start();
btTimer.reset();
lastgetBT= btTimer.read_ms();
-
+
pc.puts(version);
bluetooth.puts(version);
lastsend_version=vTimer.read_ms()-18000;
-
+
Config_Startup();
_ID = Config_GetID();
Config_Save();
-
+
boom1.attach_asserted_held( &boom1PressedHeld );
boom1.attach_deasserted_held( &boom1ReleasedHeld );
boom1.setSampleFrequency(); //default = 20 ms
@@ -1194,86 +1060,66 @@
boom4.setSamplesTillAssert(5);
boom4.setSamplesTillHeld(5);
boom4.setSampleFrequency();
-
+
motor_switch.setSampleFrequency(10000);
motor_switch.attach_asserted_held( &keyPressedHeld );
motor_switch.attach_deasserted_held( &keyReleasedHeld );
-
+
accelerometerTicker.attach(&sampleAccelerometer, 0.005);
gyroscopeTicker.attach(&sampleGyroscope, 0.005);
filterTicker.attach(&filter, FILTER_RATE);
angle_print.attach(&toprint,0.2);
activate_antenna();
-
- while(1)
- {
+
+ while(1) {
//JH send version information every 10 seconds to keep Bluetooth alive
- if ((vTimer.read_ms()-lastsend_version)>25000)
- {
+ if ((vTimer.read_ms()-lastsend_version)>25000) {
pc.puts(version);
bluetooth.puts(version);
vTimer.reset();
lastsend_version=vTimer.read_ms();
- }
-
- if ( antenna_active == 1 && gps.readable())
- {
- if (getline(false))
- {
- if ( validate_checksum(msg))
- {
- if ( bluetooth.writeable())
- {
- // bluetooth.puts(msg);
+ }
+
+ if ( antenna_active == 1 && gps.readable()) {
+ if (getline(false)) {
+ if ( validate_checksum(msg)) {
}
gps_analyse(msg);
- }
- else
- {
+ } else {
pc.puts("INVALID!!!!\r\n");
}
}
}
- if ( bluetooth.readable())
- {
- if (getline2())
- {
- btTimer.reset();
- lastgetBT= btTimer.read_ms();
- // x++;
- // trim(msg2," ");
- // pc.puts(msg2);
+ if ( bluetooth.readable()) {
+ if (getline2()) {
+ btTimer.reset();
+ lastgetBT= btTimer.read_ms();
pc_analyse(msg2);
}
}
- if ( btTimer.read_ms()-lastgetBT>1000)
- {
- //we did not get any commands over BT
- ledRED=1; //turn red
- }
- else ledRED=0;
-
- if ( ((motTimer.read_ms()-lastsend_motorstate)>8000) || (motor_enable!=lastmotor_enable))
- {
+ if ( btTimer.read_ms()-lastgetBT>1000) {
+ //we did not get any commands over BT
+ ledRED=1; //turn red
+ } else ledRED=0;
+
+ if ( ((motTimer.read_ms()-lastsend_motorstate)>8000) || (motor_enable!=lastmotor_enable)) {
bluetooth.puts(motor_enable_state);
pc.puts(motor_enable_state);
motTimer.reset();
lastsend_motorstate=motTimer.read_ms();
lastmotor_enable=motor_enable;
}
- if (boom18!=lastboom18)
- {
- boomstate[4]=boom18 | 0x80; //
- bluetooth.puts(boomstate);
- pc.puts(boomstate);
- lastboom18=boom18;
+ if (boom18!=lastboom18) {
+ boomstate[4]=boom18 | 0x80; //
+ bluetooth.puts(boomstate);
+ pc.puts(boomstate);
+ lastboom18=boom18;
}
- if ( print_euler == 1 && angle_send == 1 ) //&& reading == 0)
- {
+ if ( print_euler == 1 && angle_send == 1 ) { //&& reading == 0)
sprintf(output,"$EULER,%f,%f,%f\r\n",toDegrees(get_roll()),toDegrees(get_pitch()),toDegrees(get_yaw()));
- // pc.puts(output);
+ // pc.puts(output);
bluetooth.puts(output);
angle_send = 0;
}
}
-}
\ No newline at end of file
+}