Sarosa Castrena Abadi / Mbed OS GPR-OS2

gpr

Dependencies:   MbedJSONValue QEI

main.cpp@13:40cf48007328, 18 months ago (annotated)

Committer:
castrena23
Date:
Sun Nov 06 09:56:10 2022 +0000
Revision:
13:40cf48007328
Parent:
12:19b1755d34e9 
gpr

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emilmont 1:491820ee784d 1 #include "mbed.h"
aldyduino 12:19b1755d34e9 2 #include "stdio.h"
aldyduino 12:19b1755d34e9 3 #include "MbedJSONValue.h"
castrena23 13:40cf48007328 4 #include "QEI.h"
aldyduino 12:19b1755d34e9 5 #include "string.h"
aldyduino 12:19b1755d34e9 6 #include "stdint.h"
aldyduino 12:19b1755d34e9 7 #include <cstdlib>
aldyduino 12:19b1755d34e9 8 #define NUM_FIELDS 3
castrena23 13:40cf48007328 9 #define MAX_BUFFER 500
castrena23 13:40cf48007328 10
castrena23 13:40cf48007328 11
aldyduino 12:19b1755d34e9 12
geky 7:8d9919175929 13 Thread thread;
castrena23 13:40cf48007328 14 //Ticker timer;
aldyduino 12:19b1755d34e9 15
aldyduino 12:19b1755d34e9 16 Serial pc(USBTX,USBRX);
aldyduino 12:19b1755d34e9 17 AnalogOut DataDAC(p18);
aldyduino 12:19b1755d34e9 18 Serial gps(p13, p14);
castrena23 13:40cf48007328 19 QEI wheel (p29, p30, NC, 250, QEI::X4_ENCODING);
castrena23 13:40cf48007328 20
aldyduino 12:19b1755d34e9 21
aldyduino 12:19b1755d34e9 22 char eDataBuffer[500];
aldyduino 12:19b1755d34e9 23 int i = 0;
aldyduino 12:19b1755d34e9 24 int h_time,m_time,s_time;
aldyduino 12:19b1755d34e9 25 char testStringBuffer[MAX_BUFFER]={0};
aldyduino 12:19b1755d34e9 26 bool stringComplete = false;
castrena23 13:40cf48007328 27 double DAC_Value;
aldyduino 12:19b1755d34e9 28 int len = 0;
aldyduino 12:19b1755d34e9 29 //number of comma seperated values in the data...TODO does this remain constant?
aldyduino 12:19b1755d34e9 30 float latitude, longitude, timefix, speed, altitude;
aldyduino 12:19b1755d34e9 31 char ns, ew;
aldyduino 12:19b1755d34e9 32 void Init();
aldyduino 12:19b1755d34e9 33 void parseGPS(char *cmd, int n);
aldyduino 12:19b1755d34e9 34 void parseTime (float timeval);
aldyduino 12:19b1755d34e9 35 void baca_gps();
castrena23 13:40cf48007328 36 void input_DAC();
castrena23 13:40cf48007328 37
castrena23 13:40cf48007328 38
castrena23 13:40cf48007328 39
aldyduino 12:19b1755d34e9 40
aldyduino 12:19b1755d34e9 41 void clearBuffer(char* buffer , int len){
aldyduino 12:19b1755d34e9 42 for (int i =0; i<len ; i++){
aldyduino 12:19b1755d34e9 43 buffer[i] = '\0';
emilmont 1:491820ee784d 44 }
emilmont 1:491820ee784d 45 }
aldyduino 12:19b1755d34e9 46
aldyduino 12:19b1755d34e9 47 void onSerialInput() {
aldyduino 12:19b1755d34e9 48 if(pc.readable()){
aldyduino 12:19b1755d34e9 49 char inChar = pc.getc();
aldyduino 12:19b1755d34e9 50 //pc.printf("%c",inChar);
aldyduino 12:19b1755d34e9 51 testStringBuffer[len] = inChar;
aldyduino 12:19b1755d34e9 52 len++;
aldyduino 12:19b1755d34e9 53 if (inChar == '\n') {
castrena23 13:40cf48007328 54 // pc.printf("%s",testStringBuffer);
aldyduino 12:19b1755d34e9 55 stringComplete = true;
aldyduino 12:19b1755d34e9 56 len = 0;
aldyduino 12:19b1755d34e9 57 }
castrena23 13:40cf48007328 58 }
aldyduino 12:19b1755d34e9 59 }
castrena23 13:40cf48007328 60
aldyduino 12:19b1755d34e9 61 void Init()
aldyduino 12:19b1755d34e9 62 {
aldyduino 12:19b1755d34e9 63 gps.baud(9600);
aldyduino 12:19b1755d34e9 64 pc.baud(115200);
aldyduino 12:19b1755d34e9 65 pc.attach(&onSerialInput);
castrena23 13:40cf48007328 66 gps.attach(&baca_gps);
aldyduino 12:19b1755d34e9 67 pc.printf("Init OK\n");
aldyduino 12:19b1755d34e9 68 }
aldyduino 12:19b1755d34e9 69
castrena23 13:40cf48007328 70 float baca_enc()
castrena23 13:40cf48007328 71 {
castrena23 13:40cf48007328 72 float lp;
castrena23 13:40cf48007328 73 int pul =wheel.getPulses();
castrena23 13:40cf48007328 74 lp = (pul / 4*250)*0.04 ;
castrena23 13:40cf48007328 75 return lp;
castrena23 13:40cf48007328 76 }
castrena23 13:40cf48007328 77
castrena23 13:40cf48007328 78 void reset_enc()
castrena23 13:40cf48007328 79 {
castrena23 13:40cf48007328 80 wheel.reset();
castrena23 13:40cf48007328 81 }
castrena23 13:40cf48007328 82
aldyduino 12:19b1755d34e9 83 void parseTime (float timeval)
aldyduino 12:19b1755d34e9 84 {
aldyduino 12:19b1755d34e9 85 //format utc time to beijing time,add 8 time zone
aldyduino 12:19b1755d34e9 86 float time = timeval + 80000.00f;
aldyduino 12:19b1755d34e9 87 h_time = int(time) / 10000;
aldyduino 12:19b1755d34e9 88 m_time = (int(time) % 10000) / 100;
aldyduino 12:19b1755d34e9 89 s_time = int(time) % 100;
aldyduino 12:19b1755d34e9 90 }
aldyduino 12:19b1755d34e9 91
aldyduino 12:19b1755d34e9 92
aldyduino 12:19b1755d34e9 93 /*
aldyduino 12:19b1755d34e9 94 * NMEA sentences: https://www.gpsinformation.org/dale/nmea.htm#nmea
aldyduino 12:19b1755d34e9 95 * http://navspark.mybigcommerce.com/content/NMEA_Format_v0.1.pdf
aldyduino 12:19b1755d34e9 96 */
aldyduino 12:19b1755d34e9 97 void parseGPS(char *cmd, int n)
aldyduino 12:19b1755d34e9 98 {
aldyduino 12:19b1755d34e9 99 char tf, status;
aldyduino 12:19b1755d34e9 100 int fq, nst, fix, date; // fix quality, Number of satellites being tracked, 3D fix
aldyduino 12:19b1755d34e9 101
aldyduino 12:19b1755d34e9 102
aldyduino 12:19b1755d34e9 103 // Global Positioning System Fix Data
aldyduino 12:19b1755d34e9 104 if(strncmp(cmd,"$GPGGA", 6) == 0)
aldyduino 12:19b1755d34e9 105 {
aldyduino 12:19b1755d34e9 106 sscanf(cmd, "$GPGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
aldyduino 12:19b1755d34e9 107 //pc.printf("Latitude: %f %c, Longitude: %f %c \n", latitude, ns, longitude, ew);
aldyduino 12:19b1755d34e9 108
aldyduino 12:19b1755d34e9 109
aldyduino 12:19b1755d34e9 110 }
emilmont 1:491820ee784d 111
aldyduino 12:19b1755d34e9 112 // Satellite status
aldyduino 12:19b1755d34e9 113 else if(strncmp(cmd,"$GPGSA", 6) == 0)
aldyduino 12:19b1755d34e9 114 {
aldyduino 12:19b1755d34e9 115 sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst);
castrena23 13:40cf48007328 116 //pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
aldyduino 12:19b1755d34e9 117 }
aldyduino 12:19b1755d34e9 118
aldyduino 12:19b1755d34e9 119 // Geographic position, Latitude and Longitude
aldyduino 12:19b1755d34e9 120 else if(strncmp(cmd,"$GPGLL", 6) == 0)
aldyduino 12:19b1755d34e9 121 {
aldyduino 12:19b1755d34e9 122 sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
aldyduino 12:19b1755d34e9 123 //pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c\n", latitude, ns, longitude, ew);
aldyduino 12:19b1755d34e9 124 }
aldyduino 12:19b1755d34e9 125
aldyduino 12:19b1755d34e9 126 // Geographic position, Latitude and Longitude
aldyduino 12:19b1755d34e9 127 else if(strncmp(cmd,"$GPRMC", 6) == 0)
aldyduino 12:19b1755d34e9 128 {
aldyduino 12:19b1755d34e9 129 sscanf(cmd, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,,%d", &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date);
aldyduino 12:19b1755d34e9 130 //pc.printf("GPRMC Latitude: %f %c, Longitude: %f %c\n", latitude, ns, longitude, ew);
aldyduino 12:19b1755d34e9 131 }
aldyduino 12:19b1755d34e9 132
aldyduino 12:19b1755d34e9 133 //
aldyduino 12:19b1755d34e9 134 else if(strncmp(cmd,"$GNVTG", 6) == 0)
aldyduino 12:19b1755d34e9 135 {
aldyduino 12:19b1755d34e9 136 // pc.printf("its a Vector Track message.\n");
aldyduino 12:19b1755d34e9 137 }
aldyduino 12:19b1755d34e9 138
aldyduino 12:19b1755d34e9 139 else if(strncmp(cmd,"$GNGGA", 6) == 0)
aldyduino 12:19b1755d34e9 140 {
aldyduino 12:19b1755d34e9 141 sscanf(cmd, "$GNGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
aldyduino 12:19b1755d34e9 142 //pc.printf("GNGGA Latitude: %f %c, Longitude: %f %c \n", latitude, ns, longitude, ew);
aldyduino 12:19b1755d34e9 143 parseTime(timefix);
aldyduino 12:19b1755d34e9 144 // pc.printf("Time: %d:%d:%d\n", h_time, m_time, s_time);
aldyduino 12:19b1755d34e9 145 }
aldyduino 12:19b1755d34e9 146
aldyduino 12:19b1755d34e9 147 else if(strncmp(cmd,"$GNGSA", 6) == 0)
aldyduino 12:19b1755d34e9 148 {
aldyduino 12:19b1755d34e9 149 sscanf(cmd, "$GNGSA,%c,%d,%d", &tf, &fix, &nst);
aldyduino 12:19b1755d34e9 150 //pc.printf("GNGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
aldyduino 12:19b1755d34e9 151 }
aldyduino 12:19b1755d34e9 152
aldyduino 12:19b1755d34e9 153 else if(strncmp(cmd,"$GPGSV", 6) == 0)
aldyduino 12:19b1755d34e9 154 {
aldyduino 12:19b1755d34e9 155 // pc.printf("its a Satellite details message.\n");
aldyduino 12:19b1755d34e9 156 }
aldyduino 12:19b1755d34e9 157
aldyduino 12:19b1755d34e9 158 else if(strncmp(cmd,"$GNGLL", 6) == 0)
aldyduino 12:19b1755d34e9 159 {
aldyduino 12:19b1755d34e9 160 sscanf(cmd, "$GNGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
castrena23 13:40cf48007328 161 //pc.printf("GNGLL Latitude: %f %c, Longitude: %f %c \n", latitude, ns, longitude, ew);
aldyduino 12:19b1755d34e9 162 }
aldyduino 12:19b1755d34e9 163
aldyduino 12:19b1755d34e9 164 else
aldyduino 12:19b1755d34e9 165 {
aldyduino 12:19b1755d34e9 166 // pc.printf("Unknown message type\n");
emilmont 1:491820ee784d 167 }
emilmont 1:491820ee784d 168 }
castrena23 13:40cf48007328 169 /*void input_DAC(){
castrena23 13:40cf48007328 170 DataDAC = DAC_Value;
castrena23 13:40cf48007328 171 }*/
aldyduino 12:19b1755d34e9 172
aldyduino 12:19b1755d34e9 173 char e;
aldyduino 12:19b1755d34e9 174 void baca_gps()
aldyduino 12:19b1755d34e9 175 {
aldyduino 12:19b1755d34e9 176 while(gps.readable())
aldyduino 12:19b1755d34e9 177 {
aldyduino 12:19b1755d34e9 178
aldyduino 12:19b1755d34e9 179 if(gps.getc() == '$'); // wait a $
aldyduino 12:19b1755d34e9 180 {
aldyduino 12:19b1755d34e9 181 for(int i=0; i<sizeof(eDataBuffer); i++)
aldyduino 12:19b1755d34e9 182 {
aldyduino 12:19b1755d34e9 183 e = gps.getc();
aldyduino 12:19b1755d34e9 184 if( e == '\r' )
aldyduino 12:19b1755d34e9 185 {
aldyduino 12:19b1755d34e9 186 pc.printf("%s\n", eDataBuffer);
aldyduino 12:19b1755d34e9 187 parseGPS(eDataBuffer, i);
aldyduino 12:19b1755d34e9 188 i = sizeof(eDataBuffer);
aldyduino 12:19b1755d34e9 189 }
aldyduino 12:19b1755d34e9 190 else
aldyduino 12:19b1755d34e9 191 {
aldyduino 12:19b1755d34e9 192 eDataBuffer[i] = e;
aldyduino 12:19b1755d34e9 193
aldyduino 12:19b1755d34e9 194 }
aldyduino 12:19b1755d34e9 195 }
aldyduino 12:19b1755d34e9 196 }
aldyduino 12:19b1755d34e9 197 }
aldyduino 12:19b1755d34e9 198
aldyduino 12:19b1755d34e9 199 }
castrena23 13:40cf48007328 200 #define TOTAL_DATA 20
aldyduino 12:19b1755d34e9 201 bool threadFlag = false;
aldyduino 12:19b1755d34e9 202 MbedJSONValue threadDAC;
aldyduino 12:19b1755d34e9 203 void DAC_Thread(){
castrena23 13:40cf48007328 204 int periode;
castrena23 13:40cf48007328 205 int counterDAC = 0;
castrena23 13:40cf48007328 206 while (true) {
aldyduino 12:19b1755d34e9 207 if (threadFlag){
castrena23 13:40cf48007328 208
aldyduino 12:19b1755d34e9 209 periode = threadDAC["periode"].get<int>();
castrena23 13:40cf48007328 210 float m = periode/20;
castrena23 13:40cf48007328 211
castrena23 13:40cf48007328 212 DAC_Value = threadDAC["value"][counterDAC].get<double>();
castrena23 13:40cf48007328 213
castrena23 13:40cf48007328 214 //for (int k=0;k<20;k++){
castrena23 13:40cf48007328 215 DataDAC = DAC_Value;
castrena23 13:40cf48007328 216 wait_us(m*100);
castrena23 13:40cf48007328 217
castrena23 13:40cf48007328 218 //}
castrena23 13:40cf48007328 219
castrena23 13:40cf48007328 220 counterDAC++;
castrena23 13:40cf48007328 221 if(counterDAC>=TOTAL_DATA){
castrena23 13:40cf48007328 222 counterDAC = 0;
castrena23 13:40cf48007328 223 //continue;
castrena23 13:40cf48007328 224 }
castrena23 13:40cf48007328 225
aldyduino 12:19b1755d34e9 226 }
castrena23 13:40cf48007328 227
castrena23 13:40cf48007328 228
castrena23 13:40cf48007328 229
castrena23 13:40cf48007328 230 else {
aldyduino 12:19b1755d34e9 231 DataDAC = 0;
castrena23 13:40cf48007328 232 counterDAC = 0;
castrena23 13:40cf48007328 233
aldyduino 12:19b1755d34e9 234 }
aldyduino 12:19b1755d34e9 235
aldyduino 12:19b1755d34e9 236 }
aldyduino 12:19b1755d34e9 237 }
castrena23 13:40cf48007328 238
aldyduino 12:19b1755d34e9 239 int main()
aldyduino 12:19b1755d34e9 240 {
aldyduino 12:19b1755d34e9 241 //float e[5]={0};
aldyduino 12:19b1755d34e9 242 Init();
aldyduino 12:19b1755d34e9 243 MbedJSONValue json;
aldyduino 12:19b1755d34e9 244 MbedJSONValue jsonGPS;
castrena23 13:40cf48007328 245 MbedJSONValue jsonENCO;
aldyduino 12:19b1755d34e9 246 thread.start(DAC_Thread);
aldyduino 12:19b1755d34e9 247 std::string buffer;
aldyduino 12:19b1755d34e9 248
aldyduino 12:19b1755d34e9 249 while(1){
aldyduino 12:19b1755d34e9 250 if (stringComplete){
aldyduino 12:19b1755d34e9 251 parse(json,testStringBuffer);
aldyduino 12:19b1755d34e9 252 if (json.hasMember("type")){
castrena23 13:40cf48007328 253 if(json["type"].get<std::string>() == "STOP"){
castrena23 13:40cf48007328 254
castrena23 13:40cf48007328 255 threadFlag = false;
castrena23 13:40cf48007328 256 DataDAC = 0;
castrena23 13:40cf48007328 257 wait(1);
castrena23 13:40cf48007328 258 NVIC_SystemReset();
castrena23 13:40cf48007328 259
castrena23 13:40cf48007328 260
castrena23 13:40cf48007328 261 }
castrena23 13:40cf48007328 262 else if (json["type"].get<std::string>() == "DAC"){
castrena23 13:40cf48007328 263
aldyduino 12:19b1755d34e9 264 threadDAC = json;
castrena23 13:40cf48007328 265 threadFlag = true;
castrena23 13:40cf48007328 266
aldyduino 12:19b1755d34e9 267 }
castrena23 13:40cf48007328 268 else if(json["type"].get<std::string>() == "ENCO"){
castrena23 13:40cf48007328 269 jsonENCO["type"] = "ENCO";
castrena23 13:40cf48007328 270 jsonENCO["Pos"] = baca_enc();
castrena23 13:40cf48007328 271 buffer = jsonENCO.serialize();
castrena23 13:40cf48007328 272 pc.printf("%s\n",buffer.c_str());
castrena23 13:40cf48007328 273 //wait(0.5);
castrena23 13:40cf48007328 274 }
castrena23 13:40cf48007328 275 else if(json["type"].get<std::string>() == "ENCO_RESET"){
castrena23 13:40cf48007328 276 reset_enc();
aldyduino 12:19b1755d34e9 277 }
aldyduino 12:19b1755d34e9 278 else if(json["type"].get<std::string>() == "GPS"){
aldyduino 12:19b1755d34e9 279 jsonGPS["type"] = "GPS";
aldyduino 12:19b1755d34e9 280 jsonGPS["lat"] = latitude;
aldyduino 12:19b1755d34e9 281 jsonGPS["ns"] = ns;
aldyduino 12:19b1755d34e9 282 jsonGPS["long"] = longitude;
aldyduino 12:19b1755d34e9 283 jsonGPS["ew"] = ew;
aldyduino 12:19b1755d34e9 284 buffer = jsonGPS.serialize();
aldyduino 12:19b1755d34e9 285 pc.printf("%s\n",buffer.c_str());
castrena23 13:40cf48007328 286
aldyduino 12:19b1755d34e9 287
aldyduino 12:19b1755d34e9 288 }
aldyduino 12:19b1755d34e9 289
aldyduino 12:19b1755d34e9 290 else {
aldyduino 12:19b1755d34e9 291 pc.printf("Type is not available\n");
aldyduino 12:19b1755d34e9 292 }
aldyduino 12:19b1755d34e9 293 stringComplete = false;
aldyduino 12:19b1755d34e9 294 clearBuffer(testStringBuffer,MAX_BUFFER);
aldyduino 12:19b1755d34e9 295 }
aldyduino 12:19b1755d34e9 296 else{
aldyduino 12:19b1755d34e9 297 pc.printf("Unknown JSON Message \n");
aldyduino 12:19b1755d34e9 298 stringComplete = false;
aldyduino 12:19b1755d34e9 299 clearBuffer(testStringBuffer,MAX_BUFFER);
aldyduino 12:19b1755d34e9 300 }
aldyduino 12:19b1755d34e9 301
aldyduino 12:19b1755d34e9 302
aldyduino 12:19b1755d34e9 303 }
castrena23 13:40cf48007328 304
aldyduino 12:19b1755d34e9 305 }
aldyduino 12:19b1755d34e9 306 }