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/22
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  BOOL SSM_openssm(void);
00062 static  void SSM_log(void);
00063 static  BOOL SSM_openport(void);
00064 static  void SSM_write_packet(void);
00065 static  BOOL SSM_read_packet(pSSM_T ssm, pSSM_DATA_T data);
00066 static  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     ssm.tire_circle     = SSM_TIRE_R(ssm.tire_width, ssm.tire_flat, ssm.tire_inch);
00088     ssm.mode            = SSM_MODE_OPENSSM;
00089 
00090     ssm.wait  = 1500;
00091     ssm.last  = rtc.tick;
00092     ssm.cycle = 0xffff;
00093     ssm.error = 0x0000;
00094 
00095     ssm_data.engine     = 800;
00096     ssm_data.throttle   = 0;
00097     ssm_data.speed      = 0;
00098     ssm_data.boost      = 0.0;
00099     ssm_data.coolant    = 20.0;
00100     ssm_data.intakeair  = 20.0;
00101     ssm_data.battery    = 12.0;
00102     ssm_data.maf        = 5;
00103     ssm_data.afr        = 14.7;
00104     ssm_data.ignition   = 0;
00105     ssm_data.knock      = 0;
00106     ssm_data.fuel       = 0;
00107     ssm_data.fuel_rate  = 0;
00108     ssm_data.shift      = 0;
00109 
00110     UART1_buf_clear();
00111     UART2_buf_clear();
00112     return;
00113 }
00114 
00115 
00116 /**************************************************************************************************
00117 SSM通信処理
00118 **************************************************************************************************/
00119 BOOL SSM_main(void)
00120 {
00121     static unsigned char mode = 0xff;
00122 
00123     /* 通信モードによって動作を変える */
00124     switch(ssm.mode){
00125 
00126 
00127     /* OpenSSMモード */
00128     case SSM_MODE_OPENSSM:
00129 
00130         /* 前回のコールと異なるとき、UARTを初期化する */
00131         if(mode != ssm.mode){
00132             UART1_init(115200);
00133             UART2_init(  4800);
00134         }
00135 
00136         mode = ssm.mode;
00137         return(SSM_openssm());
00138         break;
00139 
00140 
00141     /* OpenPort下位互換モード */
00142     case SSM_MODE_OPENPORT:
00143     default:
00144 
00145         /* 前回のコールと異なるとき、UARTを初期化する */
00146         if(mode != ssm.mode){
00147             UART1_init(  4800);
00148             UART2_init(  4800);
00149         }
00150 
00151         mode = ssm.mode;
00152         return(SSM_openport());
00153         break;
00154     }
00155 
00156     /* ここにきたらバグ */
00157     return(FALSE);
00158 }
00159 
00160 
00161 /*-------------------------------------------------------------------------------------------------
00162 OpenSSMモード
00163 -------------------------------------------------------------------------------------------------*/
00164 static  BOOL SSM_openssm(void)
00165 {
00166     static unsigned int tick = 0;
00167     static unsigned char flag = 0;
00168 
00169     /* 実行周期 */
00170     if(RTC_get_ticks(tick, rtc.tick) < ssm.wait) return(FALSE);
00171     tick = rtc.tick;
00172 
00173     if(!SSM_read_packet(&ssm, &ssm_data)){
00174         ssm.error++;
00175 
00176         /* エラーが発生したときは、1サイクルのウェイトを入れてからパケットを送る */
00177         if(ssm.error >> 1){
00178             SSM_write_packet();
00179             ssm.cycle = 0xffff;
00180         }
00181     } else {
00182         ssm.cycle = RTC_get_ticks(ssm.last, rtc.tick);
00183         ssm.last = rtc.tick;
00184         SSM_write_packet();
00185     }
00186 
00187     /* スペースキーの入力があったならログを出す/止める */
00188     if(UART1_getch() == ' ')    flag++;
00189     if(flag % 2)                SSM_log();
00190 
00191     return(TRUE);
00192 }
00193 
00194 
00195 /*-------------------------------------------------------------------------------------------------
00196 UART1へログを出す
00197 -------------------------------------------------------------------------------------------------*/
00198 static  void SSM_log(void)
00199 {
00200     double sec;
00201 
00202     /* バッファから溢れそうなときは、飛ばす... */
00203     if(UART1_get_sendbuf() != UART1_TX_BUFFER_SIZE) return;
00204     UART1_buf_clear();
00205 
00206     /* 時刻 */
00207     sec = rtc.sec + (double)rtc.msec / 1000.0;
00208     UART1_putint(rtc.hour);
00209     UART1_putch(':');
00210     UART1_putint(rtc.min);
00211     UART1_putch(':');
00212     UART1_putdouble(sec, 3);
00213     UART1_putch(',');
00214 
00215     /* SSMデータ */
00216     UART1_putint(ssm_data.engine);
00217     UART1_putch(',');
00218     UART1_putint(ssm_data.speed);
00219     UART1_putch(',');
00220     UART1_putint(ssm_data.throttle);
00221     UART1_putch(',');
00222     UART1_putint(ssm_data.shift);
00223     UART1_putch(',');
00224     UART1_putint(ssm_data.coolant);
00225     UART1_putch(',');
00226     UART1_putint(ssm_data.intakeair);
00227     UART1_putch(',');
00228     UART1_putdouble(ssm_data.battery, 1);
00229     UART1_putch(',');
00230     UART1_putdouble(ssm_data.fuel, 1);
00231     UART1_putch(',');
00232     UART1_putint(ssm_data.maf);
00233     UART1_putch(',');
00234     UART1_putdouble(ssm_data.afr, 1);
00235     UART1_putch(',');
00236     UART1_putint(ssm_data.knock);
00237     UART1_putch(',');
00238     UART1_putint(ssm_data.ignition);
00239     UART1_putch(',');
00240 
00241     /* 加速度 */
00242     UART1_putint(adc.adc[0]);
00243     UART1_putch(',');
00244     UART1_putint(adc.adc[1]);
00245     UART1_putch(',');
00246     UART1_putint(adc.adc[2]);
00247 
00248     UART1_putch('¥r');
00249     UART1_putch('¥n');
00250 
00251     return;
00252 }
00253 
00254 
00255 /*-------------------------------------------------------------------------------------------------
00256 UART1 と UART2 をクロス接続
00257 OpenPort下位互換の通信モード
00258 -------------------------------------------------------------------------------------------------*/
00259 static  BOOL SSM_openport(void)
00260 {
00261     int buf;
00262 
00263     while((buf = UART1_getch()) > 0){
00264         UART2_putch(buf);
00265     }
00266 
00267     while((buf = UART2_getch()) > 0){
00268         UART1_putch(buf);
00269     }
00270 
00271     return(TRUE);
00272 }
00273 
00274 
00275 /*-------------------------------------------------------------------------------------------------
00276 SSMブロック読み込み
00277 -------------------------------------------------------------------------------------------------*/
00278 static  void SSM_write_packet(void)
00279 {
00280     unsigned char i;
00281     const unsigned char packet[] = { 
00282         0x80, 0x10, 0xf0, 0x29, 0xa8, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x00, 0x0f, 0x00, 0x00, 0x15,
00283         0x00, 0x00, 0x10, 0x00, 0x00, 0x24, 0x00, 0x00, 0x08, 0x00, 0x00, 0x12, 0x00, 0x00, 0x1c,
00284         0x00, 0x00, 0x13, 0x00, 0x00, 0x14, 0x00, 0x00, 0x46, 0x00, 0x00, 0x11, 0x00, 0x00, 0x22,
00285         0x8d,
00286     };
00287 
00288     UART2_buf_clear();
00289     for(i = 0; i < sizeof(packet); i++){
00290         UART2_putch(packet[i]);                             /* 送信エラーは無視する... */
00291     }
00292 
00293     return;
00294 }
00295 
00296 static  BOOL SSM_read_packet(pSSM_T ssm, pSSM_DATA_T data)
00297 {
00298     int buf;
00299     unsigned char i;
00300     double fuel;
00301 
00302     static unsigned char read_packet[ 18 ];
00303 
00304     /* 一旦、バッファに入れる */
00305     i = 0;
00306     while((buf = UART2_getch()) != -1 && i < 45){
00307         /* 空読み */
00308         i++;
00309     }
00310     i = 0;
00311     while((buf = UART2_getch()) != -1 && i < 18){
00312         read_packet[i] = (unsigned char)buf;
00313         i++;
00314     }
00315 
00316     /* パケットの確認 */
00317     if(read_packet[ 0] != 0x80) return(FALSE);              /* コマンドヘッダ */
00318     if(read_packet[ 1] != 0xf0) return(FALSE);              /* 通信方向 MSB */
00319     if(read_packet[ 2] != 0x10) return(FALSE);              /* 通信方向 LSB */
00320     if(read_packet[ 3] != 0x0e) return(FALSE);              /* コマンド + データのサイズ */
00321     if(read_packet[ 4] != 0xe8) return(FALSE);              /* コマンド */
00322 
00323     /* 変換 */
00324     data->engine    = ((read_packet[ 5] << 8) + read_packet[ 6]) >> 2;
00325     data->throttle  = (double)((int)read_packet[ 7] * 100) / 255.0;
00326     data->speed     = read_packet[ 8];
00327     data->boost     = (((double)read_packet[ 9] - 128.0) * 37.0) / 3570.0;
00328     data->coolant   = (int)read_packet[10] - 40;
00329     data->intakeair = (int)read_packet[11] - 40;
00330     data->battery   = (double)read_packet[12] * 0.08;
00331     data->shift     = SSM_calc_gear(ssm, data);
00332     data->maf       = (double)(((unsigned int)read_packet[13] << 8) + (unsigned int)read_packet[14]) / 100.0;
00333     data->afr       = ((double)read_packet[15] / 128.0) * 14.7;
00334     data->ignition  = (read_packet[16] - 128) >> 1;
00335     data->knock     = (read_packet[17] - 128) >> 1;
00336 
00337 
00338     if(data->engine > 0){
00339         fuel        = (data->maf / data->afr) / 761.0;
00340     } else {
00341         fuel        = 0;
00342     }
00343 
00344     data->fuel      = ((double)data->speed / 3600.0) / fuel;
00345     data->fuel_rate  = fuel * 1000;
00346 
00347     return(TRUE);
00348 }
00349 
00350 
00351 /*-------------------------------------------------------------------------------------------------
00352 ギアを推測する
00353 -------------------------------------------------------------------------------------------------*/
00354 static  unsigned char SSM_calc_gear(pSSM_T ssm, pSSM_DATA_T data)
00355 {
00356     double ratio;
00357     unsigned char shift;
00358 
00359     /* 走行中でないときは、ニュートラルとする */
00360     if(data->engine  <  1000) return(0);
00361     if(data->speed   ==    0) return(0);
00362 
00363     /* タイヤサイズ、車速、エンジン回転数からギア比を求める */
00364     ratio  = (data->engine / (data->speed * ssm->gear_ratio[ SSM_GEAR_FINAL ]) * ssm->tire_circle * 60.0);
00365     ratio /= 1000000.0;
00366 
00367     /* シフトを求める */
00368     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_1], ssm->gear_ratio[SSM_GEAR_2]) <= ratio) shift = SSM_GEAR_1;
00369     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_1], ssm->gear_ratio[SSM_GEAR_2]) >  ratio) shift = SSM_GEAR_2;
00370     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_2], ssm->gear_ratio[SSM_GEAR_3]) >  ratio) shift = SSM_GEAR_3;
00371     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_3], ssm->gear_ratio[SSM_GEAR_4]) >  ratio) shift = SSM_GEAR_4;
00372     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_4], ssm->gear_ratio[SSM_GEAR_5]) >  ratio) shift = SSM_GEAR_5;
00373     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_5], ssm->gear_ratio[SSM_GEAR_6]) >  ratio) shift = SSM_GEAR_6;
00374     if(SSM_SHIFT(ssm->gear_ratio[SSM_GEAR_6], ssm->gear_ratio[SSM_GEAR_7]) >  ratio) shift = SSM_GEAR_7;
00375 
00376     return(shift);
00377 }