Kim Youngsik
/
modem_test
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.
Revision 0:a0f1d0891dfb, committed 2016-09-07
- Comitter:
- skyyoungsik
- Date:
- Wed Sep 07 06:16:54 2016 +0000
- Commit message:
- exens
Changed in this revision
diff -r 000000000000 -r a0f1d0891dfb data_receive.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/data_receive.h Wed Sep 07 06:16:54 2016 +0000
@@ -0,0 +1,62 @@
+
+#include "mbed.h"
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+////////////////////Receive/////////////////////////////////////
+volatile unsigned char gcs2fcs[120];
+volatile unsigned int mode1,mode2;
+volatile unsigned int gcs_chksum;
+volatile unsigned int GainP[20],GainD[20],GainI[20];
+volatile unsigned int marker_mode[20],marker_speed[20];
+volatile signed long marker_latitude[20],marker_longitude[20],marker_altitude[20];
+volatile double home_latitude, home_longitude, home_altitude;
+volatile unsigned int gcs_button[5] = {false,false,false,false,false};
+bool attitude_init,rc_init;
+int poi_theta, poi_r;
+void gcs_parsing(){
+ gcs_chksum = 0;
+ for(int i=2; i<20; i++){
+ gcs_chksum += gcs2fcs[i];
+ }
+ if(gcs_chksum == (gcs2fcs[20]*256 + gcs2fcs[21])){
+ switch(gcs2fcs[2]){
+ case 0:{ // Default//
+ switch(gcs2fcs[3]){
+ case 0:{ // Home Point //
+ break;}
+ case 1: { // Not Use ///
+ break;}
+ case 3: { ///Button//
+ myled4 = !myled4;
+ switch(gcs2fcs[4]){
+ case 1: gcs_button[0] = !gcs_button[0]; break;
+ case 2: gcs_button[1] = !gcs_button[1]; break;
+ case 4: gcs_button[2] = !gcs_button[2]; break;
+ case 8: gcs_button[3] = !gcs_button[3]; break;
+ case 16: gcs_button[4] = !gcs_button[4]; break;
+ }
+ break;}
+ case 4: { //Marker//
+ myled4 = !myled4;
+ marker_mode[gcs2fcs[4]-1] = gcs2fcs[5];
+ marker_latitude[gcs2fcs[4]-1] = (gcs2fcs[9]+gcs2fcs[8]*256+gcs2fcs[7]*65536+gcs2fcs[6]*16777216)-90000000;
+ marker_longitude[gcs2fcs[4]-1] = (gcs2fcs[13]+gcs2fcs[12]*256+gcs2fcs[11]*65536+gcs2fcs[10]*16777216)-180000000;
+ marker_altitude[gcs2fcs[4]-1] = ((gcs2fcs[15]+gcs2fcs[14]*256)-10000);
+ marker_speed[gcs2fcs[4]-1] = ((gcs2fcs[17]+gcs2fcs[16]*256));
+ break;}
+ }
+ break;}
+ case 4:{ // Gain //
+ if(gcs2fcs[3] == 2){ // PID
+ GainP[gcs2fcs[4]-1] = gcs2fcs[5];
+ GainD[gcs2fcs[4]-1] = gcs2fcs[6];
+ GainI[gcs2fcs[4]-1] = gcs2fcs[7];
+ }
+
+ break;}
+ }
+ }
+
+}
diff -r 000000000000 -r a0f1d0891dfb main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Sep 07 06:16:54 2016 +0000
@@ -0,0 +1,242 @@
+
+#include "mbed.h"
+#include "math.h"
+#include "data_receive.h"
+
+Ticker Timer1;
+RawSerial pc(USBTX, USBRX);
+
+volatile unsigned int timer_cnt,timer_1hz;
+unsigned char fcs2gcs[80];
+unsigned int fcs2gcs_trigger = 0;
+unsigned int fcs2gcs_order = 0;
+unsigned int fcs2gcs_chksum;
+unsigned int gps_time = 0;
+double gps_latitude = 35.123456;
+double gps_longitude = 129.123456;
+double gps_altitude = 123.10;
+float Roll,Pitch,Yaw;
+float Vx,Vy,Vz;
+signed int Roll_rate, Pitch_rate, Yaw_rate;
+unsigned int cap1,cap2,cap3,cap4,cap5,cap6,cap7,cap8;
+unsigned int pwm1,pwm2,pwm3,pwm4,pwm5,pwm6,pwm7,pwm8;
+signed int debug1,debug2,debug3,debug4,debug5,debug6,debug7,debug8;
+void Timer1_isr()
+{
+ timer_cnt ++;
+}
+volatile unsigned char uart_modem_buffer[4];
+volatile bool gcs_rcv_trigger,gcs_permit_divide;
+volatile unsigned int modem_isr_i;
+void uart_modem_isr(){
+ uart_modem_buffer[3] = uart_modem_buffer[2];
+ uart_modem_buffer[2] = uart_modem_buffer[1];
+ uart_modem_buffer[1] = uart_modem_buffer[0];
+ uart_modem_buffer[0] = pc.getc();
+
+ if(gcs_rcv_trigger == false){
+ if((uart_modem_buffer[0] == 254)&(uart_modem_buffer[1] == 254)){
+ gcs_rcv_trigger = true;
+ modem_isr_i = 1;
+ }
+ }
+ else{
+ modem_isr_i ++;
+ gcs2fcs[modem_isr_i] = uart_modem_buffer[0];
+ if((uart_modem_buffer[0]==255)&(uart_modem_buffer[1]==255)){
+ gcs_rcv_trigger = false;
+ gcs_permit_divide = 1;
+ }
+ }
+}
+
+
+int internal_cnt;
+bool change_chk;
+void packet()
+{
+ internal_cnt ++;
+ if(internal_cnt > 20){
+ internal_cnt = 0;
+ change_chk = true;
+ }else{
+ change_chk = false;
+ }
+
+ fcs2gcs[0] = 254;
+ fcs2gcs[1] = 254;
+ if(change_chk) mode1 ++;
+ if(mode1 >16) mode1 = 0;
+ mode2 = mode1;
+ fcs2gcs[2] = mode1*16 + mode2; // Mode //
+ if(change_chk) fcs2gcs[3] += 1;
+ fcs2gcs[3] = 0; // Translate Mode (Not use) //
+ if(change_chk) fcs2gcs[4] += 1;
+ if(fcs2gcs[4] > 16) fcs2gcs[4] = 0; // Mission //
+ if(change_chk) fcs2gcs[5] += 1;
+ if(fcs2gcs[5] > 20) fcs2gcs[5] = 0; // Current Marker //
+ fcs2gcs[6] = 0;
+ fcs2gcs[7] = 0;
+ fcs2gcs[8] = 112; // Bat 1 //
+ fcs2gcs[9] = 121; // Bat 2 //
+ fcs2gcs[10] = gcs_button[0]*1 + gcs_button[1]*2 + gcs_button[2]*4 + gcs_button[3] * 8 + gcs_button[4] * 16; // Button //
+ ////GPS///////
+ gps_time = gps_time + 5;
+ fcs2gcs[11] = gps_time/16777216; //ex)gps_time : 11223344 (hhmmss.ss)
+ fcs2gcs[12] = gps_time%16777216/65536;
+ fcs2gcs[13] = gps_time%65536/256;
+ fcs2gcs[14] = gps_time%256;
+
+ gps_latitude = 37.54065 + 0.01*sin(gps_time/100.0 *3.14/180);
+ gps_longitude = 127.079053 + 0.01*cos(gps_time/100.0 *3.14/180);
+ gps_altitude = 123.10 + 200*sin(gps_time/100.0*3.14/180);
+ fcs2gcs[15] = (gps_latitude*1000000+90000000)/16777216; //ex)gps_latitude : 35123456 (deg*1000000)
+ fcs2gcs[16] = ((unsigned long ) (gps_latitude*1000000+90000000))%16777216/65536;
+ fcs2gcs[17] = ((unsigned long ) (gps_latitude*1000000+90000000))%65536 / 256;
+ fcs2gcs[18] = ((unsigned long ) (gps_latitude*1000000+90000000))%256;
+ fcs2gcs[19] = ((unsigned long ) (gps_longitude*1000000+180000000))/16777216; //ex)gps_longitude : 129123456 (deg*1000000)
+ fcs2gcs[20] = ((unsigned long ) (gps_longitude*1000000+180000000))%16777216/65536;
+ fcs2gcs[21] = ((unsigned long ) (gps_longitude*1000000+180000000))%65536 / 256;
+ fcs2gcs[22] = ((unsigned long ) (gps_longitude*1000000+180000000))%256;
+ fcs2gcs[23] = ((unsigned long ) (gps_altitude*100+10000))/256;
+ fcs2gcs[24] = ((unsigned long ) (gps_altitude*100+10000))%256;
+ fcs2gcs[25] = 7;
+ /////////AHRS///////////
+ Roll = 0 + 30*sin(gps_time/10.0 *3.14/180);
+ Pitch = 0 - 30*sin((gps_time/10.0+2) *3.14/180);
+ Yaw = 0+ 150*sin(gps_time/50.0 *3.14/180);
+ fcs2gcs[26] = ((unsigned int)((Roll+180.00)*100))/256;
+ fcs2gcs[27] = ((unsigned int)((Roll+180.00)*100))%256;
+ fcs2gcs[28] = ((unsigned int)((Pitch+90.00)*100))/256;
+ fcs2gcs[29] = ((unsigned int)((Pitch+90.00)*100))%256;
+ fcs2gcs[30] = ((unsigned int)((Yaw+180.00)*100))/256;
+ fcs2gcs[31] = ((unsigned int)((Yaw+180.00)*100))%256;
+
+ Roll_rate = 0 + 500*sin(gps_time/10.0 *3.14/180);
+ Pitch_rate = 0 - 500*sin((gps_time/10.0+2) *3.14/180);
+ Yaw_rate = 0+ 1500*sin(gps_time/50.0 *3.14/180);
+ fcs2gcs[32] = ((unsigned int)((Roll_rate+32768)))/256;
+ fcs2gcs[33] = ((unsigned int)((Roll_rate+32768)))%256;
+ fcs2gcs[34] = ((unsigned int)((Pitch_rate+32768)))/256;
+ fcs2gcs[35] = ((unsigned int)((Pitch_rate+32768)))%256;
+ fcs2gcs[36] = ((unsigned int)((Yaw_rate+32768)))/256;
+ fcs2gcs[37] = ((unsigned int)((Yaw_rate+32768)))%256;
+
+ Vx = 0 + 30*sin(gps_time/10.0 *3.14/180);
+ Vy = 0 - 30*sin((gps_time/10.0+2) *3.14/180);
+ Vz = 0+ 30*sin(gps_time/50.0 *3.14/180);
+ fcs2gcs[38] = ((unsigned int)(Vx*100+32768))/256;
+ fcs2gcs[39] = ((unsigned int)(Vx*100+32768))%256;
+ fcs2gcs[40] = ((unsigned int)(Vy*100+32768))/256;
+ fcs2gcs[41] = ((unsigned int)(Vy*100+32768))%256;
+ fcs2gcs[42] = ((unsigned int)(Vz*100+32768))/256;
+ fcs2gcs[43] = ((unsigned int)(Vz*100+32768))%256;
+
+ fcs2gcs[44] = 100 + 100*sin(gps_time/100.0 *3.14/180); // Cap1
+ fcs2gcs[45] = 100 + 100*cos(gps_time/100.0 *3.14/180); // Cap2
+ fcs2gcs[46] = 100 + 100*sin(gps_time/100.0 *3.14/180); // Cap3
+ fcs2gcs[47] = 100 + 100*cos(gps_time/100.0 *3.14/180); // Cap4
+ fcs2gcs[48] = 100 + 100*sin(gps_time/100.0 *3.14/180); // Cap5
+ fcs2gcs[49] = 100 + 100*cos(gps_time/100.0 *3.14/180); // Cap6
+ fcs2gcs[50] = 100 + 100*sin(gps_time/100.0 *3.14/180); // Cap7
+ fcs2gcs[51] = 100 + 100*cos(gps_time/100.0 *3.14/180); // Cap8
+
+ fcs2gcs[52] = 100 + 100*cos(gps_time/100.0 *3.14/180); // PWM1
+ fcs2gcs[53] = 100 + 100*sin(gps_time/100.0 *3.14/180); // PWM2
+ fcs2gcs[54] = 100 + 100*cos(gps_time/100.0 *3.14/180); // PWM3
+ fcs2gcs[55] = 100 + 100*sin(gps_time/100.0 *3.14/180); // PWM4
+ fcs2gcs[56] = 100 + 100*cos(gps_time/100.0 *3.14/180); // PWM5
+ fcs2gcs[57] = 100 + 100*sin(gps_time/100.0 *3.14/180); // PWM6
+ fcs2gcs[58] = 100 + 100*cos(gps_time/100.0 *3.14/180); // PWM7
+ fcs2gcs[59] = 100 + 100*sin(gps_time/100.0 *3.14/180); // PWM8
+
+ //tilt//
+ float tilt = 90*sin(gps_time/10.0*3.14/180);
+ fcs2gcs[60] = (unsigned int)((unsigned int)((tilt + 180)*100) / 256);
+ fcs2gcs[61] = (unsigned int)((unsigned int)((tilt + 180)*100) % 256);
+
+ float sa_theta = (gps_time/10)%360;
+ fcs2gcs[62] = (unsigned int)(sa_theta * 100) / 256;
+ fcs2gcs[63] = (unsigned int)(sa_theta * 100) % 256;
+
+ float sa_distance = 250 + 250*sin(gps_time/10.0*3.14/180);
+ fcs2gcs[64] = (unsigned int)(sa_distance) / 256;
+ fcs2gcs[65] = (unsigned int)(sa_distance) % 256;
+
+ debug1 = (125 + 125*sin(gps_time/10.0 *3.14/180)); // DEBUG1
+ debug2 = (125 + 125*cos(gps_time/10.0 *3.14/180)); // DEBUG2
+ debug3 = (125 + 125*sin(gps_time/10.0 *3.14/180)); // DEBUG3
+ debug4 = (125 + 125*cos(gps_time/10.0 *3.14/180)); // DEBUG4
+ debug5 = (125 + 125*sin(gps_time/10.0 *3.14/180)); // DEBUG5
+ debug6 = (125 + 125*cos(gps_time/10.0 *3.14/180)); // DEBUG6
+ debug7 = (30000*sin(gps_time/10.0 *3.14/180)); // DEBUG7
+ debug8 = (30000*cos(gps_time/10.0 *3.14/180)); // DEBUG8
+
+ debug1 = GainP[0];
+ debug2 = GainD[0];
+ debug3 = GainI[0];
+ debug4 = GainP[19];
+ debug5 = GainD[19];
+ debug6 = GainI[19];
+
+ fcs2gcs[66] = (debug1)%256;
+ fcs2gcs[67] = (debug2)%256;
+ fcs2gcs[68] = (debug3)%256;
+ fcs2gcs[69] = (debug4)%256;
+ fcs2gcs[70] = (debug5)%256;
+ fcs2gcs[71] = (debug6)%256;
+ fcs2gcs[72] = (debug7+31250)/256;
+ fcs2gcs[73] = (debug7+31250)%256;
+ fcs2gcs[74] = (debug8+31250)/256;
+ fcs2gcs[75] = (debug8+31250)%256;
+
+
+ fcs2gcs_chksum =0;
+ for(int i=2;i<=75;i++){
+ fcs2gcs_chksum += fcs2gcs[i];
+ }
+ fcs2gcs[76] = fcs2gcs_chksum / 256;
+ fcs2gcs[77] = fcs2gcs_chksum % 256;
+ fcs2gcs[78]=255;
+ fcs2gcs[79]=255;
+}
+int main()
+{
+ Timer1.attach(&Timer1_isr, 0.01); //10Hz//
+ pc.baud(115200);
+ pc.attach(&uart_modem_isr);
+ while(1)
+ {
+ if(timer_cnt >= 5) //20Hz//
+ {
+ packet();
+ fcs2gcs_trigger = 1;
+ timer_1hz ++;
+ myled1 = !myled1;
+ timer_cnt = 0;
+ }
+ if(timer_1hz >= 20){ //1Hz//
+ myled2 = !myled2;
+ timer_1hz = 0;
+ }
+
+ ////Trans Data/////
+ if(fcs2gcs_trigger )
+ {
+ pc.putc(fcs2gcs[fcs2gcs_order]);
+ fcs2gcs_order ++;
+
+ if(fcs2gcs_order > 79)
+ {
+ fcs2gcs_order = 0;
+ fcs2gcs_trigger = 0;
+ }
+ }
+ ///////////////
+ ////Receive Data////
+ if(gcs_permit_divide){
+ gcs_permit_divide = 0;
+ gcs_parsing();
+ }
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r a0f1d0891dfb mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Sep 07 06:16:54 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/87f2f5183dfb \ No newline at end of file