LpmsMe1Peach.cpp

00001 #include "LpmsMe1Peach.h"
00002 #include <math.h>
00003 
00004 #define PIx2 6.28318530718
00005 
00006 // コンストラクタ
00007 LpmsMe1::LpmsMe1(Serial* xserial){
00008   serial = xserial;
00009   anglex = 0.0;
00010   angley = 0.0;
00011   anglez = 0.0;
00012   pre_rawanglex = 0.0;
00013   pre_rawangley = 0.0;
00014   pre_rawanglez = 0.0;
00015 
00016   init_ignore = true;
00017   init_done = false;
00018 }
00019 
00020 // コマンドモードへの移行
00021 void LpmsMe1::goto_command_mode(){
00022   serial->putc(0x3A);
00023   serial->putc(0x01);//ID
00024   serial->putc(0x00);
00025   serial->putc(0x06);//command
00026   serial->putc(0x00);
00027   serial->putc(0x00);
00028   serial->putc(0x00);
00029   serial->putc(0x07);//check sum
00030   serial->putc(0x00);
00031   serial->putc(0x0D);
00032   serial->putc(0x0A);
00033 }
00034 
00035 // 送信データの変更(角度データのみ)
00036 void LpmsMe1::set_transmit_data(){
00037   serial->putc(0x3A);
00038   serial->putc(0x01);//ID
00039   serial->putc(0x00);
00040   serial->putc(0x0A);//command
00041   serial->putc(0x00);
00042   serial->putc(0x04);//32bit=4byte送信
00043   serial->putc(0x00);
00044   serial->putc(0x00);//送信データ１
00045   serial->putc(0x00);//送信データ２
00046   serial->putc(0x02);//17bit目：角度データのみを取得
00047   serial->putc(0x00);//送信データ４
00048   serial->putc(0x11);//checksum
00049   serial->putc(0x00);
00050   serial->putc(0x0D);
00051   serial->putc(0x0A);
00052 }
00053 
00054 // フィルタの変更(カルマンフィルタ)
00055 void LpmsMe1::set_filter_mode(){
00056   serial->putc(0x3A);
00057   serial->putc(0x01);//ID
00058   serial->putc(0x00);
00059   serial->putc(0x29);//Command
00060   serial->putc(0x00);
00061   serial->putc(0x04);//32bit=4byte送信
00062   serial->putc(0x00);
00063   serial->putc(0x02);//送信データ1 0x02 でカルマンフィルタ
00064   serial->putc(0x00);//送信データ2
00065   serial->putc(0x00);//送信データ3
00066   serial->putc(0x00);//送信データ4
00067   serial->putc(0x30);
00068   serial->putc(0x00);
00069   serial->putc(0x0D);
00070   serial->putc(0x0A);
00071 }
00072 
00073 // オフセットの設定
00074 void LpmsMe1::set_offset(){
00075   serial->putc(0x3A);
00076   serial->putc(0x01);//ID
00077   serial->putc(0x00);
00078   serial->putc(0x12);//Command
00079   serial->putc(0x00);
00080   serial->putc(0x04);//32bit=4byte送信
00081   serial->putc(0x00);
00082   serial->putc(0x00);//送信データ1 0x00 で object offset
00083   serial->putc(0x00);//送信データ2
00084   serial->putc(0x00);//送信データ3
00085   serial->putc(0x00);//送信データ4
00086   serial->putc(0x17);
00087   serial->putc(0x00);
00088   serial->putc(0x0D);
00089   serial->putc(0x0A);
00090 }
00091 
00092 // オフセットのリセット(よくわからないので使ってない)
00093 void LpmsMe1::reset_orientation_offset(){
00094   serial->putc(0x3A);
00095   serial->putc(0x01);//ID
00096   serial->putc(0x00);
00097   serial->putc(0x52);//command
00098   serial->putc(0x00);
00099   serial->putc(0x00);
00100   serial->putc(0x00);
00101   serial->putc(0x53);//check sum
00102   serial->putc(0x00);
00103   serial->putc(0x0D);
00104   serial->putc(0x0A);
00105 }
00106 
00107 // センサデータの送信要求
00108 void LpmsMe1::get_sensor_data(){
00109   serial->putc(0x3A);
00110   serial->putc(0x01);
00111   serial->putc(0x00);
00112   serial->putc(0x09);
00113   serial->putc(0x00);
00114   serial->putc(0x00);
00115   serial->putc(0x00);
00116   serial->putc(0x0A);//checksum
00117   serial->putc(0x00);
00118   serial->putc(0x0D);
00119   serial->putc(0x0A);
00120 }
00121 
00122 float LpmsMe1::get_z_angle(){
00123   if(init_done == true){
00124     get_sensor_data();
00125     int response = recv_proc(10);
00126     // 受信は浮動小数点(符号部，指数部，仮数部)に分かれて送られてくるので，以下でunpacking
00127     if(response == 16 ){
00128       unsigned int bits_dataz = (buffer[12] | (buffer[13]<<8)) | ((buffer[14]<<16) | (buffer[15]<<24));
00129       unsigned int mantissa_part = bits_dataz & 0x7FFFFF; // 仮数部を取得
00130       unsigned int copy_matissa_part = mantissa_part;
00131       float k = pow(2.0f, -23);
00132       float real_mantissa = 0.0f;
00133       
00134       for(int i = 0; i < 23; i++){
00135         real_mantissa += k * (float)(copy_matissa_part & 0x000001); 
00136         copy_matissa_part = copy_matissa_part >> 1;
00137         k *= 2.0f;
00138       }
00139       
00140       int exponent = (int)((bits_dataz & 0x7F800000) >> 23) - 127; // 指数部を取得
00141       float rawanglez = pow(2.0f, exponent) * (1.0f + real_mantissa);
00142       if(bits_dataz>>31){ // 符号部の値に応じて+-を変更
00143         rawanglez *= -1.0f;
00144       }
00145 
00146       // センサは-pi ~ +pi の範囲で取れるので，差分を積算していくことで連続して取れるようにする
00147       float diff_rawanglez;
00148       if(init_ignore) {
00149         diff_rawanglez = 0.0;
00150       }else{
00151         diff_rawanglez = rawanglez - pre_rawanglez;
00152       }
00153 
00154       if(fabs(diff_rawanglez) >= 3.0){ 
00155         if(rawanglez < 0){ //+から-へ回ったとき
00156           anglez += PIx2 + diff_rawanglez;
00157         }else{ // -から+へ回ったとき
00158           anglez += -PIx2 + diff_rawanglez;
00159         }
00160       }else{
00161         anglez += diff_rawanglez;
00162       }
00163       pre_rawanglez = rawanglez;
00164 
00165       return anglez;
00166     }
00167     return response;
00168   }
00169   return 0.0; // 初期化が終わっていない場合は，0を返す
00170 }
00171 
00172 int LpmsMe1::recv_proc(int timeout_num = 10){
00173   int state = 0, data_counter = 0, timeout_counter = 0;
00174   short command = 0, data_length = 0, check_sum = 0, recv_sum = 0;
00175   bool recv_done = false;
00176   
00177   // 受信処理
00178   while(!recv_done){
00179     if(timeout_counter > timeout_num * 500){
00180       return -1;
00181     }
00182     timeout_counter++;
00183     
00184     if (serial->readable()) {
00185       unsigned char data = serial->getc();
00186       //printf("%x ", data);
00187 
00188       switch (state) {
00189       case 0:
00190         if (data == 0x3A) state++;
00191         break;
00192       case 1:
00193         if (data == 0x01){
00194           state++;
00195           check_sum += data;
00196         }
00197         else {
00198           state = 0;
00199           check_sum = 0;
00200         }
00201         break;
00202       case 2:
00203         if (data == 0x00){
00204           state++;
00205           check_sum += data;
00206         }
00207         else{
00208           state = 0;
00209           check_sum = 0;
00210         }
00211         break;
00212       case 3:
00213         command |= data;
00214         check_sum += data;
00215         state++;
00216         break;
00217       case 4:
00218         command |= data<<8;
00219         check_sum += data;
00220         state++;
00221         break;
00222       case 5:
00223         data_length |= data;
00224         check_sum += data;
00225         state++;
00226         break;
00227       case 6:
00228         data_length |= data<<8;
00229         check_sum += data;
00230         if(data_length == 0){
00231           state += 2;
00232         }else state++;
00233         break;
00234       case 7:
00235         //Serial.print(data, HEX);
00236         //Serial.print(" ");
00237         buffer[data_counter++] = data;
00238         check_sum += data;
00239         if (data_counter == data_length) state++;
00240         break;
00241       case 8:
00242         //Serial.println("");
00243         recv_sum |= data;
00244         state++;
00245         break;
00246       case 9:
00247         recv_sum |= data<<8;
00248         if (check_sum == recv_sum) {
00249             state++;
00250           }else{
00251             return -5;
00252           }
00253         break;
00254       case 10:
00255         if(data == 0x0D) state++;
00256         else return -6;
00257         break;
00258       case 11:
00259         if(data == 0x0A){
00260           state++;
00261           recv_done = true;
00262         }
00263         else return -7;
00264         break;
00265       default:
00266         return -8;
00267       }
00268     }
00269   }
00270   return data_counter;
00271 }
00272 
00273 int LpmsMe1::init(){
00274   unsigned char trash;
00275   int datanum, result[4] = {0};
00276 
00277   if(!init_done){
00278     // ここは Serial1 をジャイロセンサに使うことを前提に書かれている
00279     // P2_3 を CTSピンとして使うための設定
00280     /***** ポートの初期化 *****/
00281     GPIO.PIBC2 &= ~0x0008; // ポート入力バッファ制御レジスタ 入力バッファ禁止
00282     GPIO.PBDC2 &= ~0x0008; // ポート双方向制御レジスタ 双方向モードを禁止
00283     GPIO.PM2 &= ~0x0008; // ポートモードレジスタ 入力モード
00284     GPIO.PMC2 &= ~0x0008; // ポートモード制御レジスタ ポートモード
00285     GPIO.PIPC2 &= ~0x0008; // ポート IP 制御レジスタ　入出力はPMn.PMnmビットによって制御されます
00286       
00287     /***** 入力機能のポート設定 *****/
00288     GPIO.PBDC2 |= 0x0008; // ポート双方向制御レジスタ 双方向モードを許可
00289       
00290     /***** ポート設定 *****/
00291     GPIO.PFC2 |= 0x0008;
00292     GPIO.PFCE2 &= ~0x0008;
00293     GPIO.PFCAE2 |= 0x0008;
00294 
00295     GPIO.PIPC2 |= 0x0008; // ポート IP 制御レジスタ　入出力はPMn.PMnmビットによって制御されます
00296     GPIO.PMC2 |= 0x0008; // ポートモード制御レジスタ ポートモード
00297 
00298     // CTSピンを使うための設定
00299     CPG.STBCR4 &= ~0x40; // FIFO内臓シリアルコミュニケーションインタフェースチャンネル1は動作(これをやらないと以下が書き変わらない)
00300     SCIF1.SCFCR |= 0x08; // CTS#1 を利用
00301 
00302     serial->baud(115200);
00303 
00304     //datanum = serial->readable();
00305     //for(int i = 0; i < datanum; i++){
00306     while(serial->readable()){
00307       trash = serial->getc(); // ゴミデータを捨てる
00308     }
00309     
00310     do{
00311       goto_command_mode();
00312       //Serial.print("goto command mode sent: ");
00313       result[0] = recv_proc(500);
00314       //Serial.println(result[0]);
00315     }while(result[0] != 0);
00316     
00317     do{
00318       set_transmit_data();
00319       //Serial.print("set transmit data command sent: ");
00320       result[1] = recv_proc(500);
00321       //Serial.println(result[1]);
00322     }while(result[1] != 0);
00323 
00324     //set_filter_mode();
00325     //Serial.print("set filter mode sent: ");
00326     //result[2] = recv_proc(1000);
00327     //Serial.println(result[2]);
00328 
00329     thread_sleep_for(10); // これがないと，データが正常に取れなくなる
00330     
00331     do{
00332       set_offset();
00333       //Serial.print("set offset sent: ");
00334       result[3] = recv_proc(500);
00335       //Serial.println(result[3]);
00336     }while(result[3] != 0);
00337 
00338     for(int i = 0; i < 5; i++){
00339       get_z_angle();
00340       thread_sleep_for(10);
00341     }
00342 
00343     init_ignore = false;
00344   }
00345 
00346   //if(result[0] >= 0 && result[1] >= 0) digitalWrite(PIN_LED3, HIGH);
00347   //if(result[2] >= 0 && result[3] >= 0) digitalWrite(PIN_LED2, HIGH);
00348   if((result[0] + result[1] + result[3]) == 0){
00349     init_done = true;
00350     return 1;
00351   } 
00352   else return (result[0] + result[1] + result[3]);
00353 }
00354