2020NSE_SSLB4
/
GPS_STM32F303K8
GPS受信機から得たデータをCANに流すプログラム STM32F303K8
main.cpp@3:a96f397dde4a, 2020-09-09 (annotated)
- Committer:
- sashida_h
- Date:
- Wed Sep 09 08:06:25 2020 +0000
- Revision:
- 3:a96f397dde4a
- Parent:
- 2:a217f4d56d32
[fix]bug
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sashida_h | 0:c0aa7d5cd549 | 1 | #include "mbed.h" |
sashida_h | 2:a217f4d56d32 | 2 | #include "LSM303D.h" |
sashida_h | 2:a217f4d56d32 | 3 | #include <math.h> |
sashida_h | 0:c0aa7d5cd549 | 4 | |
sashida_h | 0:c0aa7d5cd549 | 5 | DigitalOut myled(LED1); |
sashida_h | 0:c0aa7d5cd549 | 6 | Serial gps(PA_9, PA_10,38400); |
sashida_h | 2:a217f4d56d32 | 7 | Serial pc(PA_2, PA_3,38400); |
sashida_h | 0:c0aa7d5cd549 | 8 | CAN can(PA_11, PA_12); //pin21,22 rd,td |
sashida_h | 2:a217f4d56d32 | 9 | LSM303D lsm(PB_7,PB_6); |
sashida_h | 2:a217f4d56d32 | 10 | DigitalIn fpin(PB_1); |
sashida_h | 0:c0aa7d5cd549 | 11 | |
sashida_h | 0:c0aa7d5cd549 | 12 | CANMessage msg; |
sashida_h | 0:c0aa7d5cd549 | 13 | |
sashida_h | 2:a217f4d56d32 | 14 | float aData[3],mData[3]; |
sashida_h | 2:a217f4d56d32 | 15 | float m_x,m_y,m_z,a_x,a_y,a_z,x,y,z; |
sashida_h | 2:a217f4d56d32 | 16 | int i=0; |
sashida_h | 2:a217f4d56d32 | 17 | float max_x = -60.0; |
sashida_h | 2:a217f4d56d32 | 18 | float max_y = -60.0; |
sashida_h | 2:a217f4d56d32 | 19 | float max_z = -60.0; |
sashida_h | 2:a217f4d56d32 | 20 | float min_x = 60.0; |
sashida_h | 2:a217f4d56d32 | 21 | float min_y = 60.0; |
sashida_h | 2:a217f4d56d32 | 22 | float min_z = 60.0; |
sashida_h | 2:a217f4d56d32 | 23 | |
sashida_h | 2:a217f4d56d32 | 24 | float cal_x = -0.35207172; |
sashida_h | 2:a217f4d56d32 | 25 | float cal_y = 0.26154809; |
sashida_h | 2:a217f4d56d32 | 26 | float cal_z = 1.11167238; |
sashida_h | 2:a217f4d56d32 | 27 | |
sashida_h | 2:a217f4d56d32 | 28 | float yaw = 0; |
sashida_h | 2:a217f4d56d32 | 29 | float pitch = 0; |
sashida_h | 2:a217f4d56d32 | 30 | |
sashida_h | 0:c0aa7d5cd549 | 31 | union Float2Byte{ |
sashida_h | 0:c0aa7d5cd549 | 32 | float _float; |
sashida_h | 0:c0aa7d5cd549 | 33 | char _byte[4]; |
sashida_h | 0:c0aa7d5cd549 | 34 | }; |
sashida_h | 0:c0aa7d5cd549 | 35 | typedef union Float2Byte Float2Byte; |
sashida_h | 0:c0aa7d5cd549 | 36 | |
sashida_h | 2:a217f4d56d32 | 37 | union Int2Byte{ |
sashida_h | 3:a96f397dde4a | 38 | unsigned short _int; |
sashida_h | 2:a217f4d56d32 | 39 | char _byte[2]; |
sashida_h | 2:a217f4d56d32 | 40 | }; |
sashida_h | 2:a217f4d56d32 | 41 | typedef union Int2Byte Int2Byte; |
sashida_h | 2:a217f4d56d32 | 42 | |
sashida_h | 0:c0aa7d5cd549 | 43 | float _DMS2DEG(float raw_data){ |
sashida_h | 0:c0aa7d5cd549 | 44 | int d=(int)(raw_data/100); |
sashida_h | 0:c0aa7d5cd549 | 45 | float m=(raw_data-(float)d*100); |
sashida_h | 0:c0aa7d5cd549 | 46 | return (float)d+m/60; |
sashida_h | 0:c0aa7d5cd549 | 47 | } |
sashida_h | 0:c0aa7d5cd549 | 48 | |
sashida_h | 0:c0aa7d5cd549 | 49 | void send(float senddata,int id){ |
sashida_h | 0:c0aa7d5cd549 | 50 | //pc.printf("Master send()\n\r"); |
sashida_h | 0:c0aa7d5cd549 | 51 | |
sashida_h | 0:c0aa7d5cd549 | 52 | /*ID: 0x01*/ |
sashida_h | 0:c0aa7d5cd549 | 53 | Float2Byte sendFloat; |
sashida_h | 0:c0aa7d5cd549 | 54 | sendFloat._float = senddata; |
sashida_h | 0:c0aa7d5cd549 | 55 | //ここに送りたい値を入れる. |
sashida_h | 0:c0aa7d5cd549 | 56 | |
sashida_h | 0:c0aa7d5cd549 | 57 | char serialData[4]; |
sashida_h | 0:c0aa7d5cd549 | 58 | for(int i=0;i<4;++i){ |
sashida_h | 0:c0aa7d5cd549 | 59 | serialData[i] = sendFloat._byte[i]; |
sashida_h | 0:c0aa7d5cd549 | 60 | //pc.printf("send_char: %d\n\r", serialData[i]); |
sashida_h | 0:c0aa7d5cd549 | 61 | } |
sashida_h | 0:c0aa7d5cd549 | 62 | //pc.printf("sendFloat: %f\n\r", sendFloat._float); |
sashida_h | 0:c0aa7d5cd549 | 63 | if(can.write(CANMessage(id, serialData, 4))){ |
sashida_h | 2:a217f4d56d32 | 64 | //pc.printf("Send.\n\r"); |
sashida_h | 2:a217f4d56d32 | 65 | } |
sashida_h | 2:a217f4d56d32 | 66 | |
sashida_h | 2:a217f4d56d32 | 67 | |
sashida_h | 2:a217f4d56d32 | 68 | myled = !myled; |
sashida_h | 2:a217f4d56d32 | 69 | } |
sashida_h | 2:a217f4d56d32 | 70 | |
sashida_h | 3:a96f397dde4a | 71 | void send2(unsigned short senddata1,unsigned short senddata2,unsigned short senddata3,int id){ |
sashida_h | 2:a217f4d56d32 | 72 | //pc.printf("Master send()\n\r"); |
sashida_h | 2:a217f4d56d32 | 73 | |
sashida_h | 2:a217f4d56d32 | 74 | /*ID: 0x01*/ |
sashida_h | 2:a217f4d56d32 | 75 | Int2Byte sendInt; |
sashida_h | 2:a217f4d56d32 | 76 | sendInt._int = senddata1; |
sashida_h | 2:a217f4d56d32 | 77 | //ここに送りたい値を入れる. |
sashida_h | 2:a217f4d56d32 | 78 | |
sashida_h | 2:a217f4d56d32 | 79 | char serialData[6]; |
sashida_h | 2:a217f4d56d32 | 80 | |
sashida_h | 2:a217f4d56d32 | 81 | serialData[0] = sendInt._byte[0]; |
sashida_h | 2:a217f4d56d32 | 82 | serialData[1] = sendInt._byte[1]; |
sashida_h | 2:a217f4d56d32 | 83 | //pc.printf("send_char: %d\n\r", serialData[i]); |
sashida_h | 2:a217f4d56d32 | 84 | sendInt._int = senddata2; |
sashida_h | 2:a217f4d56d32 | 85 | serialData[2] = sendInt._byte[0]; |
sashida_h | 2:a217f4d56d32 | 86 | serialData[3] = sendInt._byte[1]; |
sashida_h | 2:a217f4d56d32 | 87 | |
sashida_h | 2:a217f4d56d32 | 88 | sendInt._int = senddata3; |
sashida_h | 2:a217f4d56d32 | 89 | serialData[4] = sendInt._byte[0]; |
sashida_h | 2:a217f4d56d32 | 90 | serialData[5] = sendInt._byte[1]; |
sashida_h | 2:a217f4d56d32 | 91 | //pc.printf("sendFloat: %f\n\r", sendFloat._float); |
sashida_h | 2:a217f4d56d32 | 92 | if(can.write(CANMessage(id, serialData, 6))){ |
sashida_h | 2:a217f4d56d32 | 93 | //pc.printf("Send.\n\r"); |
sashida_h | 0:c0aa7d5cd549 | 94 | } |
sashida_h | 0:c0aa7d5cd549 | 95 | |
sashida_h | 0:c0aa7d5cd549 | 96 | |
sashida_h | 0:c0aa7d5cd549 | 97 | myled = !myled; |
sashida_h | 0:c0aa7d5cd549 | 98 | } |
sashida_h | 0:c0aa7d5cd549 | 99 | |
sashida_h | 0:c0aa7d5cd549 | 100 | void receive(){ |
sashida_h | 0:c0aa7d5cd549 | 101 | Float2Byte getFloat; |
sashida_h | 3:a96f397dde4a | 102 | Int2Byte getInt; |
sashida_h | 0:c0aa7d5cd549 | 103 | |
sashida_h | 0:c0aa7d5cd549 | 104 | if(can.read(msg)){ |
sashida_h | 2:a217f4d56d32 | 105 | /*ID: 0x03*/ |
sashida_h | 2:a217f4d56d32 | 106 | |
sashida_h | 2:a217f4d56d32 | 107 | if(msg.id == 0x03){ |
sashida_h | 0:c0aa7d5cd549 | 108 | //pc.printf("ID: 0x01\n\r"); |
sashida_h | 0:c0aa7d5cd549 | 109 | for(int i=0;i<4;++i){ |
sashida_h | 0:c0aa7d5cd549 | 110 | getFloat._byte[i] = msg.data[i]; |
sashida_h | 0:c0aa7d5cd549 | 111 | //pc.printf("get_char: %d\n\r", getFloat._byte[i]); |
sashida_h | 0:c0aa7d5cd549 | 112 | } |
sashida_h | 2:a217f4d56d32 | 113 | pc.printf("IMU:%.2f\r\n", getFloat._float); |
sashida_h | 0:c0aa7d5cd549 | 114 | myled = !myled; |
sashida_h | 0:c0aa7d5cd549 | 115 | } |
sashida_h | 2:a217f4d56d32 | 116 | |
sashida_h | 0:c0aa7d5cd549 | 117 | /*ID: 0x02*/ |
sashida_h | 3:a96f397dde4a | 118 | if(msg.id == 0x06){ |
sashida_h | 0:c0aa7d5cd549 | 119 | //pc.printf("ID: 0x02\n\r"); |
sashida_h | 3:a96f397dde4a | 120 | getInt._byte[0] = msg.data[0]; |
sashida_h | 3:a96f397dde4a | 121 | getInt._byte[1] = msg.data[1]; |
sashida_h | 3:a96f397dde4a | 122 | pc.printf("HIKARI:%.2f", (float)getInt._int /10.0f); |
sashida_h | 3:a96f397dde4a | 123 | |
sashida_h | 3:a96f397dde4a | 124 | getInt._byte[0] = msg.data[2]; |
sashida_h | 3:a96f397dde4a | 125 | getInt._byte[1] = msg.data[3]; |
sashida_h | 3:a96f397dde4a | 126 | pc.printf(",%.2f", (float)getInt._int /10.0f); |
sashida_h | 3:a96f397dde4a | 127 | |
sashida_h | 3:a96f397dde4a | 128 | getInt._byte[0] = msg.data[4]; |
sashida_h | 3:a96f397dde4a | 129 | getInt._byte[1] = msg.data[5]; |
sashida_h | 3:a96f397dde4a | 130 | pc.printf(",%.2f\r\n", (float)getInt._int /10.0f); |
sashida_h | 3:a96f397dde4a | 131 | } |
sashida_h | 3:a96f397dde4a | 132 | |
sashida_h | 3:a96f397dde4a | 133 | if(msg.id == 0x05){ |
sashida_h | 3:a96f397dde4a | 134 | //pc.printf("ID: 0x05\n\r"); |
sashida_h | 3:a96f397dde4a | 135 | for(int i=0;i<6;++i){ |
sashida_h | 3:a96f397dde4a | 136 | msg.data[i]; |
sashida_h | 3:a96f397dde4a | 137 | pc.printf("%d,",msg.data[i]); |
sashida_h | 0:c0aa7d5cd549 | 138 | } |
sashida_h | 3:a96f397dde4a | 139 | pc.printf("\n\r"); |
sashida_h | 0:c0aa7d5cd549 | 140 | myled = !myled; |
sashida_h | 0:c0aa7d5cd549 | 141 | } |
sashida_h | 0:c0aa7d5cd549 | 142 | } |
sashida_h | 0:c0aa7d5cd549 | 143 | |
sashida_h | 0:c0aa7d5cd549 | 144 | } |
sashida_h | 0:c0aa7d5cd549 | 145 | |
sashida_h | 0:c0aa7d5cd549 | 146 | int main() { |
sashida_h | 0:c0aa7d5cd549 | 147 | char gps_data[256]; |
sashida_h | 0:c0aa7d5cd549 | 148 | int cnt_gps=0; |
sashida_h | 2:a217f4d56d32 | 149 | int c=0; |
sashida_h | 2:a217f4d56d32 | 150 | float world_time=0.0; |
sashida_h | 2:a217f4d56d32 | 151 | wait(1.0); |
sashida_h | 3:a96f397dde4a | 152 | pc.printf("Start%d\r\n",sizeof(c)); |
sashida_h | 0:c0aa7d5cd549 | 153 | can.attach(receive, CAN::RxIrq); |
sashida_h | 0:c0aa7d5cd549 | 154 | while(1){ |
sashida_h | 0:c0aa7d5cd549 | 155 | if(gps.readable()){ |
sashida_h | 0:c0aa7d5cd549 | 156 | gps_data[cnt_gps] = gps.getc(); |
sashida_h | 0:c0aa7d5cd549 | 157 | if(gps_data[cnt_gps] == '$' || cnt_gps ==256){ |
sashida_h | 0:c0aa7d5cd549 | 158 | cnt_gps = 0; |
sashida_h | 0:c0aa7d5cd549 | 159 | memset(gps_data,'\0',256); |
sashida_h | 0:c0aa7d5cd549 | 160 | }else if(gps_data[cnt_gps] == '\r'){ |
sashida_h | 0:c0aa7d5cd549 | 161 | float world_time, lon_east, lat_north; |
sashida_h | 0:c0aa7d5cd549 | 162 | int rlock, sat_num; |
sashida_h | 0:c0aa7d5cd549 | 163 | char lat,lon; |
sashida_h | 0:c0aa7d5cd549 | 164 | if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){ |
sashida_h | 0:c0aa7d5cd549 | 165 | if(rlock==1 || rlock==2){ |
sashida_h | 0:c0aa7d5cd549 | 166 | lat_north = _DMS2DEG(lat_north); |
sashida_h | 0:c0aa7d5cd549 | 167 | lon_east = _DMS2DEG(lon_east); |
sashida_h | 0:c0aa7d5cd549 | 168 | //pc.printf("%s\r\n",gps_data); |
sashida_h | 0:c0aa7d5cd549 | 169 | pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num); |
sashida_h | 0:c0aa7d5cd549 | 170 | //pc.printf("Lat:%f,Lon:%f,MAX_ALT:%f\r\n",lat_north,lon_east,alt_max); |
sashida_h | 0:c0aa7d5cd549 | 171 | send(world_time,0x01); |
sashida_h | 0:c0aa7d5cd549 | 172 | |
sashida_h | 0:c0aa7d5cd549 | 173 | }else{ |
sashida_h | 0:c0aa7d5cd549 | 174 | pc.printf("%s\r\n",gps_data); |
sashida_h | 2:a217f4d56d32 | 175 | if(sscanf(gps_data,"GPGGA,%f",&world_time)>=1){ |
sashida_h | 2:a217f4d56d32 | 176 | send(world_time,0x01); |
sashida_h | 2:a217f4d56d32 | 177 | }else{ |
sashida_h | 2:a217f4d56d32 | 178 | send(0.0,0x01); |
sashida_h | 2:a217f4d56d32 | 179 | } |
sashida_h | 2:a217f4d56d32 | 180 | |
sashida_h | 0:c0aa7d5cd549 | 181 | //pc.printf("NoGPSSignal\r\n"); |
sashida_h | 0:c0aa7d5cd549 | 182 | } |
sashida_h | 0:c0aa7d5cd549 | 183 | }else{ |
sashida_h | 0:c0aa7d5cd549 | 184 | pc.printf("No_Satellite_signal\r\n"); |
sashida_h | 2:a217f4d56d32 | 185 | send(0.0,0x01); |
sashida_h | 0:c0aa7d5cd549 | 186 | } |
sashida_h | 0:c0aa7d5cd549 | 187 | }else{ |
sashida_h | 0:c0aa7d5cd549 | 188 | cnt_gps++; |
sashida_h | 0:c0aa7d5cd549 | 189 | } |
sashida_h | 0:c0aa7d5cd549 | 190 | } |
sashida_h | 0:c0aa7d5cd549 | 191 | if(pc.readable()){ |
sashida_h | 2:a217f4d56d32 | 192 | /*pc.printf("kita!\r\n"); |
sashida_h | 0:c0aa7d5cd549 | 193 | while(1){ |
sashida_h | 0:c0aa7d5cd549 | 194 | c = pc.getc(); |
sashida_h | 0:c0aa7d5cd549 | 195 | pc.printf("%c",c); |
sashida_h | 0:c0aa7d5cd549 | 196 | } |
sashida_h | 2:a217f4d56d32 | 197 | */ |
sashida_h | 2:a217f4d56d32 | 198 | } |
sashida_h | 2:a217f4d56d32 | 199 | if(fpin == 0 && c == 0){ |
sashida_h | 2:a217f4d56d32 | 200 | lsm.read(aData,mData); |
sashida_h | 2:a217f4d56d32 | 201 | m_x = mData[0]/10.0f - cal_x; |
sashida_h | 2:a217f4d56d32 | 202 | m_y = mData[1]/10.0f - cal_y; |
sashida_h | 2:a217f4d56d32 | 203 | m_z = mData[2]/10.0f - cal_z; |
sashida_h | 2:a217f4d56d32 | 204 | |
sashida_h | 2:a217f4d56d32 | 205 | a_x = aData[0] / 100.0f; |
sashida_h | 2:a217f4d56d32 | 206 | a_y = aData[1] / 100.0f; |
sashida_h | 2:a217f4d56d32 | 207 | a_z = aData[2] / 100.0f; |
sashida_h | 2:a217f4d56d32 | 208 | |
sashida_h | 2:a217f4d56d32 | 209 | x = atan2(a_x, a_y); |
sashida_h | 2:a217f4d56d32 | 210 | x = x*180.0f/3.1415f; |
sashida_h | 2:a217f4d56d32 | 211 | y = atan2(0-a_z, (float)sqrt(a_x*a_x + a_y*a_y)); |
sashida_h | 2:a217f4d56d32 | 212 | y = y * 180.0f / 3.14159265f; |
sashida_h | 2:a217f4d56d32 | 213 | z = atan2((a_x*m_x - a_y*m_y)*sqrt(a_x*a_x + a_y*a_y + a_z*a_z) , (a_x*a_x + a_y*a_y)*m_z + a_z*(a_x*m_y + a_y*m_x)) ; |
sashida_h | 2:a217f4d56d32 | 214 | //z = atan2((a_x*a_x + a_y*a_y)*m_z + a_z*(a_x*m_y + a_y*m_x) , (a_x*m_x - a_y*m_y)*sqrt(a_x*a_x + a_y*a_y + a_z*a_z)) ; |
sashida_h | 2:a217f4d56d32 | 215 | z = z * 180.0f / 3.14159265f; |
sashida_h | 3:a96f397dde4a | 216 | |
sashida_h | 3:a96f397dde4a | 217 | if(x < 0)x+360.0; |
sashida_h | 3:a96f397dde4a | 218 | if(y < 0)y+360.0; |
sashida_h | 3:a96f397dde4a | 219 | if(z < 0)z+360.0; |
sashida_h | 2:a217f4d56d32 | 220 | //pc.printf("%.1f,%.1f,%.1f\r\n",x,y,z); |
sashida_h | 3:a96f397dde4a | 221 | send2((unsigned short)(x*10.0f),(unsigned short)(y*10.0f),(unsigned short)(z*10.0f),0x02); |
sashida_h | 3:a96f397dde4a | 222 | //send(world_time+(float)i*0.5,0x01); |
sashida_h | 3:a96f397dde4a | 223 | pc.printf("%d,%d,%d\r\n",(unsigned short)(x*10.0f),(unsigned short)(y*10.0f),(unsigned short)(z*10.0f)); |
sashida_h | 2:a217f4d56d32 | 224 | i++; |
sashida_h | 2:a217f4d56d32 | 225 | wait(0.5); |
sashida_h | 3:a96f397dde4a | 226 | if(i==5) c=1; |
sashida_h | 2:a217f4d56d32 | 227 | |
sashida_h | 0:c0aa7d5cd549 | 228 | } |
sashida_h | 0:c0aa7d5cd549 | 229 | } |
sashida_h | 0:c0aa7d5cd549 | 230 | } |