OpenSSM: OpenSSM/ssm.c ソースファイル

説明を見る。
00001 /**************************************************************************************************
00002     Title           : SSM Protocol
00003     Programer       : Yosuke FURUSAWA.
00004     Copyright       : Copyright (C) 2007-2010 Yosuke FURUSAWA.
00005     License         : 4-clause BSD License
00006     Since           : 2007/11/xx
00007 
00008     Filename        : libssm.c
00009     Last up date    : 2010/08/13
00010     Kanji-Code      : Shift-JIS
00011     TAB Space       : 4
00012 **************************************************************************************************/
00013 
00014 
00015 /*=================================================================================================
00016 ヘッダファイルをインクルード
00017 =================================================================================================*/
00018 #include <p24FJ64GA002.h>
00019 
00020 #include "types.h"
00021 #include "table.h"
00022 #include "ssm.h"
00023 
00024 #include "libadc.h"
00025 #include "librtc.h"
00026 #include "libuart.h"
00027 
00028 
00029 /*=================================================================================================
00030 マクロ定義
00031 =================================================================================================*/
00032 /* 各ギア比の中間をもって、シフトの境目とする */
00033 #define SSM_SHIFT(x,y)      (x - (x - y) / 2.0)
00034 
00035 /* タイヤの基本スペック */
00036 #define TIRE_WIDTH          235         /* タイヤの幅 (mm) */
00037 #define TIRE_FLAT           45          /* タイヤの扁平率 (%) */
00038 #define TIRE_INCH           17          /* タイヤの直径 (inch) */
00039 
00040 /* ギア比 */
00041 #define GEAR_F              3.900
00042 #define GEAR_1              3.636
00043 #define GEAR_2              2.375
00044 #define GEAR_3              1.761
00045 #define GEAR_4              1.346
00046 #define GEAR_5              1.062
00047 #define GEAR_6              0.842
00048 #define GEAR_7              0.001
00049 
00050 
00051 /*=================================================================================================
00052 グローバル変数
00053 =================================================================================================*/
00054 SSM_T ssm;
00055 SSM_DATA_T ssm_data;
00056 
00057 
00058 /*=================================================================================================
00059 プロトタイプ宣言
00060 =================================================================================================*/
00061 static inline BOOL SSM_openssm(void);
00062 static inline void SSM_log(void);
00063 static inline BOOL SSM_openport(void);
00064 static inline void SSM_write_packet(void);
00065 static inline BOOL SSM_read_packet(pSSM_T ssm, pSSM_DATA_T data);
00066 static inline unsigned char SSM_calc_gear(pSSM_T ssm, pSSM_DATA_T data);
00067 
00068 
00069 /**************************************************************************************************
00070 SSM初期化
00071 **************************************************************************************************/
00072 void SSM_init(void)
00073 {
00074     /* 値が設定されていないときは、初期化する */
00075     ssm.tire_width      = TIRE_WIDTH;
00076     ssm.tire_flat       = TIRE_FLAT;
00077     ssm.tire_inch       = TIRE_INCH;
00078     ssm.gear_ratio[0]   = GEAR_F;
00079     ssm.gear_ratio[1]   = GEAR_1;
00080     ssm.gear_ratio[2]   = GEAR_2;
00081     ssm.gear_ratio[3]   = GEAR_3;
00082     ssm.gear_ratio[4]   = GEAR_4;
00083     ssm.gear_ratio[5]   = GEAR_5;
00084     ssm.gear_ratio[6]   = GEAR_6;
00085     ssm.gear_ratio[7]   = GEAR_7;
00086     ssm.price           = 100;
00087 
00088     /* タイヤの円周を求める */
00089     ssm.tire_circle = SSM_TIRE_R(ssm.tire_width, ssm.tire_flat, ssm.tire_inch);
00090 
00091     ssm.mode = SSM_MODE_OPENSSM;
00092 
00093     ssm.wait  = 1500;
00094     ssm.last  = rtc.tick;
00095     ssm.cycle = 0xffff;
00096     ssm.error = 0x0000;
00097 
00098     UART1_buf_clear();
00099     UART2_buf_clear();
00100     return;
00101 }
00102 
00103 
00104 /**************************************************************************************************
00105 SSM通信処理
00106 **************************************************************************************************/
00107 BOOL SSM_main(void)
00108 {
00109     static unsigned char mode = 0xff;
00110 
00111     /* 通信モードによって動作を変える */
00112     switch(ssm.mode){
00113 
00114 
00115     /* OpenSSMモード */
00116     case SSM_MODE_OPENSSM:
00117 
00118         /* 前回のコールと異なるとき、UARTを初期化する */
00119         if(mode != ssm.mode){
00120             UART1_init(115200);
00121             UART2_init(  4800);
00122         }
00123 
00124         mode = ssm.mode;
00125         return(SSM_openssm());
00126         break;
00127 
00128 
00129     /* OpenPort下位互換モード */
00130     case SSM_MODE_OPENPORT:
00131     default:
00132 
00133         /* 前回のコールと異なるとき、UARTを初期化する */
00134         if(mode != ssm.mode){
00135             UART1_init(  4800);
00136             UART2_init(  4800);
00137         }
00138 
00139         mode = ssm.mode;
00140         return(SSM_openport());
00141         break;
00142     }
00143 
00144     /* ここにきたらバグ */
00145     return(FALSE);
00146 }
00147 
00148 
00149 /*-------------------------------------------------------------------------------------------------
00150 OpenSSMモード
00151 -------------------------------------------------------------------------------------------------*/
00152 static inline BOOL SSM_openssm(void)
00153 {
00154     static unsigned int tick = 0;
00155     static unsigned char flag = 0;
00156 
00157     /* 実行周期 */
00158     if(RTC_get_ticks(tick, rtc.tick) < ssm.wait) return(FALSE);
00159     tick = rtc.tick;
00160 
00161     if(!SSM_read_packet(&ssm, &ssm_data)){
00162         ssm.error++;
00163 
00164         /* エラーが発生したときは、1サイクルのウェイトを入れてからパケットを送る */
00165         if(ssm.error >> 1){
00166             SSM_write_packet();
00167             ssm.cycle = 0xffff;
00168         }
00169     } else {
00170         ssm.cycle = RTC_get_ticks(ssm.last, rtc.tick);
00171         ssm.last = rtc.tick;
00172         SSM_write_packet();
00173     }
00174 
00175     /* スペースキーの入力があったならログを出す/止める */
00176     if(UART1_getch() == ' ')    flag++;
00177     if(flag % 2)                SSM_log();
00178 
00179     return(TRUE);
00180 }
00181 
00182 
00183 /*-------------------------------------------------------------------------------------------------
00184 UART1へログを出す
00185 -------------------------------------------------------------------------------------------------*/
00186 static inline void SSM_log(void)
00187 {
00188     double sec;
00189 
00190     /* 時刻 */
00191     sec = rtc.sec + (double)rtc.msec / 1000.0;
00192     UART1_putint(rtc.hour);
00193     UART1_putch(':');
00194     UART1_putint(rtc.min);
00195     UART1_putch(':');
00196     UART1_putdouble(sec, 3);
00197     UART1_putch(',');
00198 
00199     /* SSMデータ */
00200     UART1_putint(ssm_data.engine);
00201     UART1_putch(',');
00202     UART1_putint(ssm_data.speed);
00203     UART1_putch(',');
00204     UART1_putint(ssm_data.throttle);
00205     UART1_putch(',');
00206     UART1_putint(ssm_data.shift);
00207     UART1_putch(',');
00208     UART1_putint(ssm_data.coolant);
00209     UART1_putch(',');
00210     UART1_putint(ssm_data.intakeair);
00211     UART1_putch(',');
00212     UART1_putdouble(ssm_data.battery, 1);
00213     UART1_putch(',');
00214     UART1_putdouble(ssm_data.fuel, 1);
00215     UART1_putch(',');
00216     UART1_putint(ssm_data.maf);
00217     UART1_putch(',');
00218     UART1_putdouble(ssm_data.afr, 1);
00219     UART1_putch(',');
00220     UART1_putint(ssm_data.knock);
00221     UART1_putch(',');
00222     UART1_putint(ssm_data.ignition);
00223     UART1_putch(',');
00224 
00225     /* 加速度 */
00226     UART1_putint(adc.adc[0]);
00227     UART1_putch(',');
00228     UART1_putint(adc.adc[1]);
00229     UART1_putch(',');
00230     UART1_putint(adc.adc[2]);
00231 
00232     UART1_putch('¥r');
00233     UART1_putch('¥n');
00234 
00235     return;
00236 }
00237 
00238 
00239 /*-------------------------------------------------------------------------------------------------
00240 UART1 と UART2 をクロス接続
00241 OpenPort下位互換の通信モード
00242 -------------------------------------------------------------------------------------------------*/
00243 static inline BOOL SSM_openport(void)
00244 {
00245     int buf;
00246 
00247     while((buf = UART1_getch()) > 0){
00248         UART2_putch(buf);
00249     }
00250 
00251     while((buf = UART2_getch()) > 0){
00252         UART1_putch(buf);
00253     }
00254 
00255     return(TRUE);
00256 }
00257 
00258 
00259 /*-------------------------------------------------------------------------------------------------
00260 SSMブロック読み込み
00261 -------------------------------------------------------------------------------------------------*/
00262 static inline void SSM_write_packet(void)
00263 {
00264     unsigned char i;
00265     const unsigned char packet[] = { 
00266         0x80, 0x10, 0xf0, 0x29, 0xa8, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x00, 0x0f, 0x00, 0x00, 0x15,
00267         0x00, 0x00, 0x10, 0x00, 0x00, 0x24, 0x00, 0x00, 0x08, 0x00, 0x00, 0x12, 0x00, 0x00, 0x1c,
00268         0x00, 0x00, 0x13, 0x00, 0x00, 0x14, 0x00, 0x00, 0x46, 0x00, 0x00, 0x11, 0x00, 0x00, 0x22,
00269         0x8d,
00270     };
00271 
00272     UART2_buf_clear();
00273     for(i = 0; i < sizeof(packet); i++){
00274         UART2_putch(packet[i]);                             /* 送信エラーは無視する... */
00275     }
00276 
00277     return;
00278 }
00279 
00280 static inline BOOL SSM_read_packet(pSSM_T ssm, pSSM_DATA_T data)
00281 {
00282     int buf;
00283     unsigned char i;
00284     double fuel;
00285 
00286     static unsigned char read_packet[ 18 ];
00287 
00288     /* 一旦、バッファに入れる */
00289     i = 0;
00290     while((buf = UART2_getch()) != -1 && i < 45){
00291         /* 空読み */
00292         i++;
00293     }
00294     i = 0;
00295     while((buf = UART2_getch()) != -1 && i < 18){
00296         read_packet[i] = (unsigned char)buf;
00297         i++;
00298     }
00299 
00300     /* パケットの確認 */
00301     if(read_packet[ 0] != 0x80) return(FALSE);              /* コマンドヘッダ */
00302     if(read_packet[ 1] != 0xf0) return(FALSE);              /* 通信方向 MSB */
00303     if(read_packet[ 2] != 0x10) return(FALSE);              /* 通信方向 LSB */
00304     if(read_packet[ 3] != 0x0e) return(FALSE);              /* コマンド + データのサイズ */
00305     if(read_packet[ 4] != 0xe8) return(FALSE);              /* コマンド */
00306 
00307     /* 変換 */
00308     data->engine    = ((read_packet[ 5] << 8) + read_packet[ 6]) >> 2;
00309     data->throttle  = (double)((int)read_packet[ 7] * 100) / 255.0;
00310     data->speed     = read_packet[ 8];
00311     data->boost     = (((double)read_packet[ 9] - 128.0) * 37.0) / 3570.0;
00312     data->coolant   = (int)read_packet[10] - 40;
00313     data->intakeair = (int)read_packet[11] - 40;
00314     data->battery   = (double)read_packet[12] * 0.08;
00315     data->shift     = SSM_calc_gear(ssm, data);
00316     data->maf       = (double)(((unsigned int)read_packet[13] << 8) + (unsigned int)read_packet[14]) / 100.0;
00317     data->afr       = ((double)read_packet[15] / 128.0) * 14.7;
00318     data->ignition  = (read_packet[16] - 128) >> 1;
00319     data->knock     = (read_packet[17] - 128) >> 1;
00320 
00321 
00322     if(data->engine > 0){
00323         fuel        = (data->maf / data->afr) / 761.0;
00324     } else {
00325         fuel        = 0;
00326     }
00327 
00328     data->fuel      = ((double)data->speed / 3600.0) / fuel;
00329     data->fuel_rate  = fuel * 1000;
00330 
00331     return(TRUE);
00332 }
00333 
00334 
00335 /*-------------------------------------------------------------------------------------------------
00336 ギアを推測する
00337 -------------------------------------------------------------------------------------------------*/
00338 static inline unsigned char SSM_calc_gear(pSSM_T ssm, pSSM_DATA_T data)
00339 {
00340     double ratio;
00341     unsigned char shift;
00342 
00343     /* 走行中でないときは、ニュートラルとする */
00344     if(data->engine  <  1000) return(0);
00345     if(data->speed   ==    0) return(0);
00346 
00347     /* タイヤサイズ、車速、エンジン回転数からギア比を求める */
00348     ratio  = (data->engine / (data->speed * ssm->gear_ratio[ SSM_GEAR_FINAL ]) * ssm->tire_circle * 60.0);
00349     ratio /= 1000000.0;
00350 
00351     /* シフトを求める */
00352     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_1], ssm->gear_ratio[SSM_GEAR_2]) <= ratio) shift = SSM_GEAR_1;
00353     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_1], ssm->gear_ratio[SSM_GEAR_2]) >  ratio) shift = SSM_GEAR_2;
00354     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_2], ssm->gear_ratio[SSM_GEAR_3]) >  ratio) shift = SSM_GEAR_3;
00355     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_3], ssm->gear_ratio[SSM_GEAR_4]) >  ratio) shift = SSM_GEAR_4;
00356     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_4], ssm->gear_ratio[SSM_GEAR_5]) >  ratio) shift = SSM_GEAR_5;
00357     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_5], ssm->gear_ratio[SSM_GEAR_6]) >  ratio) shift = SSM_GEAR_6;
00358     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_6], ssm->gear_ratio[SSM_GEAR_7]) >  ratio) shift = SSM_GEAR_7;
00359 
00360     return(shift);
00361 }