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
main.cpp@30:3afafa1ef16b, 2015-01-27 (annotated)
- Committer:
- maximbolduc
- Date:
- Tue Jan 27 12:22:10 2015 +0000
- Revision:
- 30:3afafa1ef16b
- Parent:
- 29:23ccb2a50b6f
- Child:
- 32:c57bc701d65c
Added a bunch f little stuff and cleaned the code.; Distance from line as well as psoition from line(right, left,on) is now working as well.
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| maximbolduc | 26:dc00998140af | 1 | #include "mbed.h" |
| maximbolduc | 26:dc00998140af | 2 | #include "PinDetect.h" |
| maximbolduc | 26:dc00998140af | 3 | #include "Point.h" |
| maximbolduc | 26:dc00998140af | 4 | #include <vector> |
| maximbolduc | 26:dc00998140af | 5 | #include "Line.h" |
| maximbolduc | 26:dc00998140af | 6 | #include "stringUtils.h" |
| jhedmonton | 28:5905886c76ee | 7 | #include "base.h" |
| jhedmonton | 28:5905886c76ee | 8 | #include "Config.h" |
| maximbolduc | 30:3afafa1ef16b | 9 | #include "imu_functions.h" |
| maximbolduc | 26:dc00998140af | 10 | |
| maximbolduc | 30:3afafa1ef16b | 11 | char *version="FreePilot V2.11 Jtan 20, 2015\r\n"; |
| jhedmonton | 27:9ac59b261d87 | 12 | long lastsend_version=0; |
| jhedmonton | 27:9ac59b261d87 | 13 | Timer vTimer; //this timer is int based! Max is 30 minutes |
| jhedmonton | 27:9ac59b261d87 | 14 | |
| maximbolduc | 26:dc00998140af | 15 | int checksumm; |
| maximbolduc | 26:dc00998140af | 16 | double distance_from_line; |
| maximbolduc | 26:dc00998140af | 17 | double cm_per_deg_lon; |
| maximbolduc | 26:dc00998140af | 18 | double cm_per_deg_lat; |
| maximbolduc | 26:dc00998140af | 19 | //all timing objects |
| maximbolduc | 26:dc00998140af | 20 | Timer gps_connecting; |
| maximbolduc | 26:dc00998140af | 21 | Timer autosteer_time; |
| maximbolduc | 26:dc00998140af | 22 | Timeout autosteer_timeout; //timeout work near as timer, but they don't give timing. they just trigger an interrupt once the time we assigned it is passed. |
| maximbolduc | 26:dc00998140af | 23 | Ticker accelerometerTicker; |
| maximbolduc | 26:dc00998140af | 24 | Ticker gyroscopeTicker; |
| maximbolduc | 26:dc00998140af | 25 | Ticker filterTicker; |
| maximbolduc | 26:dc00998140af | 26 | Ticker angle_print; |
| maximbolduc | 26:dc00998140af | 27 | |
| jhedmonton | 27:9ac59b261d87 | 28 | //Motor |
| jhedmonton | 27:9ac59b261d87 | 29 | PinDetect motor_switch(p16); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce. |
| jhedmonton | 27:9ac59b261d87 | 30 | DigitalOut enable_motor(p7); |
| jhedmonton | 28:5905886c76ee | 31 | |
| jhedmonton | 28:5905886c76ee | 32 | PwmOut pwm1(p22); |
| jhedmonton | 28:5905886c76ee | 33 | PwmOut pwm2(p21); |
| jhedmonton | 28:5905886c76ee | 34 | |
| jhedmonton | 27:9ac59b261d87 | 35 | //equipment switches |
| jhedmonton | 27:9ac59b261d87 | 36 | PinDetect boom1(p20,PullUp); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce. |
| jhedmonton | 27:9ac59b261d87 | 37 | PinDetect boom2(p19); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce. |
| jhedmonton | 27:9ac59b261d87 | 38 | PinDetect boom3(p18); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce. |
| jhedmonton | 27:9ac59b261d87 | 39 | PinDetect boom4(p17); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce. |
| maximbolduc | 30:3afafa1ef16b | 40 | |
| jhedmonton | 27:9ac59b261d87 | 41 | char boom18; //1 byte |
| jhedmonton | 27:9ac59b261d87 | 42 | char lastboom18; //1 byte |
| jhedmonton | 27:9ac59b261d87 | 43 | char boomstate[8]={'$','F','B','S',0,13,10,0 }; |
| jhedmonton | 27:9ac59b261d87 | 44 | |
| maximbolduc | 26:dc00998140af | 45 | Point position; |
| maximbolduc | 26:dc00998140af | 46 | Point looked_ahead; |
| maximbolduc | 26:dc00998140af | 47 | Point line_start; |
| maximbolduc | 26:dc00998140af | 48 | Point line_end; |
| maximbolduc | 26:dc00998140af | 49 | Point tilt_compensated_position; |
| maximbolduc | 26:dc00998140af | 50 | Point yaw_compensated_position; |
| maximbolduc | 26:dc00998140af | 51 | |
| maximbolduc | 26:dc00998140af | 52 | double distance_to_line; |
| maximbolduc | 26:dc00998140af | 53 | |
| maximbolduc | 26:dc00998140af | 54 | //FreePilot variables |
| maximbolduc | 26:dc00998140af | 55 | int timer_enabled; |
| jhedmonton | 28:5905886c76ee | 56 | double motorspeed; |
| maximbolduc | 26:dc00998140af | 57 | int enable_time; |
| maximbolduc | 26:dc00998140af | 58 | char* motor_enable_state = 0; |
| jhedmonton | 28:5905886c76ee | 59 | int motor_enable = 0; |
| jhedmonton | 28:5905886c76ee | 60 | int lastmotor_enable = 1; |
| maximbolduc | 26:dc00998140af | 61 | double pwm1_speed; |
| maximbolduc | 26:dc00998140af | 62 | double pwm2_speed; |
| jhedmonton | 28:5905886c76ee | 63 | long lastsend_motorstate=0; |
| jhedmonton | 28:5905886c76ee | 64 | Timer motTimer; //this timer is int based! Max is 30 minutes |
| jhedmonton | 28:5905886c76ee | 65 | Timer btTimer; //measure time for Bluetooth communication |
| jhedmonton | 28:5905886c76ee | 66 | long lastgetBT=0; |
| maximbolduc | 26:dc00998140af | 67 | |
| maximbolduc | 26:dc00998140af | 68 | int msg2_changed = 1; |
| maximbolduc | 26:dc00998140af | 69 | char* buffer; |
| maximbolduc | 26:dc00998140af | 70 | double meter_lat = 0; |
| maximbolduc | 26:dc00998140af | 71 | double meter_lon = 0; |
| jhedmonton | 28:5905886c76ee | 72 | |
| maximbolduc | 26:dc00998140af | 73 | char msg[256]; //GPS line buffer |
| maximbolduc | 26:dc00998140af | 74 | char msg2[256];//PC line buffer |
| maximbolduc | 26:dc00998140af | 75 | int printing; |
| maximbolduc | 26:dc00998140af | 76 | int num_of_gps_sats; |
| maximbolduc | 30:3afafa1ef16b | 77 | |
| maximbolduc | 26:dc00998140af | 78 | double decimal_lon; |
| maximbolduc | 26:dc00998140af | 79 | float longitude; |
| maximbolduc | 26:dc00998140af | 80 | float latitude; |
| maximbolduc | 26:dc00998140af | 81 | char ns, ew; |
| maximbolduc | 26:dc00998140af | 82 | int lock; |
| maximbolduc | 26:dc00998140af | 83 | int flag_gga; |
| maximbolduc | 26:dc00998140af | 84 | int reading; |
| maximbolduc | 26:dc00998140af | 85 | double decimal_latitude; |
| maximbolduc | 26:dc00998140af | 86 | int gps_satellite_quality; |
| maximbolduc | 26:dc00998140af | 87 | int day; |
| maximbolduc | 26:dc00998140af | 88 | int hour; |
| maximbolduc | 26:dc00998140af | 89 | int minute; |
| maximbolduc | 26:dc00998140af | 90 | int second; |
| maximbolduc | 26:dc00998140af | 91 | int tenths; |
| maximbolduc | 26:dc00998140af | 92 | int hundreths; |
| maximbolduc | 26:dc00998140af | 93 | char status; |
| maximbolduc | 26:dc00998140af | 94 | double track; // track made good . angle |
| maximbolduc | 26:dc00998140af | 95 | char magvar_dir; |
| maximbolduc | 26:dc00998140af | 96 | double magvar; |
| maximbolduc | 26:dc00998140af | 97 | int year; |
| maximbolduc | 26:dc00998140af | 98 | int month; |
| maximbolduc | 26:dc00998140af | 99 | double speed_km; |
| maximbolduc | 26:dc00998140af | 100 | double speed_m_s = 0; |
| maximbolduc | 26:dc00998140af | 101 | double velocity; // speed in knot |
| maximbolduc | 26:dc00998140af | 102 | 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. |
| maximbolduc | 26:dc00998140af | 103 | int connecting = 0; //are we still in phase of connecting? based on the connect_time value. |
| maximbolduc | 30:3afafa1ef16b | 104 | |
| maximbolduc | 26:dc00998140af | 105 | int angle_send = 0; |
| maximbolduc | 26:dc00998140af | 106 | int correct_rmc = 1; |
| maximbolduc | 26:dc00998140af | 107 | double m_lat = 0; |
| maximbolduc | 26:dc00998140af | 108 | double m_lon = 0; |
| maximbolduc | 26:dc00998140af | 109 | char* degminsec; |
| maximbolduc | 26:dc00998140af | 110 | double m_per_deg_lon; |
| maximbolduc | 26:dc00998140af | 111 | double m_per_deg_lat; |
| maximbolduc | 26:dc00998140af | 112 | double look_ahead_lon; |
| maximbolduc | 26:dc00998140af | 113 | double look_ahead_lat; |
| maximbolduc | 26:dc00998140af | 114 | int active_AB = 0; |
| maximbolduc | 26:dc00998140af | 115 | double compensation_vector; |
| maximbolduc | 26:dc00998140af | 116 | char output[256]; |
| maximbolduc | 26:dc00998140af | 117 | |
| maximbolduc | 26:dc00998140af | 118 | double yaw; |
| maximbolduc | 26:dc00998140af | 119 | double pitch; |
| maximbolduc | 26:dc00998140af | 120 | double roll; |
| maximbolduc | 26:dc00998140af | 121 | |
| maximbolduc | 30:3afafa1ef16b | 122 | double a_x; |
| maximbolduc | 30:3afafa1ef16b | 123 | double a_y; |
| maximbolduc | 30:3afafa1ef16b | 124 | double a_z; |
| maximbolduc | 30:3afafa1ef16b | 125 | double w_x; |
| maximbolduc | 30:3afafa1ef16b | 126 | double w_y; |
| maximbolduc | 30:3afafa1ef16b | 127 | double w_z; |
| maximbolduc | 26:dc00998140af | 128 | |
| maximbolduc | 26:dc00998140af | 129 | int readings[3]; |
| maximbolduc | 26:dc00998140af | 130 | |
| maximbolduc | 26:dc00998140af | 131 | double Freepilot_lat; |
| maximbolduc | 26:dc00998140af | 132 | double Freepilot_lon; |
| maximbolduc | 26:dc00998140af | 133 | double Freepilot_lat1; |
| maximbolduc | 26:dc00998140af | 134 | double Freepilot_lon1; |
| maximbolduc | 26:dc00998140af | 135 | double Freepilot_bearing; |
| maximbolduc | 26:dc00998140af | 136 | |
| maximbolduc | 26:dc00998140af | 137 | volatile bool newline_detected = false; |
| maximbolduc | 26:dc00998140af | 138 | |
| maximbolduc | 26:dc00998140af | 139 | Point point_add(Point a, Point b) |
| maximbolduc | 26:dc00998140af | 140 | { |
| maximbolduc | 26:dc00998140af | 141 | return Point(a.GetX() + b.GetX(), a.GetY() + b.GetY()); |
| maximbolduc | 26:dc00998140af | 142 | } |
| maximbolduc | 26:dc00998140af | 143 | |
| maximbolduc | 26:dc00998140af | 144 | Point point_sub(Point a , Point b) |
| maximbolduc | 26:dc00998140af | 145 | { |
| maximbolduc | 26:dc00998140af | 146 | return Point(a.GetX() - b.GetX(), a.GetY() - b.GetY()); |
| maximbolduc | 26:dc00998140af | 147 | } |
| maximbolduc | 26:dc00998140af | 148 | |
| maximbolduc | 26:dc00998140af | 149 | #define dot(u,v) ((u).GetX() * (v).GetX()+ (u).GetY() * (v).GetY()) |
| maximbolduc | 26:dc00998140af | 150 | #define norm(v) sqrt(dot(v,v)) // norm = length of vector |
| maximbolduc | 26:dc00998140af | 151 | #define d(u,v) norm(point_sub(u,v)) // distance = norm of difference |
| maximbolduc | 26:dc00998140af | 152 | |
| maximbolduc | 26:dc00998140af | 153 | double dist_Point_to_Line( Point P, Point line_start, Point line_end) |
| maximbolduc | 26:dc00998140af | 154 | { |
| maximbolduc | 26:dc00998140af | 155 | //Point v = point_sub(L->point1,L.point0); |
| maximbolduc | 26:dc00998140af | 156 | // Point w = point_sub(P,L.point0); |
| maximbolduc | 26:dc00998140af | 157 | Point v = point_sub(line_end,line_start); |
| maximbolduc | 26:dc00998140af | 158 | Point w = point_sub(P,line_start); |
| maximbolduc | 26:dc00998140af | 159 | |
| maximbolduc | 26:dc00998140af | 160 | double c1 = dot(w,v); |
| maximbolduc | 26:dc00998140af | 161 | double c2 = dot(v,v); |
| maximbolduc | 26:dc00998140af | 162 | double b = c1 / c2; |
| maximbolduc | 26:dc00998140af | 163 | |
| maximbolduc | 26:dc00998140af | 164 | Point resulting(b * v.GetX(),b*v.GetY()); |
| maximbolduc | 26:dc00998140af | 165 | Point Pb = point_add(line_start, resulting); |
| maximbolduc | 26:dc00998140af | 166 | return d(P, Pb); |
| maximbolduc | 26:dc00998140af | 167 | } |
| maximbolduc | 26:dc00998140af | 168 | |
| maximbolduc | 30:3afafa1ef16b | 169 | /*float Round_digits(float x, int numdigits) |
| maximbolduc | 26:dc00998140af | 170 | { |
| maximbolduc | 26:dc00998140af | 171 | // return ceil(x * pow(10,numdigits))/pow(10,numdigits); |
| maximbolduc | 26:dc00998140af | 172 | return ceil(x); |
| maximbolduc | 30:3afafa1ef16b | 173 | }*/ |
| maximbolduc | 26:dc00998140af | 174 | |
| maximbolduc | 26:dc00998140af | 175 | double lat_to_deg(char *s, char north_south) |
| maximbolduc | 26:dc00998140af | 176 | { |
| maximbolduc | 26:dc00998140af | 177 | int deg, min, sec; |
| maximbolduc | 26:dc00998140af | 178 | double fsec, val; |
| maximbolduc | 26:dc00998140af | 179 | |
| maximbolduc | 26:dc00998140af | 180 | deg = ( (s[0] - '0') * 10) + s[1] - '0'; |
| maximbolduc | 26:dc00998140af | 181 | min = ( (s[2] - '0') * 10) + s[3] - '0'; |
| maximbolduc | 26:dc00998140af | 182 | sec = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0')); |
| maximbolduc | 26:dc00998140af | 183 | fsec = (double)((double)sec /10000.0); |
| maximbolduc | 26:dc00998140af | 184 | val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0); |
| maximbolduc | 26:dc00998140af | 185 | if (north_south == 'S') |
| maximbolduc | 26:dc00998140af | 186 | { |
| maximbolduc | 26:dc00998140af | 187 | val *= -1.0; |
| maximbolduc | 26:dc00998140af | 188 | } |
| maximbolduc | 26:dc00998140af | 189 | decimal_latitude = val; |
| maximbolduc | 26:dc00998140af | 190 | return val; |
| maximbolduc | 26:dc00998140af | 191 | } |
| maximbolduc | 26:dc00998140af | 192 | |
| maximbolduc | 30:3afafa1ef16b | 193 | // isLeft(): test if a point is Left|On|Right of an infinite 2D line. |
| maximbolduc | 30:3afafa1ef16b | 194 | // Input: three points P0, P1, and P2 |
| maximbolduc | 30:3afafa1ef16b | 195 | // Return: >0 for P2 left of the line through P0 to P1 |
| maximbolduc | 30:3afafa1ef16b | 196 | // =0 for P2 on the line |
| maximbolduc | 30:3afafa1ef16b | 197 | // <0 for P2 right of the line |
| maximbolduc | 30:3afafa1ef16b | 198 | double isLeft( Point P0, Point P1, Point P2 ) |
| maximbolduc | 30:3afafa1ef16b | 199 | { |
| maximbolduc | 30:3afafa1ef16b | 200 | return ( (P1.GetY() - P0.GetY()) * (P2.GetX() - P0.GetX()) |
| maximbolduc | 30:3afafa1ef16b | 201 | - (P2.GetY() - P0.GetY()) * (P1.GetX() - P0.GetX())); |
| maximbolduc | 30:3afafa1ef16b | 202 | } |
| maximbolduc | 30:3afafa1ef16b | 203 | |
| maximbolduc | 26:dc00998140af | 204 | double lon_to_deg(char *s, char east_west) |
| maximbolduc | 26:dc00998140af | 205 | { |
| maximbolduc | 26:dc00998140af | 206 | int deg, min, sec; |
| maximbolduc | 26:dc00998140af | 207 | double fsec, val; |
| maximbolduc | 26:dc00998140af | 208 | deg = ( (s[0] - '0') * 100) + ((s[1] - '0') * 10) + (s[2] - '0'); |
| maximbolduc | 26:dc00998140af | 209 | min = ( (s[3] - '0') * 10) + s[4] - '0'; |
| maximbolduc | 26:dc00998140af | 210 | sec = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0')); |
| maximbolduc | 26:dc00998140af | 211 | fsec = (double)((double)sec /10000.0); |
| maximbolduc | 26:dc00998140af | 212 | val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0); |
| maximbolduc | 26:dc00998140af | 213 | if (east_west == 'W') |
| maximbolduc | 26:dc00998140af | 214 | { |
| maximbolduc | 26:dc00998140af | 215 | val *= -1.0; |
| maximbolduc | 26:dc00998140af | 216 | } |
| maximbolduc | 26:dc00998140af | 217 | decimal_lon = val; |
| maximbolduc | 26:dc00998140af | 218 | return val; |
| maximbolduc | 26:dc00998140af | 219 | } |
| maximbolduc | 26:dc00998140af | 220 | |
| maximbolduc | 26:dc00998140af | 221 | void nmea_gga(char *s) |
| maximbolduc | 26:dc00998140af | 222 | { |
| maximbolduc | 26:dc00998140af | 223 | char *token; |
| maximbolduc | 26:dc00998140af | 224 | int token_counter = 0; |
| maximbolduc | 26:dc00998140af | 225 | char *latitude = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 226 | char *longitude = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 227 | char *lat_dir = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 228 | char *lon_dir = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 229 | char *qual = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 230 | char *altitude = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 231 | char *sats = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 232 | |
| maximbolduc | 26:dc00998140af | 233 | token = strtok(s, ","); |
| maximbolduc | 26:dc00998140af | 234 | while (token) |
| maximbolduc | 26:dc00998140af | 235 | { |
| maximbolduc | 26:dc00998140af | 236 | switch (token_counter) |
| maximbolduc | 26:dc00998140af | 237 | { |
| maximbolduc | 26:dc00998140af | 238 | case 2: |
| maximbolduc | 26:dc00998140af | 239 | latitude = token; |
| maximbolduc | 26:dc00998140af | 240 | break; |
| maximbolduc | 26:dc00998140af | 241 | case 4: |
| maximbolduc | 26:dc00998140af | 242 | longitude = token; |
| maximbolduc | 26:dc00998140af | 243 | break; |
| maximbolduc | 26:dc00998140af | 244 | case 3: |
| maximbolduc | 26:dc00998140af | 245 | lat_dir = token; |
| maximbolduc | 26:dc00998140af | 246 | break; |
| maximbolduc | 26:dc00998140af | 247 | case 5: |
| maximbolduc | 26:dc00998140af | 248 | lon_dir = token; |
| maximbolduc | 26:dc00998140af | 249 | break; |
| maximbolduc | 26:dc00998140af | 250 | case 6: |
| maximbolduc | 26:dc00998140af | 251 | qual = token; |
| maximbolduc | 26:dc00998140af | 252 | break; |
| maximbolduc | 26:dc00998140af | 253 | case 7: |
| maximbolduc | 26:dc00998140af | 254 | sats = token; |
| maximbolduc | 26:dc00998140af | 255 | break; |
| maximbolduc | 26:dc00998140af | 256 | case 9: |
| maximbolduc | 26:dc00998140af | 257 | altitude = token; |
| maximbolduc | 26:dc00998140af | 258 | break; |
| maximbolduc | 26:dc00998140af | 259 | } |
| maximbolduc | 26:dc00998140af | 260 | token = strtok((char *)NULL, ","); |
| maximbolduc | 26:dc00998140af | 261 | token_counter++; |
| maximbolduc | 26:dc00998140af | 262 | } |
| maximbolduc | 26:dc00998140af | 263 | |
| maximbolduc | 26:dc00998140af | 264 | if (latitude && longitude && altitude && sats) |
| maximbolduc | 26:dc00998140af | 265 | { |
| maximbolduc | 26:dc00998140af | 266 | decimal_latitude = lat_to_deg(latitude, lat_dir[0]); |
| maximbolduc | 26:dc00998140af | 267 | decimal_lon = lon_to_deg(longitude, lon_dir[0]); |
| maximbolduc | 26:dc00998140af | 268 | num_of_gps_sats = atoi(sats); |
| maximbolduc | 26:dc00998140af | 269 | gps_satellite_quality = atoi(qual); |
| maximbolduc | 26:dc00998140af | 270 | } |
| maximbolduc | 26:dc00998140af | 271 | else |
| maximbolduc | 26:dc00998140af | 272 | { |
| maximbolduc | 26:dc00998140af | 273 | gps_satellite_quality = 0; |
| maximbolduc | 26:dc00998140af | 274 | } |
| maximbolduc | 26:dc00998140af | 275 | } |
| maximbolduc | 26:dc00998140af | 276 | |
| maximbolduc | 26:dc00998140af | 277 | //from farmerGPS code |
| maximbolduc | 26:dc00998140af | 278 | void get_latlon_byangle(double lat1, double lon1, double distance,double angle, double &lon2, double &lat2) |
| maximbolduc | 26:dc00998140af | 279 | { |
| maximbolduc | 26:dc00998140af | 280 | double ydist = 0; |
| maximbolduc | 26:dc00998140af | 281 | double xdist = 0; |
| maximbolduc | 26:dc00998140af | 282 | angle = angle + 180; |
| maximbolduc | 26:dc00998140af | 283 | double radiant = angle * 3.14159265359 / 180; |
| maximbolduc | 26:dc00998140af | 284 | double sinr = sin(radiant); |
| maximbolduc | 26:dc00998140af | 285 | double cosr = cos(radiant); |
| maximbolduc | 26:dc00998140af | 286 | xdist = cosr * distance; |
| maximbolduc | 26:dc00998140af | 287 | ydist = sinr * distance; |
| maximbolduc | 26:dc00998140af | 288 | lat2 = lat1 + (ydist / (69.09 * -1609.344)); |
| maximbolduc | 26:dc00998140af | 289 | lon2 = lon1 - (xdist / (69.09 * 1609.344 * cos(lat1/57.295779513))); |
| maximbolduc | 26:dc00998140af | 290 | return; |
| maximbolduc | 26:dc00998140af | 291 | } |
| maximbolduc | 26:dc00998140af | 292 | |
| maximbolduc | 26:dc00998140af | 293 | Point compensation; |
| maximbolduc | 26:dc00998140af | 294 | double compensation_angle; |
| maximbolduc | 26:dc00998140af | 295 | //antenna compensation in cm |
| maximbolduc | 26:dc00998140af | 296 | void tilt_compensate() |
| maximbolduc | 26:dc00998140af | 297 | { |
| maximbolduc | 26:dc00998140af | 298 | roll = imuFilter.getRoll(); |
| jhedmonton | 28:5905886c76ee | 299 | compensation_vector = antennaheight * sin(roll); |
| maximbolduc | 26:dc00998140af | 300 | compensation.SetX(compensation_vector * cos((toDegrees(imuFilter.getYaw()) * -1 - 90)/57.295779513)); |
| maximbolduc | 26:dc00998140af | 301 | compensation.SetY(compensation_vector * sin((toDegrees(imuFilter.getYaw()) * -1 - 90)/57.295779513)); |
| maximbolduc | 26:dc00998140af | 302 | } |
| maximbolduc | 26:dc00998140af | 303 | |
| maximbolduc | 26:dc00998140af | 304 | void yaw_compensate() |
| maximbolduc | 26:dc00998140af | 305 | { |
| maximbolduc | 26:dc00998140af | 306 | yaw = imuFilter.getYaw(); |
| maximbolduc | 26:dc00998140af | 307 | } |
| maximbolduc | 26:dc00998140af | 308 | |
| maximbolduc | 26:dc00998140af | 309 | void process_GPSHEIGHT(char* height_string) |
| maximbolduc | 26:dc00998140af | 310 | { |
| maximbolduc | 26:dc00998140af | 311 | char *token; |
| maximbolduc | 26:dc00998140af | 312 | int token_counter = 0; |
| maximbolduc | 26:dc00998140af | 313 | char *height = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 314 | token = strtok(height_string, ","); |
| maximbolduc | 26:dc00998140af | 315 | while (token) |
| maximbolduc | 26:dc00998140af | 316 | { |
| maximbolduc | 26:dc00998140af | 317 | |
| maximbolduc | 26:dc00998140af | 318 | switch (token_counter) |
| maximbolduc | 26:dc00998140af | 319 | { |
| maximbolduc | 26:dc00998140af | 320 | case 1: |
| maximbolduc | 26:dc00998140af | 321 | height = token; |
| maximbolduc | 26:dc00998140af | 322 | break; |
| maximbolduc | 26:dc00998140af | 323 | } |
| maximbolduc | 26:dc00998140af | 324 | token = strtok((char *)NULL, ","); |
| maximbolduc | 26:dc00998140af | 325 | token_counter++; |
| maximbolduc | 26:dc00998140af | 326 | } |
| maximbolduc | 26:dc00998140af | 327 | if ( height ) |
| maximbolduc | 26:dc00998140af | 328 | { |
| jhedmonton | 28:5905886c76ee | 329 | antennaheight = atof(height); |
| maximbolduc | 30:3afafa1ef16b | 330 | Config_Save(); |
| maximbolduc | 26:dc00998140af | 331 | } |
| maximbolduc | 26:dc00998140af | 332 | } |
| maximbolduc | 26:dc00998140af | 333 | |
| maximbolduc | 26:dc00998140af | 334 | void nmea_rmc(char *s) |
| maximbolduc | 26:dc00998140af | 335 | { |
| maximbolduc | 26:dc00998140af | 336 | char *token; |
| maximbolduc | 26:dc00998140af | 337 | int token_counter = 0; |
| maximbolduc | 26:dc00998140af | 338 | char *time = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 339 | char *date = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 340 | char *stat = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 341 | char *vel = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 342 | char *trk = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 343 | char *magv = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 344 | char *magd = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 345 | char *latitude = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 346 | char *longitude = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 347 | char *lat_dir = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 348 | char *lon_dir = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 349 | |
| maximbolduc | 26:dc00998140af | 350 | token = strtok(s, ",*"); |
| maximbolduc | 26:dc00998140af | 351 | while (token) |
| maximbolduc | 26:dc00998140af | 352 | { |
| maximbolduc | 26:dc00998140af | 353 | switch (token_counter) |
| maximbolduc | 26:dc00998140af | 354 | { |
| maximbolduc | 26:dc00998140af | 355 | case 9: |
| maximbolduc | 26:dc00998140af | 356 | date = token; |
| maximbolduc | 26:dc00998140af | 357 | break; |
| maximbolduc | 26:dc00998140af | 358 | case 1: |
| maximbolduc | 26:dc00998140af | 359 | time = token; |
| maximbolduc | 26:dc00998140af | 360 | break; |
| maximbolduc | 26:dc00998140af | 361 | case 2: |
| maximbolduc | 26:dc00998140af | 362 | stat = token; |
| maximbolduc | 26:dc00998140af | 363 | break; |
| maximbolduc | 26:dc00998140af | 364 | case 7: |
| maximbolduc | 26:dc00998140af | 365 | vel = token; |
| maximbolduc | 26:dc00998140af | 366 | break; |
| maximbolduc | 26:dc00998140af | 367 | case 8: |
| maximbolduc | 26:dc00998140af | 368 | trk = token; |
| maximbolduc | 26:dc00998140af | 369 | break; |
| maximbolduc | 26:dc00998140af | 370 | case 10: |
| maximbolduc | 26:dc00998140af | 371 | magv = token; |
| maximbolduc | 26:dc00998140af | 372 | break; |
| maximbolduc | 26:dc00998140af | 373 | case 11: |
| maximbolduc | 26:dc00998140af | 374 | magd = token; |
| maximbolduc | 26:dc00998140af | 375 | break; |
| maximbolduc | 26:dc00998140af | 376 | case 3: |
| maximbolduc | 26:dc00998140af | 377 | latitude = token; |
| maximbolduc | 26:dc00998140af | 378 | break; |
| maximbolduc | 26:dc00998140af | 379 | case 5: |
| maximbolduc | 26:dc00998140af | 380 | longitude = token; |
| maximbolduc | 26:dc00998140af | 381 | break; |
| maximbolduc | 26:dc00998140af | 382 | case 4: |
| maximbolduc | 26:dc00998140af | 383 | lat_dir = token; |
| maximbolduc | 26:dc00998140af | 384 | break; |
| maximbolduc | 26:dc00998140af | 385 | case 6: |
| maximbolduc | 26:dc00998140af | 386 | lon_dir = token; |
| maximbolduc | 26:dc00998140af | 387 | break; |
| maximbolduc | 26:dc00998140af | 388 | /* case 11: |
| maximbolduc | 26:dc00998140af | 389 | process_cs(token);*/ |
| maximbolduc | 26:dc00998140af | 390 | } |
| maximbolduc | 26:dc00998140af | 391 | token = strtok((char *)NULL, ","); |
| maximbolduc | 26:dc00998140af | 392 | token_counter++; |
| maximbolduc | 26:dc00998140af | 393 | } |
| maximbolduc | 26:dc00998140af | 394 | if (stat && date && time) |
| maximbolduc | 26:dc00998140af | 395 | { |
| maximbolduc | 26:dc00998140af | 396 | hour = (char)((time[0] - '0') * 10) + (time[1] - '0'); |
| maximbolduc | 26:dc00998140af | 397 | minute = (char)((time[2] - '0') * 10) + (time[3] - '0'); |
| maximbolduc | 26:dc00998140af | 398 | second = (char)((time[4] - '0') * 10) + (time[5] - '0'); |
| maximbolduc | 26:dc00998140af | 399 | day = (char)((date[0] - '0') * 10) + (date[1] - '0'); |
| maximbolduc | 26:dc00998140af | 400 | month = (char)((date[2] - '0') * 10) + (date[3] - '0'); |
| maximbolduc | 26:dc00998140af | 401 | year = (int)((date[4] - '0') * 10) + (date[5] - '0') + 2000; |
| maximbolduc | 26:dc00998140af | 402 | status = stat[0]; |
| maximbolduc | 26:dc00998140af | 403 | velocity = atof(vel); |
| maximbolduc | 26:dc00998140af | 404 | speed_km = velocity * 1.852; |
| maximbolduc | 26:dc00998140af | 405 | speed_m_s = speed_km * 3600.0 / 1000.0; |
| maximbolduc | 26:dc00998140af | 406 | //speed_m_s = 5; |
| maximbolduc | 26:dc00998140af | 407 | track = atof(trk); |
| maximbolduc | 26:dc00998140af | 408 | magvar = atof(magv); |
| maximbolduc | 26:dc00998140af | 409 | magvar_dir = magd[0]; |
| maximbolduc | 26:dc00998140af | 410 | } |
| maximbolduc | 26:dc00998140af | 411 | decimal_latitude = lat_to_deg(latitude, lat_dir[0]); |
| maximbolduc | 26:dc00998140af | 412 | decimal_lon = lon_to_deg(longitude, lon_dir[0]); |
| maximbolduc | 26:dc00998140af | 413 | position.SetX(decimal_latitude); |
| maximbolduc | 26:dc00998140af | 414 | position.SetY(decimal_lon); |
| maximbolduc | 30:3afafa1ef16b | 415 | cm_per_deg_lat = 11054000; |
| maximbolduc | 30:3afafa1ef16b | 416 | cm_per_deg_lon = 11132000 * cos(decimal_latitude); |
| maximbolduc | 26:dc00998140af | 417 | tilt_compensate(); //in centimeters |
| maximbolduc | 26:dc00998140af | 418 | compensation.SetY(compensation.GetY() / cm_per_deg_lon); |
| maximbolduc | 26:dc00998140af | 419 | compensation.SetX(compensation.GetX() / cm_per_deg_lat); |
| maximbolduc | 26:dc00998140af | 420 | |
| maximbolduc | 26:dc00998140af | 421 | // yaw_compensate(); |
| maximbolduc | 26:dc00998140af | 422 | position = point_add(position,compensation); |
| maximbolduc | 30:3afafa1ef16b | 423 | //modify_rmc(); |
| jhedmonton | 28:5905886c76ee | 424 | double lookaheaddistance = lookaheadtime * speed_m_s; |
| maximbolduc | 26:dc00998140af | 425 | |
| jhedmonton | 28:5905886c76ee | 426 | get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,track,look_ahead_lon,look_ahead_lat); |
| maximbolduc | 26:dc00998140af | 427 | looked_ahead.SetX(look_ahead_lat); |
| maximbolduc | 26:dc00998140af | 428 | looked_ahead.SetY(look_ahead_lon); |
| maximbolduc | 30:3afafa1ef16b | 429 | double filtering = 111111 / 2.0 + 111111 * cos(decimal_latitude)/2.0; |
| maximbolduc | 26:dc00998140af | 430 | distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end); |
| maximbolduc | 30:3afafa1ef16b | 431 | double sign = isLeft( line_start, line_end, looked_ahead); |
| maximbolduc | 30:3afafa1ef16b | 432 | if ( sign < 0 ) |
| maximbolduc | 30:3afafa1ef16b | 433 | { |
| maximbolduc | 30:3afafa1ef16b | 434 | distance_to_line = distance_to_line; |
| maximbolduc | 30:3afafa1ef16b | 435 | } |
| maximbolduc | 30:3afafa1ef16b | 436 | else if ( sign > 0 ) |
| maximbolduc | 30:3afafa1ef16b | 437 | { |
| maximbolduc | 30:3afafa1ef16b | 438 | distance_to_line = -distance_to_line; |
| maximbolduc | 30:3afafa1ef16b | 439 | } |
| maximbolduc | 26:dc00998140af | 440 | |
| maximbolduc | 30:3afafa1ef16b | 441 | sprintf(output,"$DIST_TO_LINE: % .12f %f\r\n\0",distance_to_line * filtering, sign); |
| maximbolduc | 30:3afafa1ef16b | 442 | pc.puts(output); |
| maximbolduc | 26:dc00998140af | 443 | } |
| maximbolduc | 26:dc00998140af | 444 | |
| maximbolduc | 26:dc00998140af | 445 | void process_FGPSAB(char* ab) |
| maximbolduc | 26:dc00998140af | 446 | { |
| maximbolduc | 26:dc00998140af | 447 | char *token; |
| maximbolduc | 26:dc00998140af | 448 | int token_counter = 0; |
| maximbolduc | 26:dc00998140af | 449 | char *line_lat = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 450 | char *line_lon = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 451 | char *line_lat1 = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 452 | char *line_lon1 = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 453 | char *bearing = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 454 | token = strtok(ab, ","); |
| maximbolduc | 26:dc00998140af | 455 | while (token) |
| maximbolduc | 26:dc00998140af | 456 | { |
| maximbolduc | 26:dc00998140af | 457 | switch (token_counter) |
| maximbolduc | 26:dc00998140af | 458 | { |
| maximbolduc | 26:dc00998140af | 459 | case 1: |
| maximbolduc | 26:dc00998140af | 460 | line_lat = token; |
| maximbolduc | 26:dc00998140af | 461 | break; |
| maximbolduc | 26:dc00998140af | 462 | case 2: |
| maximbolduc | 26:dc00998140af | 463 | line_lon = token; |
| maximbolduc | 26:dc00998140af | 464 | break; |
| maximbolduc | 26:dc00998140af | 465 | case 3: |
| maximbolduc | 26:dc00998140af | 466 | line_lat1 = token; |
| maximbolduc | 26:dc00998140af | 467 | break; |
| maximbolduc | 26:dc00998140af | 468 | case 4: |
| maximbolduc | 26:dc00998140af | 469 | line_lon1 = token; |
| maximbolduc | 26:dc00998140af | 470 | break; |
| maximbolduc | 26:dc00998140af | 471 | case 5: |
| maximbolduc | 26:dc00998140af | 472 | bearing = token; |
| maximbolduc | 26:dc00998140af | 473 | break; |
| maximbolduc | 26:dc00998140af | 474 | } |
| maximbolduc | 26:dc00998140af | 475 | token = strtok((char *)NULL, ","); |
| maximbolduc | 26:dc00998140af | 476 | token_counter++; |
| maximbolduc | 26:dc00998140af | 477 | } |
| maximbolduc | 26:dc00998140af | 478 | Freepilot_lon = atof(line_lon); |
| maximbolduc | 26:dc00998140af | 479 | Freepilot_lat = atof(line_lat); |
| maximbolduc | 26:dc00998140af | 480 | Freepilot_lon1 = atof(line_lon1); |
| maximbolduc | 26:dc00998140af | 481 | Freepilot_lat1 = atof(line_lat1); |
| maximbolduc | 26:dc00998140af | 482 | Freepilot_bearing = atof(bearing); |
| maximbolduc | 26:dc00998140af | 483 | line_start.SetX(Freepilot_lat); |
| maximbolduc | 26:dc00998140af | 484 | line_start.SetY(Freepilot_lon); |
| maximbolduc | 26:dc00998140af | 485 | line_end.SetX(Freepilot_lat1); |
| maximbolduc | 26:dc00998140af | 486 | line_end.SetY(Freepilot_lon1); |
| maximbolduc | 26:dc00998140af | 487 | active_AB = 1; |
| maximbolduc | 26:dc00998140af | 488 | |
| maximbolduc | 26:dc00998140af | 489 | sprintf(output, "$ABLINE:%f , %f, %f, %f\r\n",line_start.GetX(),line_start.GetY(),line_end.GetX(),line_end.GetY()); |
| maximbolduc | 26:dc00998140af | 490 | pc.puts(output); |
| maximbolduc | 26:dc00998140af | 491 | } |
| maximbolduc | 26:dc00998140af | 492 | |
| maximbolduc | 26:dc00998140af | 493 | void autosteer_done() |
| maximbolduc | 26:dc00998140af | 494 | { |
| maximbolduc | 26:dc00998140af | 495 | //kill the autosteer once the timeout reech |
| maximbolduc | 26:dc00998140af | 496 | enable_motor = 0; |
| maximbolduc | 26:dc00998140af | 497 | } |
| maximbolduc | 26:dc00998140af | 498 | |
| maximbolduc | 26:dc00998140af | 499 | void process_FGPSAUTO(char* FGPSAUTO) |
| maximbolduc | 26:dc00998140af | 500 | { |
| maximbolduc | 26:dc00998140af | 501 | char *token; |
| maximbolduc | 26:dc00998140af | 502 | int token_counter = 0; |
| maximbolduc | 26:dc00998140af | 503 | char *ahead = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 504 | char *center = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 505 | char *phase = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 506 | char *scl = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 507 | char *avg = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 508 | char *_kp = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 509 | char *_ki = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 510 | char *_kd = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 511 | token = strtok(FGPSAUTO, ","); |
| maximbolduc | 26:dc00998140af | 512 | while (token) |
| maximbolduc | 26:dc00998140af | 513 | { |
| maximbolduc | 26:dc00998140af | 514 | switch (token_counter) |
| maximbolduc | 26:dc00998140af | 515 | { |
| maximbolduc | 26:dc00998140af | 516 | case 1: |
| maximbolduc | 26:dc00998140af | 517 | phase = token; |
| maximbolduc | 26:dc00998140af | 518 | break; |
| maximbolduc | 26:dc00998140af | 519 | case 2: |
| maximbolduc | 26:dc00998140af | 520 | center = token; |
| maximbolduc | 26:dc00998140af | 521 | break; |
| maximbolduc | 26:dc00998140af | 522 | case 4: |
| maximbolduc | 26:dc00998140af | 523 | scl = token; |
| maximbolduc | 26:dc00998140af | 524 | break; |
| maximbolduc | 26:dc00998140af | 525 | case 5: |
| maximbolduc | 26:dc00998140af | 526 | ahead = token; |
| maximbolduc | 26:dc00998140af | 527 | break; |
| maximbolduc | 26:dc00998140af | 528 | case 6: |
| maximbolduc | 26:dc00998140af | 529 | avg = token; |
| maximbolduc | 26:dc00998140af | 530 | break; |
| maximbolduc | 26:dc00998140af | 531 | case 7: |
| maximbolduc | 26:dc00998140af | 532 | _kp = token; |
| maximbolduc | 26:dc00998140af | 533 | break; |
| maximbolduc | 26:dc00998140af | 534 | case 8: |
| maximbolduc | 26:dc00998140af | 535 | _ki = token; |
| maximbolduc | 26:dc00998140af | 536 | break; |
| maximbolduc | 26:dc00998140af | 537 | case 9: |
| maximbolduc | 26:dc00998140af | 538 | _kd = token; |
| maximbolduc | 26:dc00998140af | 539 | break; |
| maximbolduc | 26:dc00998140af | 540 | } |
| maximbolduc | 26:dc00998140af | 541 | token = strtok((char *)NULL, ","); |
| maximbolduc | 26:dc00998140af | 542 | token_counter++; |
| maximbolduc | 26:dc00998140af | 543 | } |
| maximbolduc | 26:dc00998140af | 544 | if ( _kp && _ki && _kd ) |
| maximbolduc | 26:dc00998140af | 545 | { |
| maximbolduc | 26:dc00998140af | 546 | kp = atof(_kp); |
| maximbolduc | 26:dc00998140af | 547 | ki = atof(_ki); |
| maximbolduc | 26:dc00998140af | 548 | kd = atof(_kd); |
| maximbolduc | 26:dc00998140af | 549 | } |
| maximbolduc | 26:dc00998140af | 550 | if ( phase && center && scl && avg && ahead ) |
| maximbolduc | 26:dc00998140af | 551 | { |
| jhedmonton | 28:5905886c76ee | 552 | lookaheadtime = atof(ahead); |
| maximbolduc | 26:dc00998140af | 553 | scale = atof(scl); |
| maximbolduc | 26:dc00998140af | 554 | phaseadv = atof(phase); |
| jhedmonton | 28:5905886c76ee | 555 | avgpos = atof(avg); |
| jhedmonton | 28:5905886c76ee | 556 | tcenter = atof(center); |
| maximbolduc | 30:3afafa1ef16b | 557 | sprintf(output, "$SETTINGS:%f\r\n", lookaheadtime); |
| maximbolduc | 30:3afafa1ef16b | 558 | pc.puts(output); |
| maximbolduc | 26:dc00998140af | 559 | } |
| maximbolduc | 26:dc00998140af | 560 | } |
| maximbolduc | 30:3afafa1ef16b | 561 | |
| jhedmonton | 27:9ac59b261d87 | 562 | //sets pwm1 and pwm2 and enable_motor |
| maximbolduc | 26:dc00998140af | 563 | void process_ASTEER(char* asteer) |
| maximbolduc | 26:dc00998140af | 564 | { |
| maximbolduc | 26:dc00998140af | 565 | char *token; |
| maximbolduc | 26:dc00998140af | 566 | int token_counter = 0; |
| maximbolduc | 26:dc00998140af | 567 | char *asteer_speed = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 568 | char *asteer_time = (char *)NULL; |
| maximbolduc | 26:dc00998140af | 569 | token = strtok(asteer, ","); |
| maximbolduc | 26:dc00998140af | 570 | while (token) |
| maximbolduc | 26:dc00998140af | 571 | { |
| maximbolduc | 26:dc00998140af | 572 | switch (token_counter) |
| maximbolduc | 26:dc00998140af | 573 | { |
| maximbolduc | 26:dc00998140af | 574 | case 1: |
| maximbolduc | 26:dc00998140af | 575 | asteer_speed = token; |
| maximbolduc | 26:dc00998140af | 576 | break; |
| maximbolduc | 26:dc00998140af | 577 | case 2: |
| maximbolduc | 26:dc00998140af | 578 | asteer_time = token; |
| maximbolduc | 26:dc00998140af | 579 | break; |
| maximbolduc | 26:dc00998140af | 580 | } |
| maximbolduc | 26:dc00998140af | 581 | token = strtok((char *)NULL, ","); |
| maximbolduc | 26:dc00998140af | 582 | token_counter++; |
| maximbolduc | 26:dc00998140af | 583 | } |
| maximbolduc | 26:dc00998140af | 584 | if ( asteer_speed && asteer_time ) |
| maximbolduc | 26:dc00998140af | 585 | { |
| maximbolduc | 26:dc00998140af | 586 | motorspeed = atof(asteer_speed); |
| maximbolduc | 26:dc00998140af | 587 | enable_time = atof(asteer_time); |
| jhedmonton | 28:5905886c76ee | 588 | autosteer_timeout.attach_us(autosteer_done,(double)enable_time * (double)1000.0); |
| jhedmonton | 28:5905886c76ee | 589 | if ( motorspeed > 127.0 ) |
| maximbolduc | 26:dc00998140af | 590 | { |
| jhedmonton | 28:5905886c76ee | 591 | pwm2_speed = 0.0; |
| jhedmonton | 28:5905886c76ee | 592 | pwm1_speed = ((double)motorspeed - (double)127.0) / 127.0; |
| maximbolduc | 26:dc00998140af | 593 | enable_motor = 1; |
| maximbolduc | 26:dc00998140af | 594 | } |
| jhedmonton | 28:5905886c76ee | 595 | else if ( motorspeed < 127.0 ) |
| maximbolduc | 26:dc00998140af | 596 | { |
| jhedmonton | 28:5905886c76ee | 597 | pwm2_speed = ( ((double) 127-(double)motorspeed) / 127.0 ); |
| jhedmonton | 28:5905886c76ee | 598 | pwm1_speed = 0.0; |
| maximbolduc | 26:dc00998140af | 599 | enable_motor = 1; |
| maximbolduc | 26:dc00998140af | 600 | } |
| maximbolduc | 26:dc00998140af | 601 | else |
| maximbolduc | 26:dc00998140af | 602 | { |
| maximbolduc | 26:dc00998140af | 603 | pwm1_speed = 0; |
| maximbolduc | 26:dc00998140af | 604 | pwm2_speed = 0; |
| maximbolduc | 26:dc00998140af | 605 | enable_motor = 0; |
| maximbolduc | 26:dc00998140af | 606 | } |
| jhedmonton | 29:23ccb2a50b6f | 607 | if(Authenticated) |
| jhedmonton | 29:23ccb2a50b6f | 608 | { |
| jhedmonton | 29:23ccb2a50b6f | 609 | pwm1 = pwm1_speed; |
| jhedmonton | 29:23ccb2a50b6f | 610 | pwm2 = pwm2_speed; |
| jhedmonton | 29:23ccb2a50b6f | 611 | } |
| jhedmonton | 29:23ccb2a50b6f | 612 | else |
| jhedmonton | 29:23ccb2a50b6f | 613 | { |
| jhedmonton | 29:23ccb2a50b6f | 614 | sprintf(output,"$NOT AUTHENTICATED TO STEER %f %d %f %f",motorspeed,enable_time,pwm1_speed,pwm2_speed); |
| jhedmonton | 29:23ccb2a50b6f | 615 | pc.puts(output); |
| jhedmonton | 29:23ccb2a50b6f | 616 | bluetooth.puts(output); |
| jhedmonton | 29:23ccb2a50b6f | 617 | } |
| maximbolduc | 26:dc00998140af | 618 | } |
| maximbolduc | 26:dc00998140af | 619 | } |
| jhedmonton | 28:5905886c76ee | 620 | |
| maximbolduc | 26:dc00998140af | 621 | |
| maximbolduc | 26:dc00998140af | 622 | |
| maximbolduc | 26:dc00998140af | 623 | |
| maximbolduc | 26:dc00998140af | 624 | void pc_analyse(char* pc_string) |
| maximbolduc | 26:dc00998140af | 625 | { |
| maximbolduc | 26:dc00998140af | 626 | if (!strncmp(pc_string, "$ASTEER", 7)) |
| maximbolduc | 26:dc00998140af | 627 | { |
| jhedmonton | 27:9ac59b261d87 | 628 | //sets pwm1 and pwm2 and enable_motor |
| maximbolduc | 26:dc00998140af | 629 | process_ASTEER(pc_string); |
| maximbolduc | 26:dc00998140af | 630 | } |
| jhedmonton | 29:23ccb2a50b6f | 631 | else if (!strncmp(pc_string, "$BANY",5)) |
| jhedmonton | 29:23ccb2a50b6f | 632 | { |
| jhedmonton | 29:23ccb2a50b6f | 633 | _ID = Config_GetID(); |
| jhedmonton | 29:23ccb2a50b6f | 634 | Config_Save(); |
| jhedmonton | 29:23ccb2a50b6f | 635 | } |
| maximbolduc | 30:3afafa1ef16b | 636 | else if (!strncmp(pc_string, "$GPSBAUD",8)) |
| maximbolduc | 26:dc00998140af | 637 | { |
| maximbolduc | 26:dc00998140af | 638 | process_GPSBAUD(pc_string); |
| maximbolduc | 30:3afafa1ef16b | 639 | Config_Save(); |
| jhedmonton | 27:9ac59b261d87 | 640 | sprintf(output,"%s %d\r\n",pc_string,gps_baud); |
| jhedmonton | 27:9ac59b261d87 | 641 | pc.puts(output); |
| maximbolduc | 26:dc00998140af | 642 | } |
| maximbolduc | 26:dc00998140af | 643 | else if (!strncmp(pc_string, "$FGPS,",6)) |
| maximbolduc | 26:dc00998140af | 644 | { |
| jhedmonton | 27:9ac59b261d87 | 645 | //process_initstring(pc_string); |
| jhedmonton | 27:9ac59b261d87 | 646 | int i=5; |
| jhedmonton | 27:9ac59b261d87 | 647 | char c=pc_string[i]; |
| jhedmonton | 27:9ac59b261d87 | 648 | while (c!=0) |
| jhedmonton | 27:9ac59b261d87 | 649 | { |
| jhedmonton | 27:9ac59b261d87 | 650 | i++; |
| jhedmonton | 27:9ac59b261d87 | 651 | if (i>255) break; //protect msg buffer! |
| jhedmonton | 27:9ac59b261d87 | 652 | c=pc_string[i]; |
| jhedmonton | 28:5905886c76ee | 653 | gps.putc(c); |
| jhedmonton | 27:9ac59b261d87 | 654 | pc.putc(c); |
| jhedmonton | 27:9ac59b261d87 | 655 | } |
| maximbolduc | 26:dc00998140af | 656 | } |
| maximbolduc | 30:3afafa1ef16b | 657 | |
| maximbolduc | 30:3afafa1ef16b | 658 | else if (!strncmp(pc_string, "$GPSHEIGHT",10)) |
| maximbolduc | 26:dc00998140af | 659 | { |
| maximbolduc | 26:dc00998140af | 660 | process_GPSHEIGHT(pc_string); |
| maximbolduc | 26:dc00998140af | 661 | sprintf(output,"%s\r\n",pc_string); |
| maximbolduc | 26:dc00998140af | 662 | bluetooth.puts(output); |
| maximbolduc | 30:3afafa1ef16b | 663 | Config_Save(); |
| maximbolduc | 26:dc00998140af | 664 | } |
| maximbolduc | 26:dc00998140af | 665 | else if (!strncmp(pc_string, "$FGPSAUTO",9)) |
| maximbolduc | 26:dc00998140af | 666 | { |
| maximbolduc | 26:dc00998140af | 667 | process_FGPSAUTO(pc_string); |
| maximbolduc | 26:dc00998140af | 668 | sprintf(output,"%s\r\n",pc_string); |
| maximbolduc | 26:dc00998140af | 669 | bluetooth.puts(output); |
| maximbolduc | 30:3afafa1ef16b | 670 | Config_Save(); |
| maximbolduc | 26:dc00998140af | 671 | } |
| maximbolduc | 26:dc00998140af | 672 | else if (!strncmp(pc_string, "$FGPSAB",7)) |
| maximbolduc | 26:dc00998140af | 673 | { |
| jhedmonton | 29:23ccb2a50b6f | 674 | // sprintf(output,"FOUND AB %s\r\n",pc_string); |
| maximbolduc | 26:dc00998140af | 675 | // bluetooth.puts(output); |
| jhedmonton | 29:23ccb2a50b6f | 676 | // pc.puts(output); |
| maximbolduc | 26:dc00998140af | 677 | process_FGPSAB(pc_string); |
| maximbolduc | 30:3afafa1ef16b | 678 | } |
| maximbolduc | 30:3afafa1ef16b | 679 | else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) |
| maximbolduc | 30:3afafa1ef16b | 680 | { |
| maximbolduc | 30:3afafa1ef16b | 681 | calibrateAccelerometer(); |
| maximbolduc | 30:3afafa1ef16b | 682 | Config_Save(); |
| maximbolduc | 26:dc00998140af | 683 | } |
| maximbolduc | 26:dc00998140af | 684 | else |
| maximbolduc | 26:dc00998140af | 685 | { |
| maximbolduc | 26:dc00998140af | 686 | } |
| maximbolduc | 26:dc00998140af | 687 | } |
| maximbolduc | 26:dc00998140af | 688 | |
| maximbolduc | 26:dc00998140af | 689 | void gps_analyse(char* gps_string) |
| maximbolduc | 26:dc00998140af | 690 | { |
| maximbolduc | 30:3afafa1ef16b | 691 | pc.puts(gps_string); |
| maximbolduc | 30:3afafa1ef16b | 692 | bluetooth.puts(gps_string); |
| maximbolduc | 26:dc00998140af | 693 | if (!strncmp(gps_string, "$GPRMC", 6)) |
| maximbolduc | 26:dc00998140af | 694 | { |
| maximbolduc | 26:dc00998140af | 695 | nmea_rmc(gps_string); //analyse and decompose the rmc string |
| maximbolduc | 26:dc00998140af | 696 | } |
| maximbolduc | 26:dc00998140af | 697 | } |
| maximbolduc | 26:dc00998140af | 698 | |
| jhedmonton | 27:9ac59b261d87 | 699 | int i2 = 0; |
| jhedmonton | 27:9ac59b261d87 | 700 | bool end2 = false; |
| jhedmonton | 27:9ac59b261d87 | 701 | bool start2 = false; |
| jhedmonton | 27:9ac59b261d87 | 702 | |
| jhedmonton | 27:9ac59b261d87 | 703 | bool getline2() |
| maximbolduc | 26:dc00998140af | 704 | { |
| jhedmonton | 27:9ac59b261d87 | 705 | int gotstring=false; |
| jhedmonton | 27:9ac59b261d87 | 706 | while (1) |
| jhedmonton | 27:9ac59b261d87 | 707 | { |
| jhedmonton | 27:9ac59b261d87 | 708 | if( !bluetooth.readable() ) |
| jhedmonton | 27:9ac59b261d87 | 709 | { |
| jhedmonton | 27:9ac59b261d87 | 710 | break; |
| jhedmonton | 27:9ac59b261d87 | 711 | } |
| jhedmonton | 27:9ac59b261d87 | 712 | char c = bluetooth.getc(); |
| jhedmonton | 27:9ac59b261d87 | 713 | if (c == 36 ){start2=true;end2 = false; i2 = 0;} |
| jhedmonton | 29:23ccb2a50b6f | 714 | if ((start2) && (c == 10)) |
| jhedmonton | 29:23ccb2a50b6f | 715 | { |
| jhedmonton | 29:23ccb2a50b6f | 716 | end2=true; |
| jhedmonton | 29:23ccb2a50b6f | 717 | start2 = false; |
| jhedmonton | 29:23ccb2a50b6f | 718 | } |
| jhedmonton | 29:23ccb2a50b6f | 719 | if (start2) |
| maximbolduc | 26:dc00998140af | 720 | { |
| jhedmonton | 27:9ac59b261d87 | 721 | msg2[i2]=c; |
| jhedmonton | 27:9ac59b261d87 | 722 | i2++; |
| jhedmonton | 27:9ac59b261d87 | 723 | if (i2>255) break; //protect msg buffer! |
| jhedmonton | 27:9ac59b261d87 | 724 | } |
| jhedmonton | 27:9ac59b261d87 | 725 | if (end2) |
| jhedmonton | 27:9ac59b261d87 | 726 | { |
| jhedmonton | 27:9ac59b261d87 | 727 | msg2[i2]=c; |
| jhedmonton | 27:9ac59b261d87 | 728 | msg2[i2+1] = 0; |
| jhedmonton | 27:9ac59b261d87 | 729 | start2 = false; |
| jhedmonton | 27:9ac59b261d87 | 730 | gotstring = true; |
| jhedmonton | 27:9ac59b261d87 | 731 | end2=false; |
| jhedmonton | 27:9ac59b261d87 | 732 | i2=0; |
| jhedmonton | 27:9ac59b261d87 | 733 | break; |
| maximbolduc | 26:dc00998140af | 734 | } |
| maximbolduc | 26:dc00998140af | 735 | } |
| jhedmonton | 27:9ac59b261d87 | 736 | return gotstring; |
| maximbolduc | 26:dc00998140af | 737 | } |
| maximbolduc | 26:dc00998140af | 738 | |
| jhedmonton | 27:9ac59b261d87 | 739 | |
| jhedmonton | 27:9ac59b261d87 | 740 | int i=0; |
| jhedmonton | 27:9ac59b261d87 | 741 | bool start=false; |
| jhedmonton | 27:9ac59b261d87 | 742 | bool end=false; |
| jhedmonton | 27:9ac59b261d87 | 743 | |
| jhedmonton | 27:9ac59b261d87 | 744 | bool getline(bool forward) |
| maximbolduc | 26:dc00998140af | 745 | { |
| jhedmonton | 27:9ac59b261d87 | 746 | while (1) |
| jhedmonton | 27:9ac59b261d87 | 747 | { |
| jhedmonton | 28:5905886c76ee | 748 | if( !gps.readable() ) |
| jhedmonton | 27:9ac59b261d87 | 749 | { |
| jhedmonton | 27:9ac59b261d87 | 750 | break; |
| jhedmonton | 27:9ac59b261d87 | 751 | } |
| jhedmonton | 28:5905886c76ee | 752 | char c = gps.getc(); |
| jhedmonton | 27:9ac59b261d87 | 753 | if (forward) //simply forward all to Bluetooth |
| maximbolduc | 26:dc00998140af | 754 | { |
| jhedmonton | 27:9ac59b261d87 | 755 | bluetooth.putc(c); |
| jhedmonton | 27:9ac59b261d87 | 756 | } |
| jhedmonton | 27:9ac59b261d87 | 757 | if (c == 36 ){start=true;end = false; i = 0;} |
| jhedmonton | 29:23ccb2a50b6f | 758 | if ((start) && (c == 10)) |
| jhedmonton | 29:23ccb2a50b6f | 759 | { |
| jhedmonton | 29:23ccb2a50b6f | 760 | end=true; |
| jhedmonton | 29:23ccb2a50b6f | 761 | start = false; |
| jhedmonton | 29:23ccb2a50b6f | 762 | } |
| jhedmonton | 27:9ac59b261d87 | 763 | if (start) |
| jhedmonton | 27:9ac59b261d87 | 764 | { |
| jhedmonton | 27:9ac59b261d87 | 765 | msg[i]=c; |
| jhedmonton | 27:9ac59b261d87 | 766 | i++; |
| jhedmonton | 27:9ac59b261d87 | 767 | if (i>255) break; //protect msg buffer! |
| jhedmonton | 27:9ac59b261d87 | 768 | } |
| jhedmonton | 27:9ac59b261d87 | 769 | if (end) |
| maximbolduc | 30:3afafa1ef16b | 770 | { |
| jhedmonton | 27:9ac59b261d87 | 771 | msg[i]=c; |
| maximbolduc | 26:dc00998140af | 772 | msg[i+1] = 0; |
| jhedmonton | 29:23ccb2a50b6f | 773 | i=0; |
| jhedmonton | 27:9ac59b261d87 | 774 | start = false; |
| jhedmonton | 27:9ac59b261d87 | 775 | end = true; |
| jhedmonton | 27:9ac59b261d87 | 776 | break; |
| maximbolduc | 26:dc00998140af | 777 | } |
| maximbolduc | 26:dc00998140af | 778 | } |
| jhedmonton | 27:9ac59b261d87 | 779 | return end; |
| maximbolduc | 26:dc00998140af | 780 | } |
| maximbolduc | 26:dc00998140af | 781 | |
| maximbolduc | 26:dc00998140af | 782 | void keyPressedHeld( void ) |
| maximbolduc | 26:dc00998140af | 783 | { |
| jhedmonton | 28:5905886c76ee | 784 | motor_enable_state = "$ENABLE,1\r\n"; |
| jhedmonton | 28:5905886c76ee | 785 | motor_enable = 1; |
| jhedmonton | 28:5905886c76ee | 786 | ledGREEN=1; //show green for being ready to steer |
| maximbolduc | 26:dc00998140af | 787 | } |
| maximbolduc | 26:dc00998140af | 788 | |
| maximbolduc | 26:dc00998140af | 789 | void keyReleasedHeld( void ) |
| maximbolduc | 26:dc00998140af | 790 | { |
| jhedmonton | 28:5905886c76ee | 791 | motor_enable_state = "$ENABLE,0\r\n"; |
| jhedmonton | 28:5905886c76ee | 792 | motor_enable = 0; |
| jhedmonton | 28:5905886c76ee | 793 | ledGREEN=0; |
| jhedmonton | 27:9ac59b261d87 | 794 | } |
| jhedmonton | 27:9ac59b261d87 | 795 | |
| jhedmonton | 27:9ac59b261d87 | 796 | void boom1PressedHeld( void ) |
| jhedmonton | 27:9ac59b261d87 | 797 | { |
| jhedmonton | 28:5905886c76ee | 798 | // ledGREEN=1; |
| jhedmonton | 27:9ac59b261d87 | 799 | boom18=boom18 & 0xFE; |
| jhedmonton | 27:9ac59b261d87 | 800 | } |
| jhedmonton | 27:9ac59b261d87 | 801 | |
| jhedmonton | 27:9ac59b261d87 | 802 | void boom1ReleasedHeld( void ) |
| jhedmonton | 27:9ac59b261d87 | 803 | { |
| jhedmonton | 28:5905886c76ee | 804 | //ledGREEN=0; |
| jhedmonton | 27:9ac59b261d87 | 805 | boom18=boom18 | 0x01; |
| jhedmonton | 27:9ac59b261d87 | 806 | } |
| jhedmonton | 27:9ac59b261d87 | 807 | |
| jhedmonton | 27:9ac59b261d87 | 808 | void boom2PressedHeld( void ) |
| jhedmonton | 27:9ac59b261d87 | 809 | { |
| jhedmonton | 27:9ac59b261d87 | 810 | boom18=boom18 & 0xFD; |
| maximbolduc | 26:dc00998140af | 811 | } |
| maximbolduc | 26:dc00998140af | 812 | |
| jhedmonton | 27:9ac59b261d87 | 813 | void boom2ReleasedHeld( void ) |
| jhedmonton | 27:9ac59b261d87 | 814 | { |
| jhedmonton | 27:9ac59b261d87 | 815 | boom18=boom18 | 0x02; |
| jhedmonton | 27:9ac59b261d87 | 816 | } |
| jhedmonton | 27:9ac59b261d87 | 817 | void boom3PressedHeld( void ) |
| jhedmonton | 27:9ac59b261d87 | 818 | { |
| jhedmonton | 27:9ac59b261d87 | 819 | boom18=boom18 & 0xFB; |
| jhedmonton | 27:9ac59b261d87 | 820 | } |
| jhedmonton | 27:9ac59b261d87 | 821 | |
| jhedmonton | 27:9ac59b261d87 | 822 | void boom3ReleasedHeld( void ) |
| jhedmonton | 27:9ac59b261d87 | 823 | { |
| jhedmonton | 27:9ac59b261d87 | 824 | |
| jhedmonton | 27:9ac59b261d87 | 825 | boom18=boom18 | 0x04; |
| jhedmonton | 27:9ac59b261d87 | 826 | } |
| jhedmonton | 27:9ac59b261d87 | 827 | |
| jhedmonton | 27:9ac59b261d87 | 828 | void boom4PressedHeld( void ) |
| jhedmonton | 27:9ac59b261d87 | 829 | { |
| jhedmonton | 27:9ac59b261d87 | 830 | |
| jhedmonton | 27:9ac59b261d87 | 831 | boom18=boom18 & 0xF7; |
| jhedmonton | 27:9ac59b261d87 | 832 | } |
| jhedmonton | 27:9ac59b261d87 | 833 | |
| jhedmonton | 27:9ac59b261d87 | 834 | void boom4ReleasedHeld( void ) |
| jhedmonton | 27:9ac59b261d87 | 835 | { |
| jhedmonton | 27:9ac59b261d87 | 836 | |
| jhedmonton | 27:9ac59b261d87 | 837 | boom18=boom18 | 0x08; |
| jhedmonton | 27:9ac59b261d87 | 838 | } |
| jhedmonton | 27:9ac59b261d87 | 839 | |
| maximbolduc | 26:dc00998140af | 840 | void toprint() |
| maximbolduc | 26:dc00998140af | 841 | { |
| maximbolduc | 26:dc00998140af | 842 | angle_send = 1; |
| maximbolduc | 26:dc00998140af | 843 | } |
| maximbolduc | 26:dc00998140af | 844 | |
| maximbolduc | 30:3afafa1ef16b | 845 | int getCheckSum(char *string) |
| maximbolduc | 26:dc00998140af | 846 | { |
| maximbolduc | 30:3afafa1ef16b | 847 | int i; |
| maximbolduc | 30:3afafa1ef16b | 848 | int XOR; |
| maximbolduc | 30:3afafa1ef16b | 849 | int c; |
| maximbolduc | 26:dc00998140af | 850 | // Calculate checksum ignoring any $'s in the string |
| maximbolduc | 30:3afafa1ef16b | 851 | for (XOR = 0, i = 0; i < strlen(string); i++) |
| maximbolduc | 30:3afafa1ef16b | 852 | { |
| maximbolduc | 30:3afafa1ef16b | 853 | c = (unsigned char)string[i]; |
| maximbolduc | 30:3afafa1ef16b | 854 | if (c == '*') break; |
| maximbolduc | 30:3afafa1ef16b | 855 | if (c != '$') XOR ^= c; |
| maximbolduc | 30:3afafa1ef16b | 856 | } |
| maximbolduc | 30:3afafa1ef16b | 857 | return XOR; |
| maximbolduc | 26:dc00998140af | 858 | } |
| maximbolduc | 26:dc00998140af | 859 | |
| maximbolduc | 26:dc00998140af | 860 | int main() |
| maximbolduc | 26:dc00998140af | 861 | { |
| jhedmonton | 27:9ac59b261d87 | 862 | int x=0; |
| jhedmonton | 27:9ac59b261d87 | 863 | bluetooth.baud(115200); |
| jhedmonton | 28:5905886c76ee | 864 | gps.baud(38400); |
| maximbolduc | 30:3afafa1ef16b | 865 | pc.baud(38400); |
| jhedmonton | 27:9ac59b261d87 | 866 | |
| jhedmonton | 27:9ac59b261d87 | 867 | //JH prepare and send version info |
| jhedmonton | 27:9ac59b261d87 | 868 | vTimer.start(); |
| jhedmonton | 27:9ac59b261d87 | 869 | vTimer.reset(); |
| jhedmonton | 28:5905886c76ee | 870 | motTimer.start(); |
| jhedmonton | 28:5905886c76ee | 871 | motTimer.reset(); |
| jhedmonton | 29:23ccb2a50b6f | 872 | lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s |
| jhedmonton | 29:23ccb2a50b6f | 873 | motor_enable_state = "$ENABLE,0\r\n"; |
| jhedmonton | 29:23ccb2a50b6f | 874 | |
| jhedmonton | 28:5905886c76ee | 875 | btTimer.start(); |
| jhedmonton | 28:5905886c76ee | 876 | btTimer.reset(); |
| jhedmonton | 28:5905886c76ee | 877 | lastgetBT= btTimer.read_ms(); |
| jhedmonton | 28:5905886c76ee | 878 | |
| jhedmonton | 27:9ac59b261d87 | 879 | pc.puts(version); |
| jhedmonton | 27:9ac59b261d87 | 880 | bluetooth.puts(version); |
| jhedmonton | 29:23ccb2a50b6f | 881 | lastsend_version=vTimer.read_ms()-18000; |
| jhedmonton | 27:9ac59b261d87 | 882 | |
| maximbolduc | 30:3afafa1ef16b | 883 | Config_Startup(); |
| maximbolduc | 30:3afafa1ef16b | 884 | _ID = Config_GetID(); |
| maximbolduc | 30:3afafa1ef16b | 885 | Config_Save(); |
| jhedmonton | 27:9ac59b261d87 | 886 | |
| jhedmonton | 27:9ac59b261d87 | 887 | boom1.attach_asserted_held( &boom1PressedHeld ); |
| jhedmonton | 27:9ac59b261d87 | 888 | boom1.attach_deasserted_held( &boom1ReleasedHeld ); |
| jhedmonton | 27:9ac59b261d87 | 889 | boom1.setSampleFrequency(); //default = 20 ms |
| jhedmonton | 27:9ac59b261d87 | 890 | boom1.setSamplesTillAssert(5); |
| jhedmonton | 27:9ac59b261d87 | 891 | boom1.setSamplesTillHeld(5); |
| jhedmonton | 27:9ac59b261d87 | 892 | boom2.attach_asserted_held( &boom2PressedHeld ); |
| jhedmonton | 27:9ac59b261d87 | 893 | boom2.attach_deasserted_held( &boom2ReleasedHeld ); |
| jhedmonton | 27:9ac59b261d87 | 894 | boom2.setSamplesTillAssert(5); |
| jhedmonton | 27:9ac59b261d87 | 895 | boom2.setSamplesTillHeld(5); |
| jhedmonton | 27:9ac59b261d87 | 896 | boom2.setSampleFrequency(); |
| jhedmonton | 27:9ac59b261d87 | 897 | boom3.attach_asserted_held( &boom3PressedHeld ); |
| jhedmonton | 27:9ac59b261d87 | 898 | boom3.attach_deasserted_held( &boom3ReleasedHeld ); |
| jhedmonton | 27:9ac59b261d87 | 899 | boom3.setSamplesTillAssert(5); |
| jhedmonton | 27:9ac59b261d87 | 900 | boom3.setSamplesTillHeld(5); |
| jhedmonton | 27:9ac59b261d87 | 901 | boom3.setSampleFrequency(); |
| jhedmonton | 27:9ac59b261d87 | 902 | boom4.attach_asserted_held( &boom4PressedHeld ); |
| jhedmonton | 27:9ac59b261d87 | 903 | boom4.attach_deasserted_held( &boom4ReleasedHeld ); |
| jhedmonton | 27:9ac59b261d87 | 904 | boom4.setSamplesTillAssert(5); |
| jhedmonton | 27:9ac59b261d87 | 905 | boom4.setSamplesTillHeld(5); |
| jhedmonton | 27:9ac59b261d87 | 906 | boom4.setSampleFrequency(); |
| jhedmonton | 27:9ac59b261d87 | 907 | |
| maximbolduc | 26:dc00998140af | 908 | motor_switch.setSampleFrequency(10000); |
| maximbolduc | 26:dc00998140af | 909 | motor_switch.attach_asserted_held( &keyPressedHeld ); |
| maximbolduc | 26:dc00998140af | 910 | motor_switch.attach_deasserted_held( &keyReleasedHeld ); |
| maximbolduc | 30:3afafa1ef16b | 911 | initializeAccelerometer(); |
| maximbolduc | 30:3afafa1ef16b | 912 | //calibrateAccelerometer(); |
| maximbolduc | 30:3afafa1ef16b | 913 | initializeGyroscope(); |
| maximbolduc | 30:3afafa1ef16b | 914 | calibrateGyroscope(); |
| maximbolduc | 30:3afafa1ef16b | 915 | accelerometerTicker.attach(&sampleAccelerometer, 0.005); |
| maximbolduc | 30:3afafa1ef16b | 916 | gyroscopeTicker.attach(&sampleGyroscope, 0.005); |
| maximbolduc | 30:3afafa1ef16b | 917 | filterTicker.attach(&filter, FILTER_RATE); |
| maximbolduc | 30:3afafa1ef16b | 918 | angle_print.attach(&toprint,0.2); |
| maximbolduc | 26:dc00998140af | 919 | activate_antenna(); |
| jhedmonton | 29:23ccb2a50b6f | 920 | |
| maximbolduc | 26:dc00998140af | 921 | while(1) |
| maximbolduc | 26:dc00998140af | 922 | { |
| jhedmonton | 27:9ac59b261d87 | 923 | //JH send version information every 10 seconds to keep Bluetooth alive |
| jhedmonton | 29:23ccb2a50b6f | 924 | if ((vTimer.read_ms()-lastsend_version)>25000) |
| maximbolduc | 26:dc00998140af | 925 | { |
| jhedmonton | 27:9ac59b261d87 | 926 | pc.puts(version); |
| jhedmonton | 27:9ac59b261d87 | 927 | bluetooth.puts(version); |
| jhedmonton | 27:9ac59b261d87 | 928 | vTimer.reset(); |
| jhedmonton | 27:9ac59b261d87 | 929 | lastsend_version=vTimer.read_ms(); |
| jhedmonton | 27:9ac59b261d87 | 930 | } |
| jhedmonton | 27:9ac59b261d87 | 931 | |
| jhedmonton | 28:5905886c76ee | 932 | if ( antenna_active == 1 && gps.readable()) |
| jhedmonton | 27:9ac59b261d87 | 933 | { |
| jhedmonton | 27:9ac59b261d87 | 934 | if (getline(true)) |
| maximbolduc | 26:dc00998140af | 935 | { |
| maximbolduc | 30:3afafa1ef16b | 936 | checksumm = getCheckSum(msg); |
| maximbolduc | 30:3afafa1ef16b | 937 | gps_analyse(msg); |
| maximbolduc | 26:dc00998140af | 938 | } |
| maximbolduc | 26:dc00998140af | 939 | } |
| maximbolduc | 26:dc00998140af | 940 | if ( bluetooth.readable()) |
| maximbolduc | 26:dc00998140af | 941 | { |
| jhedmonton | 27:9ac59b261d87 | 942 | if (getline2()) |
| jhedmonton | 27:9ac59b261d87 | 943 | { |
| jhedmonton | 28:5905886c76ee | 944 | btTimer.reset(); |
| jhedmonton | 28:5905886c76ee | 945 | lastgetBT= btTimer.read_ms(); |
| jhedmonton | 27:9ac59b261d87 | 946 | x++; |
| maximbolduc | 26:dc00998140af | 947 | trim(msg2," "); |
| jhedmonton | 27:9ac59b261d87 | 948 | sprintf(output,"%d %s",x,msg2); |
| maximbolduc | 26:dc00998140af | 949 | pc.puts(output); |
| maximbolduc | 26:dc00998140af | 950 | pc_analyse(msg2); |
| maximbolduc | 26:dc00998140af | 951 | } |
| maximbolduc | 26:dc00998140af | 952 | } |
| jhedmonton | 28:5905886c76ee | 953 | if ( btTimer.read_ms()-lastgetBT>1000) |
| jhedmonton | 28:5905886c76ee | 954 | { |
| jhedmonton | 28:5905886c76ee | 955 | //we did not get any commands over BT |
| jhedmonton | 28:5905886c76ee | 956 | ledRED=1; //turn red |
| jhedmonton | 28:5905886c76ee | 957 | } |
| jhedmonton | 28:5905886c76ee | 958 | else ledRED=0; |
| jhedmonton | 28:5905886c76ee | 959 | |
| jhedmonton | 29:23ccb2a50b6f | 960 | if ( ((motTimer.read_ms()-lastsend_motorstate)>8000) || (motor_enable!=lastmotor_enable)) |
| maximbolduc | 26:dc00998140af | 961 | { |
| jhedmonton | 28:5905886c76ee | 962 | |
| maximbolduc | 26:dc00998140af | 963 | bluetooth.puts(motor_enable_state); |
| jhedmonton | 28:5905886c76ee | 964 | pc.puts(motor_enable_state); |
| jhedmonton | 28:5905886c76ee | 965 | motTimer.reset(); |
| jhedmonton | 28:5905886c76ee | 966 | lastsend_motorstate=motTimer.read_ms(); |
| jhedmonton | 28:5905886c76ee | 967 | lastmotor_enable=motor_enable; |
| maximbolduc | 26:dc00998140af | 968 | } |
| jhedmonton | 27:9ac59b261d87 | 969 | |
| jhedmonton | 27:9ac59b261d87 | 970 | if (boom18!=lastboom18) |
| jhedmonton | 27:9ac59b261d87 | 971 | { |
| jhedmonton | 27:9ac59b261d87 | 972 | boomstate[4]=boom18 | 0x80; // |
| jhedmonton | 27:9ac59b261d87 | 973 | bluetooth.puts(boomstate); |
| jhedmonton | 27:9ac59b261d87 | 974 | pc.puts(boomstate); |
| jhedmonton | 27:9ac59b261d87 | 975 | lastboom18=boom18; |
| jhedmonton | 27:9ac59b261d87 | 976 | } |
| jhedmonton | 27:9ac59b261d87 | 977 | |
| maximbolduc | 30:3afafa1ef16b | 978 | if ( print_euler == 1 && angle_send == 1 ) //&& reading == 0) |
| maximbolduc | 26:dc00998140af | 979 | { |
| maximbolduc | 26:dc00998140af | 980 | sprintf(output,"$EULER,%f,%f,%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),toDegrees(imuFilter.getYaw())); |
| maximbolduc | 30:3afafa1ef16b | 981 | // pc.puts(output); |
| maximbolduc | 30:3afafa1ef16b | 982 | bluetooth.puts(output); |
| maximbolduc | 26:dc00998140af | 983 | angle_send = 0; |
| jhedmonton | 27:9ac59b261d87 | 984 | } |
| maximbolduc | 26:dc00998140af | 985 | } |
| maximbolduc | 26:dc00998140af | 986 | } |