2020NSE_SSLB4 / Mbed 2 deprecated GPS_STM32F303K8

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

Dependencies:   mbed LSM303D

main.cpp@2:a217f4d56d32, 2020-09-09 (annotated)

Committer:
sashida_h
Date:
Wed Sep 09 02:54:36 2020 +0000
Revision:
2:a217f4d56d32
Parent:
1:03c3b1db6179
Child:
3:a96f397dde4a
20200909;

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 2:a217f4d56d32 38 int _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 2:a217f4d56d32 71 void send2(int senddata1,int senddata2,int 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 0:c0aa7d5cd549 102
sashida_h 0:c0aa7d5cd549 103 if(can.read(msg)){
sashida_h 2:a217f4d56d32 104 /*ID: 0x03*/
sashida_h 2:a217f4d56d32 105
sashida_h 2:a217f4d56d32 106 if(msg.id == 0x03){
sashida_h 0:c0aa7d5cd549 107 //pc.printf("ID: 0x01\n\r");
sashida_h 0:c0aa7d5cd549 108 for(int i=0;i<4;++i){
sashida_h 0:c0aa7d5cd549 109 getFloat._byte[i] = msg.data[i];
sashida_h 0:c0aa7d5cd549 110 //pc.printf("get_char: %d\n\r", getFloat._byte[i]);
sashida_h 0:c0aa7d5cd549 111 }
sashida_h 2:a217f4d56d32 112 pc.printf("IMU:%.2f\r\n", getFloat._float);
sashida_h 0:c0aa7d5cd549 113 myled = !myled;
sashida_h 0:c0aa7d5cd549 114 }
sashida_h 2:a217f4d56d32 115
sashida_h 0:c0aa7d5cd549 116 /*ID: 0x02*/
sashida_h 0:c0aa7d5cd549 117 if(msg.id == 0x02){
sashida_h 0:c0aa7d5cd549 118 //pc.printf("ID: 0x02\n\r");
sashida_h 0:c0aa7d5cd549 119 for(int i=0;i<4;++i){
sashida_h 0:c0aa7d5cd549 120 getFloat._byte[i] = msg.data[i];
sashida_h 0:c0aa7d5cd549 121 //pc.printf("get_char: %d\n\r", getFloat._byte[i]);
sashida_h 0:c0aa7d5cd549 122 }
sashida_h 2:a217f4d56d32 123 pc.printf("HIKARI:%.1f\n\r", getFloat._float);
sashida_h 0:c0aa7d5cd549 124 myled = !myled;
sashida_h 0:c0aa7d5cd549 125 }
sashida_h 0:c0aa7d5cd549 126 }
sashida_h 0:c0aa7d5cd549 127
sashida_h 0:c0aa7d5cd549 128 }
sashida_h 0:c0aa7d5cd549 129
sashida_h 0:c0aa7d5cd549 130 int main() {
sashida_h 0:c0aa7d5cd549 131 char gps_data[256];
sashida_h 0:c0aa7d5cd549 132 int cnt_gps=0;
sashida_h 2:a217f4d56d32 133 int c=0;
sashida_h 2:a217f4d56d32 134 float world_time=0.0;
sashida_h 2:a217f4d56d32 135 wait(1.0);
sashida_h 0:c0aa7d5cd549 136 pc.printf("Start\r\n");
sashida_h 0:c0aa7d5cd549 137 can.attach(receive, CAN::RxIrq);
sashida_h 0:c0aa7d5cd549 138 while(1){
sashida_h 0:c0aa7d5cd549 139 if(gps.readable()){
sashida_h 0:c0aa7d5cd549 140 gps_data[cnt_gps] = gps.getc();
sashida_h 0:c0aa7d5cd549 141 if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
sashida_h 0:c0aa7d5cd549 142 cnt_gps = 0;
sashida_h 0:c0aa7d5cd549 143 memset(gps_data,'\0',256);
sashida_h 0:c0aa7d5cd549 144 }else if(gps_data[cnt_gps] == '\r'){
sashida_h 0:c0aa7d5cd549 145 float world_time, lon_east, lat_north;
sashida_h 0:c0aa7d5cd549 146 int rlock, sat_num;
sashida_h 0:c0aa7d5cd549 147 char lat,lon;
sashida_h 0:c0aa7d5cd549 148 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 149 if(rlock==1 || rlock==2){
sashida_h 0:c0aa7d5cd549 150 lat_north = _DMS2DEG(lat_north);
sashida_h 0:c0aa7d5cd549 151 lon_east = _DMS2DEG(lon_east);
sashida_h 0:c0aa7d5cd549 152 //pc.printf("%s\r\n",gps_data);
sashida_h 0:c0aa7d5cd549 153 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 154 //pc.printf("Lat:%f,Lon:%f,MAX_ALT:%f\r\n",lat_north,lon_east,alt_max);
sashida_h 0:c0aa7d5cd549 155 send(world_time,0x01);
sashida_h 0:c0aa7d5cd549 156
sashida_h 0:c0aa7d5cd549 157 }else{
sashida_h 0:c0aa7d5cd549 158 pc.printf("%s\r\n",gps_data);
sashida_h 2:a217f4d56d32 159 if(sscanf(gps_data,"GPGGA,%f",&world_time)>=1){
sashida_h 2:a217f4d56d32 160 send(world_time,0x01);
sashida_h 2:a217f4d56d32 161 }else{
sashida_h 2:a217f4d56d32 162 send(0.0,0x01);
sashida_h 2:a217f4d56d32 163 }
sashida_h 2:a217f4d56d32 164
sashida_h 0:c0aa7d5cd549 165 //pc.printf("NoGPSSignal\r\n");
sashida_h 0:c0aa7d5cd549 166 }
sashida_h 0:c0aa7d5cd549 167 }else{
sashida_h 0:c0aa7d5cd549 168 pc.printf("No_Satellite_signal\r\n");
sashida_h 2:a217f4d56d32 169 send(0.0,0x01);
sashida_h 0:c0aa7d5cd549 170 }
sashida_h 0:c0aa7d5cd549 171 }else{
sashida_h 0:c0aa7d5cd549 172 cnt_gps++;
sashida_h 0:c0aa7d5cd549 173 }
sashida_h 0:c0aa7d5cd549 174 }
sashida_h 0:c0aa7d5cd549 175 if(pc.readable()){
sashida_h 2:a217f4d56d32 176 /*pc.printf("kita!\r\n");
sashida_h 0:c0aa7d5cd549 177 while(1){
sashida_h 0:c0aa7d5cd549 178 c = pc.getc();
sashida_h 0:c0aa7d5cd549 179 pc.printf("%c",c);
sashida_h 0:c0aa7d5cd549 180 }
sashida_h 2:a217f4d56d32 181 */
sashida_h 2:a217f4d56d32 182 }
sashida_h 2:a217f4d56d32 183 if(fpin == 0 && c == 0){
sashida_h 2:a217f4d56d32 184 lsm.read(aData,mData);
sashida_h 2:a217f4d56d32 185 m_x = mData[0]/10.0f - cal_x;
sashida_h 2:a217f4d56d32 186 m_y = mData[1]/10.0f - cal_y;
sashida_h 2:a217f4d56d32 187 m_z = mData[2]/10.0f - cal_z;
sashida_h 2:a217f4d56d32 188
sashida_h 2:a217f4d56d32 189 a_x = aData[0] / 100.0f;
sashida_h 2:a217f4d56d32 190 a_y = aData[1] / 100.0f;
sashida_h 2:a217f4d56d32 191 a_z = aData[2] / 100.0f;
sashida_h 2:a217f4d56d32 192
sashida_h 2:a217f4d56d32 193 x = atan2(a_x, a_y);
sashida_h 2:a217f4d56d32 194 x = x*180.0f/3.1415f;
sashida_h 2:a217f4d56d32 195 y = atan2(0-a_z, (float)sqrt(a_x*a_x + a_y*a_y));
sashida_h 2:a217f4d56d32 196 y = y * 180.0f / 3.14159265f;
sashida_h 2:a217f4d56d32 197 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 198 //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 199 z = z * 180.0f / 3.14159265f;
sashida_h 2:a217f4d56d32 200 //pc.printf("%.1f,%.1f,%.1f\r\n",x,y,z);
sashida_h 2:a217f4d56d32 201 send2((int)(x*100.0f),(int)(y*100.0f),(int)(z*100.0f),0x02);
sashida_h 2:a217f4d56d32 202 send(world_time+(float)i*0.5,0x01);
sashida_h 2:a217f4d56d32 203 pc.printf("%d,%d,%d\r\n",(int)(x*100.0f),(int)(y*100.0f),(int)(z*100.0f));
sashida_h 2:a217f4d56d32 204 i++;
sashida_h 2:a217f4d56d32 205 wait(0.5);
sashida_h 2:a217f4d56d32 206 if(i==10) c=1;
sashida_h 2:a217f4d56d32 207
sashida_h 0:c0aa7d5cd549 208 }
sashida_h 0:c0aa7d5cd549 209 }
sashida_h 0:c0aa7d5cd549 210 }