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