00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
00037 #define TIRE_FLAT 45
00038 #define TIRE_INCH 17
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
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
00118
00119 BOOL SSM_main(void)
00120 {
00121 static unsigned char mode = 0xff;
00122
00123
00124 switch(ssm.mode){
00125
00126
00127
00128 case SSM_MODE_OPENSSM:
00129
00130
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
00142 case SSM_MODE_OPENPORT:
00143 default:
00144
00145
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
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
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
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
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
00257
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
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);
00319 if(read_packet[ 2] != 0x10) return(FALSE);
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 }