sens_itf_sensor.cpp

00001 #include <string.h>
00002 #include <stdint.h>
00003 #include "mbed.h"
00004 #include "sens_itf.h "
00005 #include "sens_util.h"
00006 #include "../util/buf_io.h"
00007 #include "../util/crc16.h"
00008 #include "../pt/pt.h"
00009 #include "SLCD.h"
00010 #include "MMA8451Q.h"
00011 
00012 #define SENS_ITF_SENSOR_DBG_FRAME     1
00013 #define SENS_ITF_DBG_FRAME 1
00014 #define SENS_ITF_SENSOR_NUM_OF_POINTS 5
00015 
00016 #define MMA8451_I2C_ADDRESS (0x1d<<1)
00017 
00018 static uint8_t main_svr_addr[SENS_ITF_SERVER_ADDR_SIZE];
00019 static uint8_t secon_svr_addr[SENS_ITF_SERVER_ADDR_SIZE];
00020 static uint8_t rx_frame[SENS_ITF_MAX_FRAME_SIZE];
00021 static volatile uint8_t num_rx_bytes;
00022 static Timeout rx_trmout_timer ;
00023 static Ticker acq_data_timer;
00024 static sens_itf_point_ctrl_t sensor_points;
00025 static sens_itf_cmd_brd_id_t board_info;
00026 static struct pt pt_updt;
00027 static struct pt pt_data;
00028 static volatile uint8_t frame_timeout;
00029 static volatile uint8_t acq_data ;
00030 static DigitalOut  greenLED(LED1);
00031 static DigitalOut  redLED(LED2);
00032 static AnalogIn    lightSensor(PTE22);
00033 static SLCD        sLCD;
00034 static Serial      pcSerial(USBTX, USBRX);
00035 static MMA8451Q    acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
00036 static InterruptIn mode_switch(SW3);    
00037 static volatile uint8_t  view_mode;
00038 
00039 void scroll_message(char *message, unsigned char len)
00040 {
00041     sLCD.All_Segments(0);
00042     
00043     for (int start = 0; start < len - 4; start++)
00044     {
00045         for (int digit = 0; digit < 4; digit++)
00046             sLCD.putc(message[start + digit]);
00047         wait(0.4);
00048     }
00049 }
00050 
00051 static void dump_frame(uint8_t *frame, uint8_t size)
00052 {
00053     int n,m;
00054     char buf[64];
00055     
00056     buf[0] = buf[1] = buf[2] = buf[3] = ' ';
00057     
00058     for(n = 0, m = 4; n < size ; n++, m+=3)
00059     {
00060         sprintf(&buf[m],"%02X_",frame[n]);
00061     }
00062     
00063     buf[m] = buf[m+1] = buf[m+2] = buf[m+3] = ' ';
00064     buf[m+4] = '\0';
00065     
00066     m +=4;
00067     scroll_message(buf,m);
00068     
00069 }
00070     
00071 static uint8_t sens_itf_get_point_type(uint8_t point)
00072 {
00073     return sensor_points.points[point].desc.type;
00074 }
00075 
00076 static uint8_t sens_itf_get_number_of_points(void)
00077 {
00078     return SENS_ITF_SENSOR_NUM_OF_POINTS;
00079 }
00080 
00081 static sens_itf_cmd_point_desc_t *sens_itf_get_point_desc(uint8_t point)
00082 {
00083     sens_itf_cmd_point_desc_t *d = 0;
00084 
00085     if (point < sens_itf_get_number_of_points())
00086         d = &sensor_points.points[point].desc;
00087 
00088     return d;
00089 }
00090 
00091 static sens_itf_cmd_point_t *sens_itf_get_point_value(uint8_t point)
00092 {
00093     sens_itf_cmd_point_t *v = 0;
00094 
00095     if (point < sens_itf_get_number_of_points())
00096         v = &sensor_points.points[point].value;
00097 
00098     return v;
00099 }
00100 
00101 static uint8_t sens_itf_set_point_value(uint8_t point, sens_itf_cmd_point_t *v)
00102 {
00103     uint8_t ret = 0;
00104 
00105     if (point < sens_itf_get_number_of_points())
00106     {
00107         sensor_points.points[point].value = *v;
00108         ret = 1;
00109     }
00110     else
00111     {
00112         ret = 0;
00113     }
00114 
00115     return ret;
00116 }
00117 
00118 static sens_itf_cmd_brd_id_t *sens_itf_get_board_info(void)
00119 {
00120     return &board_info;
00121 }
00122 
00123 static uint8_t sens_itf_sensor_send_frame(uint8_t *frame, uint8_t size)
00124 {
00125     //dump_frame(frame, size);
00126     
00127     for(int n = 0 ; n < size ; n++)
00128         pcSerial.putc((unsigned int )frame[n]);
00129     //pcSerial.puts((char *)frame,size); // puts returns the size sent ?
00130     return size;
00131 }
00132 
00133 static uint8_t sens_itf_sensor_check_register_map(sens_itf_cmd_req_t *cmd, sens_itf_cmd_res_t *ans, uint8_t *frame)
00134 {
00135     uint8_t size = 0;
00136     if ( // check global register map for valid address ranges
00137                 ((cmd->hdr.addr > SENS_ITF_REGMAP_SVR_SEC_ADDR) && 
00138                 (cmd->hdr.addr < SENS_ITF_REGMAP_POINT_DESC_1)) ||
00139                 (cmd->hdr.addr > SENS_ITF_REGMAP_WRITE_POINT_DATA_32) ||
00140                 // check local register map - reading
00141                 ((cmd->hdr.addr >= SENS_ITF_REGMAP_READ_POINT_DATA_1) && 
00142                 (cmd->hdr.addr <= SENS_ITF_REGMAP_READ_POINT_DATA_32) &&
00143                 ((cmd->hdr.addr - SENS_ITF_REGMAP_READ_POINT_DATA_1) >= sens_itf_get_number_of_points())) ||
00144                 // check local register map - writing
00145                 ((cmd->hdr.addr >= SENS_ITF_REGMAP_WRITE_POINT_DATA_1) && 
00146                 (cmd->hdr.addr <= SENS_ITF_REGMAP_WRITE_POINT_DATA_32) &&
00147                 ((cmd->hdr.addr - SENS_ITF_REGMAP_WRITE_POINT_DATA_1) >= sens_itf_get_number_of_points())))
00148     {
00149         sens_util_log(SENS_ITF_SENSOR_DBG_FRAME, "Invalid register address %02X",cmd->hdr.addr);
00150         ans->hdr.status = SENS_ITF_ANS_REGISTER_NOT_IMPLEMENTED;
00151         size = sens_itf_pack_cmd_res(ans, frame);
00152     }
00153     return size;
00154 }
00155 
00156 static uint8_t sens_itf_sensor_writings(sens_itf_cmd_req_t *cmd, sens_itf_cmd_res_t *ans, uint8_t *frame)
00157 {
00158     uint8_t size = 0;
00159     if ((cmd->hdr.addr >= SENS_ITF_REGMAP_WRITE_POINT_DATA_1) && 
00160         (cmd->hdr.addr <= SENS_ITF_REGMAP_WRITE_POINT_DATA_32))
00161     {
00162         uint8_t point = cmd->hdr.addr - SENS_ITF_REGMAP_WRITE_POINT_DATA_1;
00163         //sLCD.printf("R%dV%d",point,cmd->payload.point_value_cmd.value.u8);
00164         //wait(0.5);
00165         uint8_t acr = sens_itf_get_point_desc(point)->access_rights & SENS_ITF_ACCESS_WRITE_ONLY;
00166             
00167         if (acr)
00168         {
00169             ans->hdr.status = SENS_ITF_ANS_OK;
00170             sens_itf_set_point_value(point,&cmd->payload.point_value_cmd);
00171         }
00172         else
00173         {
00174             sens_util_log(SENS_ITF_SENSOR_DBG_FRAME, "Point %d does not allow writings",point);                
00175             ans->hdr.status = SENS_ITF_ANS_READY_ONLY;
00176         }
00177         size = sens_itf_pack_cmd_res(ans, frame);
00178     }
00179     return size;
00180 }
00181 
00182 static uint8_t sens_itf_sensor_readings(sens_itf_cmd_req_t *cmd, sens_itf_cmd_res_t *ans, uint8_t *frame)
00183 {
00184     uint8_t size = 0;
00185     if ((cmd->hdr.addr >= SENS_ITF_REGMAP_READ_POINT_DATA_1) &&
00186         (cmd->hdr.addr <= SENS_ITF_REGMAP_READ_POINT_DATA_32))
00187     {
00188         uint8_t point = cmd->hdr.addr - SENS_ITF_REGMAP_READ_POINT_DATA_1;
00189         uint8_t acr = sens_itf_get_point_desc(point)->access_rights & SENS_ITF_ANS_READY_ONLY;
00190             
00191         if (acr)
00192         {
00193             ans->hdr.status = SENS_ITF_ANS_OK;
00194             ans->payload.point_value_cmd = *sens_itf_get_point_value(point);
00195         }
00196         else
00197         {
00198             sens_util_log(SENS_ITF_SENSOR_DBG_FRAME, "Point %d does not allow readings",point);                
00199             ans->hdr.status = SENS_ITF_ANS_WRITE_ONLY;
00200         }
00201         size = sens_itf_pack_cmd_res(ans, frame);
00202     }
00203     return size;
00204 }
00205 
00206 static uint8_t sens_itf_check_other_cmds(sens_itf_cmd_req_t *cmd, sens_itf_cmd_res_t *ans, uint8_t *frame)
00207 {
00208     uint8_t size = 0;
00209     switch (cmd->hdr.addr)
00210     {
00211         case SENS_ITF_REGMAP_ITF_VERSION:
00212             ans->payload.itf_version_cmd.version = SENS_ITF_LATEST_VERSION;
00213             break;
00214         case SENS_ITF_REGMAP_BRD_ID:
00215             memcpy(&ans->payload.brd_id_cmd,sens_itf_get_board_info(),sizeof(sens_itf_cmd_brd_id_t));
00216             break;
00217         case SENS_ITF_REGMAP_BRD_STATUS:
00218             ans->payload.brd_status_cmd.status = 0; // TBD
00219             break;
00220         case SENS_ITF_REGMAP_BRD_CMD:
00221             ans->payload.command_res_cmd.status = 0; // TBD
00222             break;
00223         case SENS_ITF_REGMAP_READ_BAT_STATUS:
00224             ans->payload.bat_status_cmd.status = 0; // TBD
00225             break;
00226         case SENS_ITF_REGMAP_READ_BAT_CHARGE:
00227             ans->payload.bat_charge_cmd.charge = 100; // TBD
00228             break;
00229         case SENS_ITF_REGMAP_SVR_MAIN_ADDR:
00230             memcpy(ans->payload.svr_addr_cmd.addr,main_svr_addr, SENS_ITF_SERVER_ADDR_SIZE); 
00231             break;
00232         case SENS_ITF_REGMAP_SVR_SEC_ADDR:
00233             memcpy(ans->payload.svr_addr_cmd.addr,secon_svr_addr, SENS_ITF_SERVER_ADDR_SIZE);
00234             break;
00235         default:
00236             break;
00237 
00238     }
00239     
00240     if ((cmd->hdr.addr >= SENS_ITF_REGMAP_POINT_DESC_1) && (cmd->hdr.addr <= SENS_ITF_REGMAP_POINT_DESC_32))
00241     {
00242         uint8_t point = cmd->hdr.addr - SENS_ITF_REGMAP_POINT_DESC_1;
00243         memcpy(&ans->payload.point_desc_cmd, &sensor_points.points[point].desc, sizeof(sens_itf_cmd_point_desc_t));
00244     }
00245         
00246     ans->hdr.status = SENS_ITF_ANS_OK;
00247     size = sens_itf_pack_cmd_res(ans, frame);
00248     return size;
00249 }
00250 static void sens_itf_process_cmd(uint8_t *frame, uint8_t num_rx_bytes)
00251 {
00252     uint8_t ret;
00253     uint8_t size = 0;
00254     sens_itf_cmd_req_t cmd;
00255     sens_itf_cmd_res_t ans;
00256 
00257     ret = sens_itf_unpack_cmd_req(&cmd, frame, num_rx_bytes);
00258 
00259     if (ret > 0)
00260     {
00261         ans.hdr.addr = cmd.hdr.addr;
00262         
00263         size = sens_itf_sensor_check_register_map(&cmd, &ans,frame);
00264         if (size == 0)
00265             size = sens_itf_sensor_writings(&cmd, &ans,frame);
00266 
00267         if (size == 0)
00268             size = sens_itf_sensor_readings(&cmd, &ans,frame);
00269  
00270         if (size == 0)
00271             size = sens_itf_check_other_cmds(&cmd, &ans,frame);
00272 
00273         if (size == 0)
00274         {
00275             ans.hdr.status = SENS_ITF_ANS_ERROR;
00276             sLCD.printf("   D");
00277         }
00278  
00279         size = sens_itf_pack_cmd_res(&ans,frame);
00280         sens_itf_sensor_send_frame(frame, size);
00281     }
00282 }
00283 
00284 void sens_itf_init_point_db(void)
00285 {
00286     uint8_t n;
00287     char *point_names[SENS_ITF_POINT_NAME_SIZE] = { "LIGHT", "LEDG", "ACCX", "ACCY", "ACCXZ" };
00288     uint8_t data_types[SENS_ITF_POINT_NAME_SIZE] = {SENS_ITF_DT_U8, SENS_ITF_DT_U8, SENS_ITF_DT_FLOAT, SENS_ITF_DT_FLOAT, SENS_ITF_DT_FLOAT};
00289     uint8_t access_rights[SENS_ITF_POINT_NAME_SIZE] = { SENS_ITF_ACCESS_READ_ONLY, SENS_ITF_ACCESS_WRITE_ONLY, SENS_ITF_ACCESS_READ_ONLY,
00290         SENS_ITF_ACCESS_READ_ONLY, SENS_ITF_ACCESS_READ_ONLY};
00291     uint32_t sampling_time[SENS_ITF_POINT_NAME_SIZE] = {4*10, 0, 4*15, 4*20, 4*25};
00292 
00293     memset(&sensor_points, 0, sizeof(sensor_points));
00294     memset(&board_info, 0, sizeof(board_info));
00295     
00296     strcpy((char *)board_info.model, "KL46Z");
00297     strcpy((char *)board_info.manufactor, "TESLA");
00298     board_info.sensor_id = 0xDEADBEEF;
00299     board_info.hardware_revision = 0x01;
00300     board_info.num_of_points = SENS_ITF_SENSOR_NUM_OF_POINTS;
00301     board_info.cabalities = SENS_ITF_CAPABILITIES_DISPLAY |
00302         SENS_ITF_CAPABILITIES_WPAN_STATUS | 
00303         SENS_ITF_CAPABILITIES_BATTERY_STATUS;
00304 
00305     sensor_points.num_of_points = SENS_ITF_SENSOR_NUM_OF_POINTS;
00306 
00307     for (n = 0; n < SENS_ITF_SENSOR_NUM_OF_POINTS; n++)
00308     {
00309         strcpy((char *)sensor_points.points[n].desc.name, point_names[n]);
00310         sensor_points.points[n].desc.type = data_types[n];
00311         sensor_points.points[n].desc.unit = 0; // TDB
00312         sensor_points.points[n].desc.access_rights = access_rights[n];
00313         sensor_points.points[n].desc.sampling_time_x250ms = sampling_time[n];
00314         sensor_points.points[n].value.type = data_types[n];
00315     }
00316 }
00317 
00318 static void sens_itf_rx_tmrout_timer_func(void)
00319 {
00320     //greenLED = greenLED == 1 ? 0 : 1; 
00321     frame_timeout = 1;
00322 }
00323 
00324 static void sens_itf_acq_data_timer_func(void)
00325 {
00326     redLED = redLED == 1 ? 0 : 1;   
00327     acq_data = 1;
00328 }
00329 
00330 static void sens_itf_rx_tmrout_timer_reesched(void)
00331 {
00332     rx_trmout_timer.detach();
00333     rx_trmout_timer.attach_us(sens_itf_rx_tmrout_timer_func,500*1000);
00334 }
00335 
00336 // Serial or SPI interrupt, called when a new byte is received
00337 static void sens_itf_sensor_rx_byte(void)
00338 {
00339     uint8_t value;
00340     
00341     // DISABLE INTERRUPTS
00342     if (frame_timeout)
00343         return;
00344 
00345     value = (uint8_t) pcSerial.getc();
00346     
00347     if (num_rx_bytes < SENS_ITF_MAX_FRAME_SIZE)
00348         rx_frame[num_rx_bytes] = value;
00349     
00350     num_rx_bytes++;
00351     if (num_rx_bytes >= SENS_ITF_MAX_FRAME_SIZE)
00352         num_rx_bytes = 0;
00353 
00354     sens_itf_rx_tmrout_timer_reesched();
00355     // ENABLE INTERRUPTS
00356 }
00357 
00358 uint8_t sens_itf_sensor_init(void)
00359 {
00360 
00361     sens_itf_init_point_db();
00362     memcpy(main_svr_addr,"1212121212121212",SENS_ITF_SERVER_ADDR_SIZE);
00363     memcpy(secon_svr_addr,"aabbccddeeff1122",SENS_ITF_SERVER_ADDR_SIZE);
00364     num_rx_bytes = 0;
00365     acq_data = 0;
00366     frame_timeout = 0;
00367     greenLED = 0; 
00368     redLED = 1;
00369     sens_itf_rx_tmrout_timer_reesched();
00370     acq_data_timer.attach(sens_itf_acq_data_timer_func,2);
00371     pcSerial.attach(sens_itf_sensor_rx_byte);
00372     
00373     return 1;
00374 }
00375 
00376 static int pt_data_func(struct pt *pt)
00377 {
00378     PT_BEGIN(pt);
00379 
00380     while (1)
00381     {
00382         // wait a frame timeout
00383         PT_WAIT_UNTIL(pt, frame_timeout == 1);
00384 
00385         if (num_rx_bytes > 0)
00386         {
00387             // process it
00388             sens_itf_process_cmd(rx_frame, num_rx_bytes);
00389             num_rx_bytes = 0;
00390         }
00391 
00392         // restart reception
00393         frame_timeout = 0;
00394         sens_itf_rx_tmrout_timer_reesched();
00395     }
00396 
00397     PT_END(pt);
00398 }
00399 
00400 static int pt_updt_func(struct pt *pt)
00401 {
00402     PT_BEGIN(pt);
00403 
00404     while (1)
00405     {
00406         char buf[5];    
00407         uint8_t v;
00408         
00409         // wait job
00410         PT_WAIT_UNTIL(pt, acq_data == 1);
00411 
00412         v = (uint8_t)(lightSensor.read()*100);  
00413         sensor_points.points[0].value.value.u8 = v;
00414         greenLED = sensor_points.points[1].value.value.u8;
00415         sensor_points.points[2].value.value.fp32 = 1.0 - abs(acc.getAccX());
00416         sensor_points.points[3].value.value.fp32 = 1.0 - abs(acc.getAccY());
00417         sensor_points.points[4].value.value.fp32 = 1.0 - abs(acc.getAccZ());
00418         
00419         sLCD.All_Segments(0);
00420         switch(view_mode)
00421         {
00422             case 0:
00423                 sprintf(buf,"L %2d",v);
00424                 sLCD.printf("%s",buf);      
00425                 break;
00426             case 1:
00427                 sprintf(buf,"%4d",(uint16_t) (sensor_points.points[2].value.value.fp32*1000));
00428                 sLCD.printf("%s",buf);      
00429                 break;
00430             case 2:
00431                 sprintf(buf,"%4d",(uint16_t) (sensor_points.points[3].value.value.fp32*1000));
00432                 sLCD.printf("%s",buf);      
00433                 break;
00434             case 3:
00435                 sprintf(buf,"%4d",(uint16_t) (sensor_points.points[4].value.value.fp32*1000));
00436                 sLCD.printf("%s",buf);      
00437                 break;
00438             default:
00439                 break;  
00440         }
00441    
00442         acq_data = 0;
00443     }
00444 
00445     PT_END(pt);
00446 }
00447 
00448 void set_mode(void)
00449 {
00450     view_mode = ++view_mode > 3 ? 0 : view_mode;
00451 }
00452 
00453 void main(void)
00454 {
00455     frame_timeout = 0;
00456     acq_data = 0;
00457     view_mode = 0;
00458 
00459     sLCD.All_Segments(0); 
00460     sLCD.DP2(0);
00461     sLCD.printf("INIT"); 
00462     pcSerial.baud(115200);
00463     sens_itf_sensor_init();
00464     
00465     mode_switch.rise(set_mode);
00466     
00467     PT_INIT(&pt_data);
00468     PT_INIT(&pt_updt);
00469 
00470     
00471     while(1)
00472     {
00473         pt_data_func(&pt_data);
00474         pt_updt_func(&pt_updt);
00475     }    
00476 }
00477