Diff: sens_itf/sens_itf_sensor.cpp

Revision:

1:acdf490d94a7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sens_itf/sens_itf_sensor.cpp	Tue Apr 08 16:34:20 2014 +0000
@@ -0,0 +1,477 @@
+#include <string.h>
+#include <stdint.h>
+#include "mbed.h"
+#include "sens_itf.h"
+#include "sens_util.h"
+#include "../util/buf_io.h"
+#include "../util/crc16.h"
+#include "../pt/pt.h"
+#include "SLCD.h"
+#include "MMA8451Q.h"
+
+#define SENS_ITF_SENSOR_DBG_FRAME     1
+#define SENS_ITF_DBG_FRAME 1
+#define SENS_ITF_SENSOR_NUM_OF_POINTS 5
+
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+
+static uint8_t main_svr_addr[SENS_ITF_SERVER_ADDR_SIZE];
+static uint8_t secon_svr_addr[SENS_ITF_SERVER_ADDR_SIZE];
+static uint8_t rx_frame[SENS_ITF_MAX_FRAME_SIZE];
+static volatile uint8_t num_rx_bytes;
+static Timeout rx_trmout_timer ;
+static Ticker acq_data_timer;
+static sens_itf_point_ctrl_t sensor_points;
+static sens_itf_cmd_brd_id_t board_info;
+static struct pt pt_updt;
+static struct pt pt_data;
+static volatile uint8_t frame_timeout;
+static volatile uint8_t acq_data ;
+static DigitalOut  greenLED(LED1);
+static DigitalOut  redLED(LED2);
+static AnalogIn    lightSensor(PTE22);
+static SLCD        sLCD;
+static Serial      pcSerial(USBTX, USBRX);
+static MMA8451Q    acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
+static InterruptIn mode_switch(SW3);    
+static volatile uint8_t  view_mode;
+
+void scroll_message(char *message, unsigned char len)
+{
+    sLCD.All_Segments(0);
+    
+    for (int start = 0; start < len - 4; start++)
+    {
+        for (int digit = 0; digit < 4; digit++)
+            sLCD.putc(message[start + digit]);
+        wait(0.4);
+    }
+}
+
+static void dump_frame(uint8_t *frame, uint8_t size)
+{
+	int n,m;
+	char buf[64];
+	
+	buf[0] = buf[1] = buf[2] = buf[3] = ' ';
+	
+	for(n = 0, m = 4; n < size ; n++, m+=3)
+	{
+		sprintf(&buf[m],"%02X_",frame[n]);
+	}
+	
+	buf[m] = buf[m+1] = buf[m+2] = buf[m+3] = ' ';
+	buf[m+4] = '\0';
+	
+	m +=4;
+	scroll_message(buf,m);
+	
+}
+	
+static uint8_t sens_itf_get_point_type(uint8_t point)
+{
+    return sensor_points.points[point].desc.type;
+}
+
+static uint8_t sens_itf_get_number_of_points(void)
+{
+    return SENS_ITF_SENSOR_NUM_OF_POINTS;
+}
+
+static sens_itf_cmd_point_desc_t *sens_itf_get_point_desc(uint8_t point)
+{
+    sens_itf_cmd_point_desc_t *d = 0;
+
+    if (point < sens_itf_get_number_of_points())
+        d = &sensor_points.points[point].desc;
+
+    return d;
+}
+
+static sens_itf_cmd_point_t *sens_itf_get_point_value(uint8_t point)
+{
+    sens_itf_cmd_point_t *v = 0;
+
+    if (point < sens_itf_get_number_of_points())
+        v = &sensor_points.points[point].value;
+
+    return v;
+}
+
+static uint8_t sens_itf_set_point_value(uint8_t point, sens_itf_cmd_point_t *v)
+{
+    uint8_t ret = 0;
+
+    if (point < sens_itf_get_number_of_points())
+    {
+        sensor_points.points[point].value = *v;
+        ret = 1;
+    }
+    else
+    {
+        ret = 0;
+    }
+
+    return ret;
+}
+
+static sens_itf_cmd_brd_id_t *sens_itf_get_board_info(void)
+{
+    return &board_info;
+}
+
+static uint8_t sens_itf_sensor_send_frame(uint8_t *frame, uint8_t size)
+{
+    //dump_frame(frame, size);
+    
+    for(int n = 0 ; n < size ; n++)
+    	pcSerial.putc((unsigned int )frame[n]);
+    //pcSerial.puts((char *)frame,size); // puts returns the size sent ?
+    return size;
+}
+
+static uint8_t sens_itf_sensor_check_register_map(sens_itf_cmd_req_t *cmd, sens_itf_cmd_res_t *ans, uint8_t *frame)
+{
+    uint8_t size = 0;
+    if ( // check global register map for valid address ranges
+                ((cmd->hdr.addr > SENS_ITF_REGMAP_SVR_SEC_ADDR) && 
+                (cmd->hdr.addr < SENS_ITF_REGMAP_POINT_DESC_1)) ||
+                (cmd->hdr.addr > SENS_ITF_REGMAP_WRITE_POINT_DATA_32) ||
+                // check local register map - reading
+                ((cmd->hdr.addr >= SENS_ITF_REGMAP_READ_POINT_DATA_1) && 
+                (cmd->hdr.addr <= SENS_ITF_REGMAP_READ_POINT_DATA_32) &&
+                ((cmd->hdr.addr - SENS_ITF_REGMAP_READ_POINT_DATA_1) >= sens_itf_get_number_of_points())) ||
+                // check local register map - writing
+                ((cmd->hdr.addr >= SENS_ITF_REGMAP_WRITE_POINT_DATA_1) && 
+                (cmd->hdr.addr <= SENS_ITF_REGMAP_WRITE_POINT_DATA_32) &&
+                ((cmd->hdr.addr - SENS_ITF_REGMAP_WRITE_POINT_DATA_1) >= sens_itf_get_number_of_points())))
+    {
+        sens_util_log(SENS_ITF_SENSOR_DBG_FRAME, "Invalid register address %02X",cmd->hdr.addr);
+        ans->hdr.status = SENS_ITF_ANS_REGISTER_NOT_IMPLEMENTED;
+        size = sens_itf_pack_cmd_res(ans, frame);
+    }
+    return size;
+}
+
+static uint8_t sens_itf_sensor_writings(sens_itf_cmd_req_t *cmd, sens_itf_cmd_res_t *ans, uint8_t *frame)
+{
+    uint8_t size = 0;
+    if ((cmd->hdr.addr >= SENS_ITF_REGMAP_WRITE_POINT_DATA_1) && 
+        (cmd->hdr.addr <= SENS_ITF_REGMAP_WRITE_POINT_DATA_32))
+    {
+        uint8_t point = cmd->hdr.addr - SENS_ITF_REGMAP_WRITE_POINT_DATA_1;
+        //sLCD.printf("R%dV%d",point,cmd->payload.point_value_cmd.value.u8);
+        //wait(0.5);
+        uint8_t acr = sens_itf_get_point_desc(point)->access_rights & SENS_ITF_ACCESS_WRITE_ONLY;
+            
+        if (acr)
+        {
+            ans->hdr.status = SENS_ITF_ANS_OK;
+            sens_itf_set_point_value(point,&cmd->payload.point_value_cmd);
+        }
+        else
+        {
+            sens_util_log(SENS_ITF_SENSOR_DBG_FRAME, "Point %d does not allow writings",point);                
+            ans->hdr.status = SENS_ITF_ANS_READY_ONLY;
+        }
+        size = sens_itf_pack_cmd_res(ans, frame);
+    }
+    return size;
+}
+
+static uint8_t sens_itf_sensor_readings(sens_itf_cmd_req_t *cmd, sens_itf_cmd_res_t *ans, uint8_t *frame)
+{
+    uint8_t size = 0;
+    if ((cmd->hdr.addr >= SENS_ITF_REGMAP_READ_POINT_DATA_1) &&
+        (cmd->hdr.addr <= SENS_ITF_REGMAP_READ_POINT_DATA_32))
+    {
+        uint8_t point = cmd->hdr.addr - SENS_ITF_REGMAP_READ_POINT_DATA_1;
+        uint8_t acr = sens_itf_get_point_desc(point)->access_rights & SENS_ITF_ANS_READY_ONLY;
+            
+        if (acr)
+        {
+            ans->hdr.status = SENS_ITF_ANS_OK;
+            ans->payload.point_value_cmd = *sens_itf_get_point_value(point);
+        }
+        else
+        {
+            sens_util_log(SENS_ITF_SENSOR_DBG_FRAME, "Point %d does not allow readings",point);                
+            ans->hdr.status = SENS_ITF_ANS_WRITE_ONLY;
+        }
+        size = sens_itf_pack_cmd_res(ans, frame);
+    }
+    return size;
+}
+
+static uint8_t sens_itf_check_other_cmds(sens_itf_cmd_req_t *cmd, sens_itf_cmd_res_t *ans, uint8_t *frame)
+{
+    uint8_t size = 0;
+    switch (cmd->hdr.addr)
+    {
+        case SENS_ITF_REGMAP_ITF_VERSION:
+            ans->payload.itf_version_cmd.version = SENS_ITF_LATEST_VERSION;
+            break;
+        case SENS_ITF_REGMAP_BRD_ID:
+            memcpy(&ans->payload.brd_id_cmd,sens_itf_get_board_info(),sizeof(sens_itf_cmd_brd_id_t));
+            break;
+        case SENS_ITF_REGMAP_BRD_STATUS:
+            ans->payload.brd_status_cmd.status = 0; // TBD
+            break;
+        case SENS_ITF_REGMAP_BRD_CMD:
+            ans->payload.command_res_cmd.status = 0; // TBD
+            break;
+        case SENS_ITF_REGMAP_READ_BAT_STATUS:
+            ans->payload.bat_status_cmd.status = 0; // TBD
+            break;
+        case SENS_ITF_REGMAP_READ_BAT_CHARGE:
+            ans->payload.bat_charge_cmd.charge = 100; // TBD
+            break;
+        case SENS_ITF_REGMAP_SVR_MAIN_ADDR:
+            memcpy(ans->payload.svr_addr_cmd.addr,main_svr_addr, SENS_ITF_SERVER_ADDR_SIZE); 
+            break;
+        case SENS_ITF_REGMAP_SVR_SEC_ADDR:
+            memcpy(ans->payload.svr_addr_cmd.addr,secon_svr_addr, SENS_ITF_SERVER_ADDR_SIZE);
+            break;
+        default:
+        	break;
+
+    }
+    
+    if ((cmd->hdr.addr >= SENS_ITF_REGMAP_POINT_DESC_1) && (cmd->hdr.addr <= SENS_ITF_REGMAP_POINT_DESC_32))
+    {
+        uint8_t point = cmd->hdr.addr - SENS_ITF_REGMAP_POINT_DESC_1;
+        memcpy(&ans->payload.point_desc_cmd, &sensor_points.points[point].desc, sizeof(sens_itf_cmd_point_desc_t));
+    }
+        
+    ans->hdr.status = SENS_ITF_ANS_OK;
+    size = sens_itf_pack_cmd_res(ans, frame);
+    return size;
+}
+static void sens_itf_process_cmd(uint8_t *frame, uint8_t num_rx_bytes)
+{
+    uint8_t ret;
+    uint8_t size = 0;
+    sens_itf_cmd_req_t cmd;
+    sens_itf_cmd_res_t ans;
+
+    ret = sens_itf_unpack_cmd_req(&cmd, frame, num_rx_bytes);
+
+    if (ret > 0)
+    {
+        ans.hdr.addr = cmd.hdr.addr;
+        
+        size = sens_itf_sensor_check_register_map(&cmd, &ans,frame);
+        if (size == 0)
+            size = sens_itf_sensor_writings(&cmd, &ans,frame);
+
+        if (size == 0)
+            size = sens_itf_sensor_readings(&cmd, &ans,frame);
+ 
+        if (size == 0)
+            size = sens_itf_check_other_cmds(&cmd, &ans,frame);
+
+        if (size == 0)
+        {
+            ans.hdr.status = SENS_ITF_ANS_ERROR;
+            sLCD.printf("   D");
+        }
+ 
+        size = sens_itf_pack_cmd_res(&ans,frame);
+        sens_itf_sensor_send_frame(frame, size);
+    }
+}
+
+void sens_itf_init_point_db(void)
+{
+    uint8_t n;
+    char *point_names[SENS_ITF_POINT_NAME_SIZE] = { "LIGHT", "LEDG", "ACCX", "ACCY", "ACCXZ" };
+    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};
+    uint8_t access_rights[SENS_ITF_POINT_NAME_SIZE] = { SENS_ITF_ACCESS_READ_ONLY, SENS_ITF_ACCESS_WRITE_ONLY, SENS_ITF_ACCESS_READ_ONLY,
+        SENS_ITF_ACCESS_READ_ONLY, SENS_ITF_ACCESS_READ_ONLY};
+    uint32_t sampling_time[SENS_ITF_POINT_NAME_SIZE] = {4*10, 0, 4*15, 4*20, 4*25};
+
+	memset(&sensor_points, 0, sizeof(sensor_points));
+	memset(&board_info, 0, sizeof(board_info));
+	
+    strcpy((char *)board_info.model, "KL46Z");
+    strcpy((char *)board_info.manufactor, "TESLA");
+    board_info.sensor_id = 0xDEADBEEF;
+    board_info.hardware_revision = 0x01;
+    board_info.num_of_points = SENS_ITF_SENSOR_NUM_OF_POINTS;
+    board_info.cabalities = SENS_ITF_CAPABILITIES_DISPLAY |
+        SENS_ITF_CAPABILITIES_WPAN_STATUS | 
+        SENS_ITF_CAPABILITIES_BATTERY_STATUS;
+
+    sensor_points.num_of_points = SENS_ITF_SENSOR_NUM_OF_POINTS;
+
+    for (n = 0; n < SENS_ITF_SENSOR_NUM_OF_POINTS; n++)
+    {
+        strcpy((char *)sensor_points.points[n].desc.name, point_names[n]);
+        sensor_points.points[n].desc.type = data_types[n];
+        sensor_points.points[n].desc.unit = 0; // TDB
+        sensor_points.points[n].desc.access_rights = access_rights[n];
+        sensor_points.points[n].desc.sampling_time_x250ms = sampling_time[n];
+        sensor_points.points[n].value.type = data_types[n];
+    }
+}
+
+static void sens_itf_rx_tmrout_timer_func(void)
+{
+	//greenLED = greenLED == 1 ? 0 : 1; 
+    frame_timeout = 1;
+}
+
+static void sens_itf_acq_data_timer_func(void)
+{
+    redLED = redLED == 1 ? 0 : 1;   
+    acq_data = 1;
+}
+
+static void sens_itf_rx_tmrout_timer_reesched(void)
+{
+	rx_trmout_timer.detach();
+	rx_trmout_timer.attach_us(sens_itf_rx_tmrout_timer_func,500*1000);
+}
+
+// Serial or SPI interrupt, called when a new byte is received
+static void sens_itf_sensor_rx_byte(void)
+{
+    uint8_t value;
+    
+    // DISABLE INTERRUPTS
+    if (frame_timeout)
+        return;
+
+	value = (uint8_t) pcSerial.getc();
+	
+    if (num_rx_bytes < SENS_ITF_MAX_FRAME_SIZE)
+        rx_frame[num_rx_bytes] = value;
+    
+    num_rx_bytes++;
+    if (num_rx_bytes >= SENS_ITF_MAX_FRAME_SIZE)
+        num_rx_bytes = 0;
+
+    sens_itf_rx_tmrout_timer_reesched();
+    // ENABLE INTERRUPTS
+}
+
+uint8_t sens_itf_sensor_init(void)
+{
+
+    sens_itf_init_point_db();
+    memcpy(main_svr_addr,"1212121212121212",SENS_ITF_SERVER_ADDR_SIZE);
+    memcpy(secon_svr_addr,"aabbccddeeff1122",SENS_ITF_SERVER_ADDR_SIZE);
+    num_rx_bytes = 0;
+    acq_data = 0;
+    frame_timeout = 0;
+    greenLED = 0; 
+    redLED = 1;
+    sens_itf_rx_tmrout_timer_reesched();
+    acq_data_timer.attach(sens_itf_acq_data_timer_func,2);
+	pcSerial.attach(sens_itf_sensor_rx_byte);
+	
+    return 1;
+}
+
+static int pt_data_func(struct pt *pt)
+{
+    PT_BEGIN(pt);
+
+    while (1)
+    {
+        // wait a frame timeout
+        PT_WAIT_UNTIL(pt, frame_timeout == 1);
+
+        if (num_rx_bytes > 0)
+        {
+            // process it
+            sens_itf_process_cmd(rx_frame, num_rx_bytes);
+            num_rx_bytes = 0;
+        }
+
+        // restart reception
+        frame_timeout = 0;
+        sens_itf_rx_tmrout_timer_reesched();
+    }
+
+    PT_END(pt);
+}
+
+static int pt_updt_func(struct pt *pt)
+{
+    PT_BEGIN(pt);
+
+    while (1)
+    {
+	    char buf[5];    
+	    uint8_t v;
+	    
+        // wait job
+        PT_WAIT_UNTIL(pt, acq_data == 1);
+
+	    v = (uint8_t)(lightSensor.read()*100);  
+	    sensor_points.points[0].value.value.u8 = v;
+	    greenLED = sensor_points.points[1].value.value.u8;
+	    sensor_points.points[2].value.value.fp32 = 1.0 - abs(acc.getAccX());
+	    sensor_points.points[3].value.value.fp32 = 1.0 - abs(acc.getAccY());
+	    sensor_points.points[4].value.value.fp32 = 1.0 - abs(acc.getAccZ());
+	    
+	    sLCD.All_Segments(0);
+	    switch(view_mode)
+	    {
+	    	case 0:
+			  	sprintf(buf,"L %2d",v);
+	    		sLCD.printf("%s",buf);   	
+	    		break;
+	    	case 1:
+			  	sprintf(buf,"%4d",(uint16_t) (sensor_points.points[2].value.value.fp32*1000));
+	    		sLCD.printf("%s",buf);   	
+	    		break;
+	    	case 2:
+			  	sprintf(buf,"%4d",(uint16_t) (sensor_points.points[3].value.value.fp32*1000));
+	    		sLCD.printf("%s",buf);   	
+	    		break;
+	    	case 3:
+			  	sprintf(buf,"%4d",(uint16_t) (sensor_points.points[4].value.value.fp32*1000));
+	    		sLCD.printf("%s",buf);   	
+	    		break;
+	    	default:
+	  		  	break;	
+	    }
+   
+        acq_data = 0;
+    }
+
+    PT_END(pt);
+}
+
+void set_mode(void)
+{
+	view_mode = ++view_mode > 3 ? 0 : view_mode;
+}
+
+void main(void)
+{
+    frame_timeout = 0;
+    acq_data = 0;
+    view_mode = 0;
+
+    sLCD.All_Segments(0); 
+    sLCD.DP2(0);
+    sLCD.printf("INIT"); 
+	pcSerial.baud(115200);
+    sens_itf_sensor_init();
+    
+    mode_switch.rise(set_mode);
+    
+    PT_INIT(&pt_data);
+    PT_INIT(&pt_updt);
+
+	
+    while(1)
+    {
+        pt_data_func(&pt_data);
+        pt_updt_func(&pt_updt);
+    }    
+}
+