1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 | #include "SoftwareSerial.h" SoftwareSerial mySerial(2,4);//Rx Tx char data[40]; int i = 0; double roll, pitch, yaw; void printAll(char* ch) { int n; for(n=0; n<i; n++){ Serial.print(ch[n]); } Serial.println(""); } void trans_ascii_to_num(char * SBuf, double * DegRoll, double * DegPitch, double * DegYaw) { int value, value2; value=FindComma(SBuf); //Serial.print(value); SBuf[value]='\0'; *DegRoll=atof(SBuf); value++; value2=FindComma(&SBuf[value]); SBuf[value+value2]='\0'; *DegPitch=atof(&SBuf[value]); value=value+value2+1; value2=FindComma(&SBuf[value]); SBuf[value+value2]='\0'; *DegYaw=atof(&SBuf[value]); Serial.print(*DegRoll); Serial.print(", ");Serial.print(*DegPitch); Serial.print(", ");Serial.println(*DegYaw); } int FindComma(char * buf) { int n; for(n=0;n<40;n++) { if(buf[n]==',') break; } return n; } void setup() { Serial.begin(115200); mySerial.begin(57600); } void loop() { if(mySerial.available()){ data[i]= mySerial.read(); i++; if(data[i-1] == 0x0A) data[i-1] = ','; if(data[i-1] == '*'){ // printAll(data); trans_ascii_to_num(data, &roll, &pitch, &yaw); i=0; } } } | cs |