/*** Move forward ***/
void move_forward() {
int i=0;
int sensor_detect=0;
for(i=0; i!=8; i++) { // loop 8 times and detect objects every 250 ms
display_movement(&forward);
/* Detect any objects in the way */
sensor_detect = get_sensor_data();
while(sensor_detect){
MOTORPORT=0xFF;
//stop execution of queue
ms_delay(250);
sensor_detect = get_sensor_data();
}
MOTORDDR=0xFF; // enable motor port as output
MOTORPORT=0xFA; // turn on motor port(0)
ms_delay(250);
}
if(training) { // if training mode boolean is 1, add direction to array
add_to_array(1);
}
move_stop();
}
/*
* process incoming data request on node
* function sends back the sensor data to coord
* node --> coord#
*/
void app_fh_com_process_data_req(uint8_t* pUDPpacket) {
int i = 0;
//UART_PRINT("NODE: got SN_data_request\r\n");
char* u = get_sensor_data();
SN_data_frame_t pdata;
pdata.command = COMMAND_COORD_DATA_RESPONSE;
pdata.length = strlen(u);
for (i = 0; i < strlen(u); i++) {
pdata.payload[i] = u[i];
UART_PRINT("%c",u[i]);
}
send_SN_data_wireless(DEFAULT_COORD_ADDR, (uint8_t *) &pdata,
sizeof(SN_data_frame_t), UDP_PORT_SENSN_END_ROUTER,
UDP_PORT_SENSN_COORD);
}
int main(void)
{
/* Unlock protected registers */
SYS_UnlockReg();
/* Init System, peripheral clock and multi-function I/O */
SYS_Init();
/* Lock protected registers */
SYS_LockReg();
init_neuron_system();
init_block( );
while(1)
{
// protocol.
parse_uart0_recv_buffer();
flush_uart1_to_uart0();
flush_uart0_local_buffer();
// led.
poll_led_request();
get_sensor_data();
// on line.
if(g_block_no != 0)
{
send_sensor_report_online();
}
// off line.
else if(g_block_no == 0)
{
uint32_t current_millis = millis();
if((current_millis - s_offline_start_mills) > OFF_LINE_REPORT_PERIOD)
{
s_offline_start_mills = current_millis;
send_sensor_report_offline();
}
}
}
}
static void *read_data(void *param)
{
char *buffer_access;
char data[1048], *dptr, tmp[24];
short sensor[3];
int q[3], i, ind, left_over_size, buf_size;
int ret, fp,read_size;
unsigned short hdr;
bool done_flag;
#define PRESSURE_HDR 0x8000
#define ACCEL_HDR 0x4000
#define GYRO_HDR 0x2000
#define COMPASS_HDR 0x1000
#define LPQUAT_HDR 0x0800
#define SIXQUAT_HDR 0x0400
#define PEDQUAT_HDR 0x0200
#define STEP_DETECTOR_HDR 0x0100
#define STEP_INDICATOR_HDR 0x0001
#define END_MARKER 0x0010
#define EMPTY_MARKER 0x0020
printf("read_data Thread: %s\n", dev_dir_name);
ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, dev_dir_name);
if (ret < 0)
goto error_alloc_scan_el_dir;
ret = asprintf(&buffer_access, "/dev/iio:device%d", dev_num);
if (ret < 0)
goto error_alloc_buffer_access;
fp = open(buffer_access, O_RDONLY | O_NONBLOCK);
if (fp == -1) { /*If it isn't there make the node */
printf("Failed to open %s\n", buffer_access);
ret = -errno;
goto error_open_buffer_access;
}
ind = 0;
while(1) {
struct pollfd pfd = {
.fd = fp,
.events = POLLIN,
};
poll(&pfd, 1, -1);
if (left_over_size > 0)
memcpy(data, tmp, left_over_size);
dptr = data + left_over_size;
read_size = read(fp, dptr, 1024);
printf("readsize=%d, left_over_size=%d\n", read_size, left_over_size);
if (read_size <= 0) {
printf("Wrong size=%d\n", read_size);
pthread_mutex_unlock(&data_switch_lock);
continue;
}
ind = read_size + left_over_size;
dptr = data;
printf("ind=%d\n", ind);
buf_size = ind - (dptr - data);
done_flag = false;
while ((buf_size > 0) && (!done_flag)) {
hdr = *((short *)(dptr));
if ((hdr & 0xf) && (hdr != STEP_INDICATOR_HDR))
printf("STEP$$$$$$$$$$$$$$$=%x ", hdr);
switch (hdr & (~0xf)) {
case PRESSURE_HDR:
if (buf_size >= 16) {
get_sensor_data(dptr, sensor);
dptr += 8;
printf("PRESSURE:%d, %lld\n", (sensor[1] << 16) + (unsigned short)sensor[2], *(long long *)dptr);
} else
done_flag = true;
break;
case ACCEL_HDR:
if (buf_size >= 16) {
get_sensor_data(dptr, sensor);
dptr += 8;
printf("A:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr);
} else
done_flag = true;
break;
case GYRO_HDR:
if (buf_size >= 16) {
get_sensor_data(dptr, sensor);
dptr += 8;
printf("G:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr);
} else
done_flag = true;
break;
case COMPASS_HDR:
if (buf_size >= 16) {
get_sensor_data(dptr, sensor);
dptr += 8;
printf("M:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr);
} else
done_flag = true;
break;
case PEDQUAT_HDR:
if (buf_size >= 16) {
get_sensor_data(dptr, sensor);
dptr += 8;
printf("PED:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr);
} else
done_flag = true;
break;
case LPQUAT_HDR:
if (buf_size >= 24) {
q[0] = *(int *)(dptr + 4);
dptr += 8;
q[1] = *(int *)(dptr);
q[2] = *(int *)(dptr + 4);
dptr += 8;
printf("LPQ:%d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr);
} else
done_flag = true;
break;
case SIXQUAT_HDR:
if (buf_size >= 24) {
q[0] = *(int *)(dptr + 4);
dptr += 8;
q[1] = *(int *)(dptr);
q[2] = *(int *)(dptr + 4);
dptr += 8;
printf("SIXQ:%d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr);
} else
done_flag = true;
break;
case STEP_DETECTOR_HDR:
if (buf_size >= 16) {
printf("STEP DETECTOR ");
dptr += 8;
printf(" %lld\n", *(long long *)dptr);
} else
done_flag = true;
break;
default:
if (hdr == EMPTY_MARKER) {
printf("emptry marker !!!!!!!!!!!\n");
} else if (hdr == END_MARKER) {
printf("end marker !!!!!\n");
} else {
dptr +=8;
printf("%lld\n", *(long long *)dptr);
}
break;
}
if (!done_flag)
dptr += 8;
buf_size = ind - (dptr - data);
}
if (ind - (dptr - data) > 0)
memcpy(tmp, dptr, ind - (dptr - data));
left_over_size = ind - (dptr - data);
}
close(fp);
error_open_buffer_access:
free(buffer_access);
error_alloc_buffer_access:
free(scan_el_dir);
error_alloc_scan_el_dir:
return 0;
}
int SensorCfg::delay( int i) {
if (i < num_sensors)
return get_sensor_data( i, X_delay );
return( 0 );
}
int SensorCfg::zone( int i) {
if (i < num_sensors)
return get_sensor_data( i, X_zone );
return( -1 );
}
int SensorCfg::green( int i) {
if (i < num_sensors)
return get_sensor_data( i, X_green );
return( -1 );
}
int SensorCfg::red( int i) {
if (i < num_sensors)
return get_sensor_data( i, X_red );
return( -1 );
}
// accessor functions for sensor configuration info
bool SensorCfg::sense( int i) {
if (i < num_sensors)
return ((get_sensor_data( i, X_info ) & S_hi) != 0);
return( 0 );
}
static int read_data(char *buffer_access, iio_info* iio_info)
{
#define ACCEL_HDR 0x4000
#define GYRO_HDR 0x2000
#define COMPASS_HDR 0x1000
static int left_over_size = 0;
char data[1048], *dptr, tmp[24];
int q[3];
int ret, i, ind, fp;
int buf_size, read_size;
unsigned short hdr;
bool done_flag;
fp = open(buffer_access, O_RDONLY | O_NONBLOCK);
if(fp == -1)
{ /* if it isn't there make the node */
printf("Failed to open %s\n", buffer_access);
return -errno;
}
ind = 0;
while(1)
{
struct pollfd pfd = {
.fd = fp,
.events = POLLIN,
};
poll(&pfd, 1, -1);
if(left_over_size > 0)
memcpy(data, tmp, left_over_size);
dptr = data + left_over_size;
read_size = read(fp, dptr, 1024);
if(read_size <= 0)
{
printf("Wrong size=%d\n", read_size);
return -EINVAL;
}
ind = read_size + left_over_size;
dptr = data;
buf_size = ind - (dptr - data);
done_flag = false;
while ((buf_size > 0) && (!done_flag)) {
hdr = *((short *)(dptr));
if (hdr & 1)
printf("STEP\n");
switch (hdr & (~1)) {
case ACCEL_HDR:
if (buf_size >= 16) {
get_sensor_data(dptr, iio_info);
dptr += 8;
// printf("ACCEL, %d, %d, %d\n", iio_info->x, iio_info->y, iio_info->z);
} else
done_flag = true;
break;
case GYRO_HDR:
if (buf_size >= 16) {
get_sensor_data(dptr, iio_info + 1);
dptr += 8;
// printf("GYRO, %d, %d, %d\n", (iio_info + 1)->x, (iio_info + 1)->y, (iio_info + 1)->z);
} else
done_flag = true;
break;
case COMPASS_HDR:
if (buf_size >= 16) {
get_sensor_data(dptr, iio_info + 2);
dptr += 8;
// printf("COMPASS, %d, %d, %d\n", (iio_info + 2)->x, (iio_info + 2)->y, (iio_info + 2)->z);
} else
done_flag = true;
break;
default:
printf("unknown, \n");
for (i = 0; i < 8; i++)
printf("%02x, ", dptr[i]);
printf("\n");
break;
}
if (!done_flag)
dptr += 8;
buf_size = ind - (dptr - data);
}
if (ind - (dptr - data) > 0)
memcpy(tmp, dptr, ind - (dptr - data));
left_over_size = ind - (dptr - data);
}
close(fp);
}
/* Read nearest ir */
static ssize_t show_proxim_ir(struct device *dev,
struct device_attribute *devattr, char *buf)
{
return get_sensor_data(dev, buf, COMMMAND1_OPMODE_PROX_ONCE);
}
/* Read lux */
static ssize_t show_lux(struct device *dev,
struct device_attribute *devattr, char *buf)
{
return get_sensor_data(dev, buf, COMMMAND1_OPMODE_ALS_ONCE);
}
int main(int argc, char** argv)
{
init_robot(&robot);
struct coord *destination;
destination = new_coord(0, 0);
destination->x = 0;
destination->y = 0;
pthread_mutex_init(&state.frame_lock, NULL);
pthread_cond_init(&state.has_frame, NULL);
robot.dest = destination;
gpioSetMode(4, PI_OUTPUT);
gpioSetMode(24, PI_OUTPUT);
gpioSetMode(5, PI_OUTPUT);
gpioSetMode(14, PI_OUTPUT);
//870 for at the ground,
// gpioServo(24,1380);
// gpioServo(4, 2100);
pthread_t range_thread, render_thread, cv_thread;
pthread_create(&range_thread, NULL, &range_read, &robot);
pthread_create(&render_thread, NULL, &render_task, NULL);
// pthread_create(&cv_thread, NULL, &find_circles, &state);
gpioWrite(5, 0);
gpioWrite(14, 0);
start_control_panel(&robot, &state);
pthread_t this_thread = pthread_self();
//set_thread_priority(this_thread, 0);
//set_thread_priority(robot.server->serve_thread, 1);
//set_thread_priority(range_thread, 2);
read(robot.encoder_handle, robot.sensor_data, sizeof(robot.sensor_data));
getQuaternion(&robot.position.orientation, robot.sensor_data);
robot.position.last_left = *((int *) (robot.sensor_data + 42));
robot.position.last_right = *((int *) (robot.sensor_data + 46));
struct robot_task *position_task, *point_task, *remote_task,
*spin_task, *move_task, *adjust_task, *stand_up_task, *fly_task,
*avoid_task, *stay_task;
position_task = (struct robot_task *)malloc(sizeof(*position_task));
position_task->task_func = &update_position2;
fly_task = (struct robot_task *)malloc(sizeof(*fly_task));
fly_task->task_func = &buzz;
point_task = (struct robot_task *)malloc(sizeof(*point_task));
robot.turret.target.x = 500;
robot.turret.target.y = 0;
robot.turret.target.z = 0;
point_task->task_func = &pointer;
remote_task = (struct robot_task *)malloc(sizeof(*remote_task));
remote_task->task_func = &remote;
avoid_task = (struct robot_task *)malloc(sizeof(*avoid_task));
avoid_task->task_func = &stay_away;
stay_task = (struct robot_task *)malloc(sizeof(*stay_task));
stay_task->task_func = &stay_within;
move_task = (struct robot_task *)malloc(sizeof(*move_task));
move_task->task_func = &move;
struct waypoint the_point;
the_point.point.x = 1000;
the_point.point.y = 0;
the_point.point.z = -40;
INIT_LIST_HEAD(&the_point.list_entry);
// list_add(&the_point.list_entry, &robot.waypoints);
adjust_task = (struct robot_task *)malloc(sizeof(*adjust_task));
adjust_task->task_func = &adjust_speeds;
stand_up_task = (struct robot_task *)malloc(sizeof(*stand_up_task));
stand_up_task->task_func = &stand_up;
INIT_LIST_HEAD(&position_task->list_item);
INIT_LIST_HEAD(&fly_task->list_item);
INIT_LIST_HEAD(&point_task->list_item);
INIT_LIST_HEAD(&remote_task->list_item);
INIT_LIST_HEAD(&avoid_task->list_item);
INIT_LIST_HEAD(&stay_task->list_item);
INIT_LIST_HEAD(&move_task->list_item);
INIT_LIST_HEAD(&adjust_task->list_item);
INIT_LIST_HEAD(&stand_up_task->list_item);
list_add_tail(&position_task->list_item, &robot.task_list);
// list_add_tail(&fly_task->list_item, &robot.task_list);
list_add_tail(&point_task->list_item, &robot.task_list);
list_add_tail(&remote_task->list_item, &robot.task_list);
// list_add_tail(&avoid_task->list_item, &robot.task_list);
// list_add_tail(&stay_task->list_item, &robot.task_list);
list_add_tail(&move_task->list_item, &robot.task_list);
list_add_tail(&adjust_task->list_item, &robot.task_list);
list_add_tail(&stand_up_task->list_item, &robot.task_list);
get_sensor_data(&robot.position2, robot.sensor_data);
init_position(&robot.position2);
//Variables for Kalman filter
struct matrix_2x2 A, B, H, Q, R, current_prob_estimate;
float current_state_estimate[2];
current_state_estimate[0] = 0;
current_state_estimate[1] = 0;
A.elements[0][0] = 1;
A.elements[0][1] = 0;
A.elements[1][0] = 0;
A.elements[1][1] = 1;
Q.elements[0][0] = 0.01;
Q.elements[0][1] = 0;
Q.elements[1][0] = 0;
Q.elements[1][1] = 0.01;
R.elements[0][0] = 0.4;
R.elements[0][1] = 0;
R.elements[1][0] = 0;
R.elements[1][1] = 0.4;
current_prob_estimate.elements[0][0] = 1;
current_prob_estimate.elements[0][1] = 0;
current_prob_estimate.elements[1][0] = 0;
current_prob_estimate.elements[1][1] = 1;
static int look_wait = 0;
while(1)
{
read(robot.encoder_handle, robot.sensor_data, sizeof(robot.sensor_data));
get_sensor_data(&robot.position2, robot.sensor_data);
// fprintf(stderr, "%i, %i\n", robot.position2.left, robot.position2.right);
if (!((robot.position2.orientation.x == robot.position2.orientation.y) &&
(robot.position2.orientation.x == robot.position2.orientation.z) &&
(robot.position2.orientation.x == robot.position2.orientation.w)))
{
do_robot_tasks(&robot);
}
state.roll = robot.turret.roll * 180 / M_PI;
static struct vect sightings[3];
int has_dest = list_empty(&robot.waypoints);
int is_spinning = list_empty(&fly_task->list_item);
// fprintf(stderr, "%i, %i\n", has_dest, is_spinning);
//Prediction Step
float predicted_state_estimate[2];
mat_mult_2xv(predicted_state_estimate, &A, current_state_estimate);
struct matrix_2x2 predicted_prob_estimate;
mat_add_2x2(&predicted_prob_estimate, & current_prob_estimate, &Q);
if (!isnan(state.x_pos) && state.set_target >= 1)
{
float x_in_vid = (state.x_pos - .5) * -2;
float y_in_vid = (state.y_pos - .5) * -2;
// Get obervation.
screen_to_world_vect(&robot, &sightings[0], x_in_vid, y_in_vid);
float observation[2];
observation[0] = sightings[0].x;
observation[1] = sightings[0].y;
float innovation[2];
innovation[0] = observation[0] - predicted_state_estimate[0];
innovation[1] = observation[1] - predicted_state_estimate[1];
float innovation_magnitude = sqrtf((innovation[0] * innovation[0]) + (innovation [1] * innovation[1]));
//fprintf(stderr, "%f, %f, %f, %f, %f, %f, %f, %f\n", innovation_magnitude, robot.turret.yaw, robot.turret.pitch, x_in_vid, y_in_vid, sightings[0].x, sightings[0].y, sightings[0].z);
struct matrix_2x2 innovation_covariance;
mat_add_2x2(&innovation_covariance, &predicted_prob_estimate, &R);
// Update
struct matrix_2x2 kalman_gain, inv_innovation_covariance;
mat_inverse_2x2(&inv_innovation_covariance, &innovation_covariance);
mat_mult_2x2(&kalman_gain, &predicted_prob_estimate, &inv_innovation_covariance);
float step[2];
mat_mult_2xv(step, &kalman_gain, innovation);
current_state_estimate[0] = step[0] + predicted_state_estimate[0];
current_state_estimate[1] = step[1] + predicted_state_estimate[1];
struct matrix_2x2 intermediate;
intermediate.elements[0][0] = 1 - kalman_gain.elements[0][0];
intermediate.elements[0][1] = 0 - kalman_gain.elements[0][1];
intermediate.elements[1][0] = 0 - kalman_gain.elements[1][0];
intermediate.elements[1][1] = 1 - kalman_gain.elements[1][1];
mat_mult_2x2(¤t_prob_estimate, &intermediate, &predicted_prob_estimate);
//fprintf(stderr, "%f, %f\n", current_state_estimate[0], current_state_estimate[1]);
if (innovation_magnitude < 800)
{
look_wait = 300;
list_del_init(&fly_task->list_item);
if (innovation_magnitude < 100)
{
if (list_empty(&robot.waypoints))
{
list_add(&the_point.list_entry, &robot.waypoints);
}
robot.turret.target.x = current_state_estimate[0];
robot.turret.target.y = current_state_estimate[1];
robot.turret.target.z = -40;
the_point.point.x = current_state_estimate[0];
the_point.point.y = current_state_estimate[1];
the_point.point.z = -40;
//fprintf(stderr, "time to stop spinning\n");
}
//fprintf(stderr, "%f, %f, %f\n", current_state_estimate[0], current_state_estimate[1], innovation_magnitude);
}
/* if (fabs(state.x_pos) > .25 || fabs(state.y_pos) > .25)
{
robot.turret.target = temp;
the_point.point = temp;
}
else
{
the_point.point = temp;
}
if (list_empty(&robot.waypoints))
{
list_del_init(&fly_task->list_item);
list_add_tail(&the_point.list_entry, &robot.waypoints);
}*/
// fprintf(stderr, "%f, %f\n", state.x_pos, state.y_pos);
// fprintf(stderr, "%f, %f, %f\n", temp.x, temp.y, temp.z);
state.set_target = 0;
}
if (look_wait > 0)
{
look_wait--;
}
if (list_empty(&robot.waypoints) && list_empty(&fly_task->list_item) && look_wait == 0)
{
list_add_tail(&fly_task->list_item, &robot.task_list);
// fprintf(stderr, "I should spin\n");
}
// fprintf(stderr, "%i, %i\n", robot.position2.left, robot.position2.right);
float distance = get_expected_distance(&robot);
// fprintf(stderr, "%f, %f\n", distance, robot.range);
// fprintf(stderr, "%f, %f, %f\n", robot.turret.target.x, robot.turret.target.y, robot.turret.target.z);
// screen_to_world_vect(&robot, &robot.turret.target, 0, 0);
// fprintf(stderr, "%f, %f, %f\n", robot.turret.target.x, robot.turret.target.y, robot.turret.target.z);
struct timeval now, then;
// printf("%f, %f, %f, %f, %f, %f\n", robot.position2.position.x, robot.position2.position.y, robot.turret.target.x, robot.turret.target.y, robot.dest->x, robot.dest->y);
gettimeofday(&now, NULL);
/* fprintf(stderr, "%ld.%06ld, %f, %f, %f, %f, %i, %i, %f\n", now.tv_sec, now.tv_usec,
robot.position2.orientation.x, robot.position2.orientation.y,
robot.position2.orientation.z, robot.position2.orientation.w,
robot.position2.left, robot.position2.right, robot.position2.heading);
*/
//printf("%f\n", robot.range);
//printf("%f, %f, %f\n", robot.position2.position.x, robot.position2.position.y, robot.position2.tilt);
// fprintf(stderr, "%f, %f, %f, %i, %i\n", robot.position2.position.x, robot.position2.position.y, robot.position2.heading, robot.position2.left, robot.position2.right);
}
}
int frizz_ioctl_sensor(struct file_id_node *node, unsigned int cmd, unsigned long arg) {
int sensor_id;
int status = 0;
packet_t packet;
int period_ms;
sensor_enable_t sensor_enable = { 0 };
sensor_delay_t sensor_delay = { 0 };
pedometer_counter_t pedo_counter = { 0 };
batch_t batch = { 0 };
packet.header.prefix = 0xFF;
packet.header.type = 0x81;
packet.header.sen_id = HUB_MGR_ID;
switch (cmd) {
case FRIZZ_IOCTL_SENSOR_SET_ENABLE:
status = copy_from_user_sensor_enable_t(cmd, (void*) arg,
&sensor_enable);
sensor_id = convert_code_to_id(sensor_enable.code);
packet.header.num = 1;
switch (sensor_enable.flag) {
case 0:
packet.data[0] = HUB_MGR_GEN_CMD_CODE(HUB_MGR_CMD_DEACTIVATE_SENSOR,
sensor_id, 0x00, 0x00);
break;
case 1:
packet.data[0] = set_sensor_active(sensor_id);
break;
}
DEBUG_PRINT("SENSOR_SET_ENABLE %x %d \n", packet.data[0], sensor_id);
status = create_frizz_workqueue((void*) &packet);
//if is the pedometer sensor shutdown, clean the data in the firmware
if((sensor_enable.code == SENSOR_TYPE_STEP_COUNTER) && (sensor_enable.flag == 0)) {
pedo_onoff = 0;
} else if((sensor_enable.code == SENSOR_TYPE_STEP_COUNTER) && (sensor_enable.flag == 1)){
pedo_onoff = 1;
}
if(sensor_enable.code == SENSOR_TYPE_GESTURE) {
if(sensor_enable.flag == 1) {
gesture_on = 1;
}else if(sensor_enable.flag == 0) {
gesture_on = 0;
}
}
//if close the gsensor, set the Gsensor_min_delay to default
if(sensor_enable.code == SENSOR_TYPE_ACCELEROMETER && sensor_enable.flag == 0) {
Gsensor_min_delay = 10000;
}
if(sensor_enable.code == SENSOR_TYPE_ACCEL_FALL_DOWN || sensor_enable.code == SENSOR_TYPE_ACCEL_POS_DET) {
set_fall_parameter();
}
break;
case FRIZZ_IOCTL_SENSOR_SET_DELAY:
copy_from_user_sensor_delay_t(cmd, (void*) arg, &sensor_delay);
sensor_id = convert_code_to_id(sensor_delay.code);
packet.header.num = 2;
packet.data[0] = HUB_MGR_GEN_CMD_CODE(HUB_MGR_CMD_SET_SENSOR_INTERVAL,
sensor_id, 0x00, 0x00);
packet.data[1] = sensor_delay.ms;
if(sensor_delay.code == SENSOR_TYPE_ACCELEROMETER) {
if(sensor_delay.ms < Gsensor_min_delay) {
packet.data[1] = sensor_delay.ms;
Gsensor_min_delay = sensor_delay.ms;
} else {
break;
}
}
DEBUG_PRINT("SENSOR_SET_DELAY %d %x\n", sensor_id, packet.data[0]);
status = create_frizz_workqueue((void*) &packet);
break;
case FRIZZ_IOCTL_SENSOR_GET_ENABLE:
get_sensor_enable(cmd, (void*) arg);
break;
case FRIZZ_IOCTL_SENSOR_GET_DELAY:
get_sensor_delay(cmd, (void*) arg);
break;
case FRIZZ_IOCTL_SENSOR_GET_DATA:
get_sensor_data(node, cmd, (void*) arg);
break;
case FRIZZ_IOCTL_SENSOR_SIMULATE_TIMEOUT:
simulate_timeout(cmd, (void*) arg);
break;
case FRIZZ_IOCTL_SENSOR_SET_BATCH:
copy_from_user_batch_t(cmd, (void*) arg, &batch);
sensor_id = convert_code_to_id(batch.code);
DEBUG_PRINT("FIFO_FULL_FLAG %d %d \n", batch.fifo_full_flag,
batch.code);
if (batch.fifo_full_flag == 1) {
update_fifo_full_flag(batch.fifo_full_flag);
}
update_timeout(batch.timeout_ns);
if (batch.period_ns < 1000000) {
period_ms = 1;
DEBUG_PRINT("1ms\n");
} else {
period_ms = div64_u64(batch.period_ns, 1000000);
DEBUG_PRINT("period ms %d\n", period_ms);
}
DEBUG_PRINT("batch sensor id %d %d \n", sensor_id, period_ms);
packet.header.num = 2;
packet.data[0] = HUB_MGR_GEN_CMD_CODE(HUB_MGR_CMD_SET_SENSOR_INTERVAL,
sensor_id, 0x00, 0x00);
packet.data[1] = period_ms;
status = create_frizz_workqueue((void*) &packet);
packet.header.num = 1;
if (batch.timeout_ns == 0) {
packet.data[0] = HUB_MGR_GEN_CMD_CODE(HUB_MGR_CMD_DEACTIVATE_SENSOR,
sensor_id, 0x00, 0x00);
} else {
DEBUG_PRINT("enable batch\n");
packet.data[0] = HUB_MGR_GEN_CMD_CODE(
HUB_MGR_CMD_SET_SENSOR_ACTIVATE, sensor_id, 0x01, 0x00);
}
status = create_frizz_workqueue((void*) &packet);
break;
case FRIZZ_IOCTL_SENSOR_FLUSH_FIFO:
DEBUG_PRINT("FLUSH\n");
break;
case FRIZZ_IOCTL_GET_SENSOR_DATA_CHANGED:
DEBUG_PRINT("GET data changed flag\n");
get_sensor_data_changed(node, cmd, (void*) arg);
break;
case FRIZZ_IOCTL_GET_FIRMWARE_VERSION:
DEBUG_PRINT("GET firmware_version");
get_firmware_version(cmd, (void*) arg);
break;
case FRIZZ_IOCTL_SET_PEDO_COUNTER:
copy_from_user_pedometer_counter_t(cmd, (void*)arg, &pedo_counter);
if(pedo_counter.counter < 0) {
kprint("Frizz pedo_counter EVALUE (%d)!!, ignore.",pedo_counter.counter);
break;
}
set_pedo_interval(pedo_counter.counter);
break;
default:
kprint("%s :NONE IOCTL SENSORxxx %x", __func__, cmd);
return -1;
break;
}
return status;
}