1 /*
2 * Copyright (C) 2015-2017 Alibaba Group Holding Limited
3 *
4 *
5 */
6
7 #include <stdio.h>
8 #include <stdlib.h>
9 #include <string.h>
10 #include "aos/kernel.h"
11 #include "sensor_hal.h"
12
13
14 #define LSM6DSL_ACC_MUL 1000
15 #define MPU9250_GYRO_SENSITIVITY_2000DPS 70000
16
17 static int32_t cur_acc_factor = 61;
18 static int32_t cur_gyro_factor = MPU9250_GYRO_SENSITIVITY_2000DPS;
19
20 extern void CAN1_dataReceive(void* rxDataBuffer, uint8_t dataLen);
21
22
drv_canbus_acc_inv_mpu9250_open(void)23 static int drv_canbus_acc_inv_mpu9250_open(void)
24 {
25 return 0;
26 }
27
28
drv_canbus_acc_inv_mpu9250_read(void * buf,size_t len)29 static int drv_canbus_acc_inv_mpu9250_read(void *buf, size_t len)
30 {
31 size_t size;
32 short rxAccData[6] = {0};
33 accel_data_t *accel = (accel_data_t *)buf;
34 if(buf == NULL){
35 return -1;
36 }
37
38 size = sizeof(accel_data_t);
39 if(len < size){
40 return -1;
41 }
42
43 CAN1_dataReceive(rxAccData, 6);
44 accel->data[DATA_AXIS_X] = rxAccData[0];
45 accel->data[DATA_AXIS_Y] = rxAccData[1];
46 accel->data[DATA_AXIS_Z] = rxAccData[2];
47
48 if(cur_acc_factor != 0){
49 accel->data[DATA_AXIS_X] = (accel->data[DATA_AXIS_X] * cur_acc_factor)/LSM6DSL_ACC_MUL;
50 accel->data[DATA_AXIS_Y] = (accel->data[DATA_AXIS_Y] * cur_acc_factor)/LSM6DSL_ACC_MUL;
51 accel->data[DATA_AXIS_Z] = (accel->data[DATA_AXIS_Z] * cur_acc_factor)/LSM6DSL_ACC_MUL;
52 }
53
54 accel->timestamp = aos_now_ms();
55
56 return (int)size;
57 }
58
59
drv_canbus_acc_inv_mpu9250_init(void)60 int drv_canbus_acc_inv_mpu9250_init(void){
61 int ret = 0;
62 sensor_obj_t sensor;
63 memset(&sensor, 0, sizeof(sensor));
64 /* fill the sensor obj parameters here */
65 sensor.io_port = CAN_PORT;
66 sensor.tag = TAG_DEV_ACC;
67 sensor.path = dev_acc_path;
68 sensor.open = drv_canbus_acc_inv_mpu9250_open;
69 sensor.close = NULL;
70 sensor.read = drv_canbus_acc_inv_mpu9250_read;
71 sensor.write = NULL;
72 sensor.ioctl = NULL;
73 sensor.irq_handle = NULL;
74
75 ret = sensor_create_obj(&sensor);
76 if(unlikely(ret)){
77 return -1;
78 }
79
80 LOG("%s %s successfully \n", SENSOR_STR, __func__);
81 return 0;
82 }
83
84
85
86 //gyro
drv_canbus_gyro_inv_mpu9250_open(void)87 static int drv_canbus_gyro_inv_mpu9250_open(void)
88 {
89 return 0;
90 }
91
92
drv_canbus_gyro_inv_mpu9250_read(void * buf,size_t len)93 static int drv_canbus_gyro_inv_mpu9250_read(void *buf, size_t len)
94 {
95 size_t size;
96 short rxGyroData[6] = {0};
97 gyro_data_t *gyro = (gyro_data_t *)buf;
98 if(buf == NULL){
99 return -1;
100 }
101
102 size = sizeof(gyro_data_t);
103 if(len < size){
104 return -1;
105 }
106
107 CAN1_dataReceive(rxGyroData, 6);
108 gyro->data[DATA_AXIS_X] = rxGyroData[3];
109 gyro->data[DATA_AXIS_Y] = rxGyroData[4];
110 gyro->data[DATA_AXIS_Z] = rxGyroData[5];
111
112 if(cur_gyro_factor != 0){
113 gyro->data[DATA_AXIS_X] = (gyro->data[DATA_AXIS_X] * cur_gyro_factor);
114 gyro->data[DATA_AXIS_Y] = (gyro->data[DATA_AXIS_Y] * cur_gyro_factor);
115 gyro->data[DATA_AXIS_Z] = (gyro->data[DATA_AXIS_Z] * cur_gyro_factor);
116 }
117
118 gyro->timestamp = aos_now_ms();
119
120 return (int)size;
121 }
122
123
drv_canbus_gyro_inv_mpu9250_init(void)124 int drv_canbus_gyro_inv_mpu9250_init(void){
125 int ret = 0;
126 sensor_obj_t sensor;
127 memset(&sensor, 0, sizeof(sensor));
128 /* fill the sensor obj parameters here */
129 sensor.io_port = CAN_PORT;
130 sensor.tag = TAG_DEV_GYRO;
131 sensor.path = dev_gyro_path;
132 sensor.open = drv_canbus_gyro_inv_mpu9250_open;
133 sensor.close = NULL;
134 sensor.read = drv_canbus_gyro_inv_mpu9250_read;
135 sensor.write = NULL;
136 sensor.ioctl = NULL;
137 sensor.irq_handle = NULL;
138
139 ret = sensor_create_obj(&sensor);
140 if(unlikely(ret)){
141 return -1;
142 }
143
144 LOG("%s %s successfully \n", SENSOR_STR, __func__);
145 return 0;
146 }
147
148 SENSOR_DRV_ADD(drv_canbus_acc_inv_mpu9250_init);
149 SENSOR_DRV_ADD(drv_canbus_gyro_inv_mpu9250_init);
150
151
152