CanSat2015aizu / Mbed 2 deprecated Cansat_program2

2015.07.23

Dependencies:   ADXL345 CanSat_aoki HMC5883L ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed

Fork of Cansat_program by CanSat2015aizu

main.cpp@1:f1f7413ae6bd, 2015-07-11 (annotated)

Committer:
kityann
Date:
Sat Jul 11 06:35:56 2015 +0000
Revision:
1:f1f7413ae6bd
Parent:
0:1fcc61be1dcf
Child:
3:0bd9ad37f319
2015/07/11

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:1fcc61be1dcf 1 /**********************************************/
kityann 0:1fcc61be1dcf 2 //更新情報
kityann 0:1fcc61be1dcf 3
kityann 0:1fcc61be1dcf 4 /**********************************************/
kityann 0:1fcc61be1dcf 5
kityann 0:1fcc61be1dcf 6 #include "mbed.h"
kityann 0:1fcc61be1dcf 7 #include "XBee.h"
kityann 0:1fcc61be1dcf 8 #include "MBed_Adafruit_GPS.h"
kityann 1:f1f7413ae6bd 9 //#include "AigamozuControlPackets.h"
kityann 0:1fcc61be1dcf 10 #include "agzIDLIST.h"
kityann 0:1fcc61be1dcf 11 #include "aigamozuSetting.h"
kityann 1:f1f7413ae6bd 12 #include "HMC5883L.h"
kityann 1:f1f7413ae6bd 13 #include "VNH5019.h"
kityann 0:1fcc61be1dcf 14 #include "math.h"
kityann 0:1fcc61be1dcf 15
kityann 0:1fcc61be1dcf 16 #define SIGMA_MIN 0.0001
kityann 0:1fcc61be1dcf 17
kityann 0:1fcc61be1dcf 18 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 19 //
kityann 0:1fcc61be1dcf 20 //Pin Setting
kityann 0:1fcc61be1dcf 21 //
kityann 0:1fcc61be1dcf 22 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 23 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:1fcc61be1dcf 24
kityann 0:1fcc61be1dcf 25
kityann 0:1fcc61be1dcf 26 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 27 //
kityann 0:1fcc61be1dcf 28 //Connection Setting
kityann 0:1fcc61be1dcf 29 //
kityann 0:1fcc61be1dcf 30 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 31
kityann 0:1fcc61be1dcf 32 //Serial Connect Setting: PC <--> mbed
kityann 0:1fcc61be1dcf 33 Serial pc(USBTX, USBRX);
kityann 0:1fcc61be1dcf 34
kityann 0:1fcc61be1dcf 35 //Serial Connect Setting: GPS <--> mbed
kityann 0:1fcc61be1dcf 36 Serial * gps_Serial;
kityann 0:1fcc61be1dcf 37
kityann 0:1fcc61be1dcf 38 //Serial Connect Setting: XBEE <--> mbed
kityann 0:1fcc61be1dcf 39 XBee xbee(p13,p14);
kityann 0:1fcc61be1dcf 40 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:1fcc61be1dcf 41
kityann 0:1fcc61be1dcf 42 //set up GPS module
kityann 0:1fcc61be1dcf 43
kityann 0:1fcc61be1dcf 44 //set up AigamozuControlPackets library
kityann 1:f1f7413ae6bd 45 //AigamozuControlPackets agz(agz_motorShield);
kityann 0:1fcc61be1dcf 46
kityann 0:1fcc61be1dcf 47
kityann 0:1fcc61be1dcf 48 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 49 //
kityann 0:1fcc61be1dcf 50 //For Kalman data
kityann 0:1fcc61be1dcf 51 //
kityann 0:1fcc61be1dcf 52 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 53 #define FIRST_S2_1 1.0e-8
kityann 0:1fcc61be1dcf 54 #define FIRST_S2_2 1.0e-6
kityann 0:1fcc61be1dcf 55 #define COUNTER_MAX 10000
kityann 0:1fcc61be1dcf 56 #define ERROR_RANGE 0.001
kityann 0:1fcc61be1dcf 57
kityann 0:1fcc61be1dcf 58 double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
kityann 0:1fcc61be1dcf 59 double s2x_cur=FIRST_S2_1,s2x_prev=FIRST_S2_1,s2y_cur=FIRST_S2_1,s2y_prev=FIRST_S2_1;//緯度経度のの時刻tと時刻t-1での共分散
kityann 0:1fcc61be1dcf 60 double s2_R=FIRST_S2_2;//GPSセンサの分散
kityann 0:1fcc61be1dcf 61 double s2_Q=FIRST_S2_2;
kityann 0:1fcc61be1dcf 62 double Kx=0,Ky=0;//カルマンゲイン
kityann 0:1fcc61be1dcf 63 double zx,zy;//観測値
kityann 0:1fcc61be1dcf 64 void Kalman(double Latitude,double Longitude);
kityann 0:1fcc61be1dcf 65 int change = 0;
kityann 0:1fcc61be1dcf 66
kityann 0:1fcc61be1dcf 67
kityann 0:1fcc61be1dcf 68
kityann 0:1fcc61be1dcf 69 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 70 //
kityann 0:1fcc61be1dcf 71 //Kalman Processing
kityann 0:1fcc61be1dcf 72 //
kityann 0:1fcc61be1dcf 73 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 74 void calc_Kalman(){
kityann 0:1fcc61be1dcf 75 //calc Kalman gain
kityann 0:1fcc61be1dcf 76 Kx = (s2x_prev+s2_Q)/(s2x_prev+s2_R+s2_Q);
kityann 0:1fcc61be1dcf 77 Ky = (s2y_prev+s2_Q)/(s2y_prev+s2_R+s2_Q);
kityann 0:1fcc61be1dcf 78 //estimate
kityann 0:1fcc61be1dcf 79 x_cur = x_prev + Kx*(zx-x_prev);
kityann 0:1fcc61be1dcf 80 y_cur = y_prev + Ky*(zy-y_prev);
kityann 0:1fcc61be1dcf 81 //calc sigma
kityann 0:1fcc61be1dcf 82 s2x_cur = (1-Kx)*(s2x_prev+s2_Q);
kityann 0:1fcc61be1dcf 83 s2y_cur = (1-Ky)*(s2y_prev+s2_Q);
kityann 0:1fcc61be1dcf 84
kityann 0:1fcc61be1dcf 85 }
kityann 0:1fcc61be1dcf 86
kityann 0:1fcc61be1dcf 87 void Kalman(double Latitude,double Longitude){
kityann 0:1fcc61be1dcf 88
kityann 0:1fcc61be1dcf 89 zx = Latitude;
kityann 0:1fcc61be1dcf 90 zy = Longitude;
kityann 0:1fcc61be1dcf 91
kityann 0:1fcc61be1dcf 92 calc_Kalman();
kityann 0:1fcc61be1dcf 93
kityann 0:1fcc61be1dcf 94 //更新
kityann 0:1fcc61be1dcf 95 x_prev = x_cur;
kityann 0:1fcc61be1dcf 96 y_prev = y_cur;
kityann 0:1fcc61be1dcf 97 s2x_prev = s2x_cur;
kityann 0:1fcc61be1dcf 98 s2y_prev = s2y_cur;
kityann 0:1fcc61be1dcf 99
kityann 0:1fcc61be1dcf 100 //agzPontKalmanとagzCovに格納する
kityann 1:f1f7413ae6bd 101 //agz.set_agzPointKalman_lati(x_cur);
kityann 1:f1f7413ae6bd 102 //agz.set_agzPointKalman_longi(y_cur);
kityann 1:f1f7413ae6bd 103 //agz.set_agzCov(s2x_cur,s2y_cur);
kityann 0:1fcc61be1dcf 104
kityann 0:1fcc61be1dcf 105 }
kityann 0:1fcc61be1dcf 106
kityann 0:1fcc61be1dcf 107 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 108 //
kityann 0:1fcc61be1dcf 109 //Get GPS function
kityann 0:1fcc61be1dcf 110 //
kityann 0:1fcc61be1dcf 111 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 112
kityann 0:1fcc61be1dcf 113 void Get_GPS(Adafruit_GPS *myGPS){
kityann 0:1fcc61be1dcf 114 static int flag = 0;
kityann 0:1fcc61be1dcf 115
kityann 0:1fcc61be1dcf 116 if (myGPS->fix) {
kityann 0:1fcc61be1dcf 117
kityann 1:f1f7413ae6bd 118 //agz.nowStatus = GPS_AVAIL;
kityann 1:f1f7413ae6bd 119 //agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
kityann 0:1fcc61be1dcf 120
kityann 0:1fcc61be1dcf 121 if(flag < COUNTER_MAX){
kityann 0:1fcc61be1dcf 122 flag++;
kityann 0:1fcc61be1dcf 123 }
kityann 0:1fcc61be1dcf 124 if(flag == 5){
kityann 1:f1f7413ae6bd 125 //x_prev = agz.get_agzPoint_lati();
kityann 1:f1f7413ae6bd 126 //y_prev = agz.get_agzPoint_longi();
kityann 0:1fcc61be1dcf 127 }
kityann 0:1fcc61be1dcf 128
kityann 0:1fcc61be1dcf 129 if(flag >= 6){
kityann 1:f1f7413ae6bd 130 if(1){//abs(x_prev - agz.get_agzPoint_lati()) < ERROR_RANGE && abs(y_prev - agz.get_agzPoint_longi()) < ERROR_RANGE){
kityann 1:f1f7413ae6bd 131 //Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
kityann 0:1fcc61be1dcf 132 change = 1;
kityann 0:1fcc61be1dcf 133 }
kityann 0:1fcc61be1dcf 134 else{
kityann 0:1fcc61be1dcf 135 change = 0;
kityann 0:1fcc61be1dcf 136 }
kityann 1:f1f7413ae6bd 137 //printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
kityann 1:f1f7413ae6bd 138 //agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 1:f1f7413ae6bd 139 //agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 1:f1f7413ae6bd 140 //agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 1:f1f7413ae6bd 141 }
kityann 0:1fcc61be1dcf 142 }
kityann 1:f1f7413ae6bd 143 else ;//agz.nowStatus = GPS_UNAVAIL;
kityann 0:1fcc61be1dcf 144
kityann 0:1fcc61be1dcf 145 }
kityann 0:1fcc61be1dcf 146
kityann 0:1fcc61be1dcf 147 /******************************
kityann 0:1fcc61be1dcf 148 スタンバイモード
kityann 0:1fcc61be1dcf 149 ******************************/
kityann 0:1fcc61be1dcf 150 void standby(){
kityann 0:1fcc61be1dcf 151 ;
kityann 0:1fcc61be1dcf 152 }
kityann 0:1fcc61be1dcf 153
kityann 0:1fcc61be1dcf 154 /******************************
kityann 0:1fcc61be1dcf 155 落下モード
kityann 0:1fcc61be1dcf 156 ******************************/
kityann 0:1fcc61be1dcf 157 void falling(){
kityann 0:1fcc61be1dcf 158
kityann 0:1fcc61be1dcf 159 }
kityann 0:1fcc61be1dcf 160
kityann 0:1fcc61be1dcf 161 /******************************
kityann 0:1fcc61be1dcf 162 走行モード
kityann 0:1fcc61be1dcf 163 ******************************/
kityann 0:1fcc61be1dcf 164 void running(){
kityann 0:1fcc61be1dcf 165
kityann 0:1fcc61be1dcf 166 }
kityann 0:1fcc61be1dcf 167
kityann 0:1fcc61be1dcf 168 /******************************
kityann 0:1fcc61be1dcf 169 停止モード
kityann 0:1fcc61be1dcf 170 ******************************/
kityann 0:1fcc61be1dcf 171 void stopping(){
kityann 0:1fcc61be1dcf 172
kityann 0:1fcc61be1dcf 173
kityann 0:1fcc61be1dcf 174 }
kityann 0:1fcc61be1dcf 175
kityann 0:1fcc61be1dcf 176 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 177 //
kityann 0:1fcc61be1dcf 178 //Main Processing
kityann 0:1fcc61be1dcf 179 //
kityann 0:1fcc61be1dcf 180 /////////////////////////////////////////
kityann 0:1fcc61be1dcf 181 int main() {
kityann 0:1fcc61be1dcf 182 //start up time
kityann 0:1fcc61be1dcf 183 wait(3);
kityann 0:1fcc61be1dcf 184 //set pc frequency to 57600bps
kityann 0:1fcc61be1dcf 185 pc.baud(PC_BAUD_RATE);
kityann 0:1fcc61be1dcf 186 //set xbee frequency to 57600bps
kityann 0:1fcc61be1dcf 187 xbee.begin(XBEE_BAUD_RATE);
kityann 0:1fcc61be1dcf 188
kityann 0:1fcc61be1dcf 189
kityann 0:1fcc61be1dcf 190 //GPS setting
kityann 0:1fcc61be1dcf 191 gps_Serial = new Serial(p28,p27);
kityann 0:1fcc61be1dcf 192 Adafruit_GPS myGPS(gps_Serial);
kityann 0:1fcc61be1dcf 193 Timer refresh_Timer;
kityann 0:1fcc61be1dcf 194 const int refresh_Time = 1000; //refresh time in ms
kityann 0:1fcc61be1dcf 195 int count = 0;
kityann 0:1fcc61be1dcf 196
kityann 0:1fcc61be1dcf 197 myGPS.begin(GPS_BAUD_RATE);
kityann 0:1fcc61be1dcf 198
kityann 0:1fcc61be1dcf 199 //GPS Send Command
kityann 0:1fcc61be1dcf 200 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:1fcc61be1dcf 201 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:1fcc61be1dcf 202 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:1fcc61be1dcf 203
kityann 0:1fcc61be1dcf 204 wait_ms(2000);
kityann 0:1fcc61be1dcf 205
kityann 0:1fcc61be1dcf 206 //interrupt start
kityann 0:1fcc61be1dcf 207 refresh_Timer.start();
kityann 0:1fcc61be1dcf 208
kityann 0:1fcc61be1dcf 209 printf("start\n");
kityann 0:1fcc61be1dcf 210
kityann 0:1fcc61be1dcf 211 int mode = -1;
kityann 0:1fcc61be1dcf 212
kityann 0:1fcc61be1dcf 213 while (true) {
kityann 0:1fcc61be1dcf 214
kityann 0:1fcc61be1dcf 215 switch(mode){
kityann 0:1fcc61be1dcf 216 //スタートモード：パラシュートが開くまではこのモードを実行
kityann 0:1fcc61be1dcf 217 case -1:
kityann 0:1fcc61be1dcf 218 standby();
kityann 0:1fcc61be1dcf 219 break;
kityann 0:1fcc61be1dcf 220 //落下モード：落下時はこのモード。気圧計または、時間でロボットとパラシュートを分離する
kityann 0:1fcc61be1dcf 221 case 1:
kityann 0:1fcc61be1dcf 222 falling();
kityann 0:1fcc61be1dcf 223 break;
kityann 0:1fcc61be1dcf 224 //走行モード：ターゲットにむかって走行を行う
kityann 0:1fcc61be1dcf 225 case 2:
kityann 0:1fcc61be1dcf 226 running();
kityann 0:1fcc61be1dcf 227 break;
kityann 0:1fcc61be1dcf 228 //停止モード：ターゲット
kityann 0:1fcc61be1dcf 229 case 100:
kityann 0:1fcc61be1dcf 230 stopping();
kityann 0:1fcc61be1dcf 231 break;
kityann 0:1fcc61be1dcf 232 }
kityann 0:1fcc61be1dcf 233
kityann 0:1fcc61be1dcf 234 myGPS.read();
kityann 0:1fcc61be1dcf 235 //recive gps module
kityann 0:1fcc61be1dcf 236 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:1fcc61be1dcf 237 if ( myGPS.newNMEAreceived() ) {
kityann 0:1fcc61be1dcf 238 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:1fcc61be1dcf 239 continue;
kityann 0:1fcc61be1dcf 240 }
kityann 0:1fcc61be1dcf 241 else{
kityann 0:1fcc61be1dcf 242 count++;
kityann 0:1fcc61be1dcf 243 }
kityann 0:1fcc61be1dcf 244 }
kityann 0:1fcc61be1dcf 245 //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
kityann 0:1fcc61be1dcf 246 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:1fcc61be1dcf 247 refresh_Timer.reset();
kityann 0:1fcc61be1dcf 248 //print_gps(count);
kityann 0:1fcc61be1dcf 249 Get_GPS(&myGPS);
kityann 0:1fcc61be1dcf 250
kityann 0:1fcc61be1dcf 251 }
kityann 0:1fcc61be1dcf 252 }
kityann 0:1fcc61be1dcf 253
kityann 0:1fcc61be1dcf 254 }