2020NSE_SSLB4 / Mbed 2 deprecated GPS_STM32F303K8

GPS受信機から得たデータをCANに流すプログラム STM32F303K8

Dependencies:   mbed LSM303D

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?

UserRevisionLine numberNew 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 }