From 8eba2fb52125777028b4f929a808e4fc5d42af0d Mon Sep 17 00:00:00 2001 From: Florent Chretien Date: Tue, 27 Jun 2023 23:56:20 +0200 Subject: [PATCH 1/3] Add a new function get_orientation Change lib version to 1.4 catch error on the serial communication for the yaw --- CMakeLists.txt | 2 +- include/serial_megapi.h | 1 + src/serial_megapi.c | 93 +++++++++++++++-------------------------- 3 files changed, 35 insertions(+), 61 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index aa2f65e..e85ebc6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.9) -project(serial_megapi VERSION 1.3 DESCRIPTION "lib to communicate with the megapi over serial") +project(serial_megapi VERSION 1.4 DESCRIPTION "lib to communicate with the megapi over serial") include(GNUInstallDirs) set(package serial_megapi) diff --git a/include/serial_megapi.h b/include/serial_megapi.h index 5aa5a83..d00b641 100644 --- a/include/serial_megapi.h +++ b/include/serial_megapi.h @@ -75,6 +75,7 @@ int is_gyro_new_data(); int is_motor_new_data(); int is_ultrasonic_new_data(char port); +int get_orientation(float * ypr); float get_gyro_roll(); float get_gyro_pitch(); float get_gyro_yaw(); diff --git a/src/serial_megapi.c b/src/serial_megapi.c index 756f161..6e004df 100644 --- a/src/serial_megapi.c +++ b/src/serial_megapi.c @@ -1,5 +1,7 @@ #include "serial_megapi.h" +#include + char makeblock_response_msg[MKBLK_MAX_MSG_SIZE]; union @@ -279,70 +281,29 @@ int decode_data(){ 0xff == makeblock_response_msg[0] && 0x55 == makeblock_response_msg[1] && 0xd == makeblock_response_msg[48] && 0xa == makeblock_response_msg[49] ){ - char data_type[9] = { - makeblock_response_msg[3], - makeblock_response_msg[8], - makeblock_response_msg[13], - makeblock_response_msg[18], - makeblock_response_msg[23], - makeblock_response_msg[28], - makeblock_response_msg[33], - makeblock_response_msg[38], - makeblock_response_msg[43] - }; - // printf("New Gyro data available \n"); + ret = 0; piLock (IMU_MUTEX) ; - for(int value = 0; value < 9; value++){ - // if(DATA_TYPE_DOUBLE == data_type[value]){ - if(DATA_TYPE_FLOAT == data_type[value]){ - - switch (value) - { - case 0x0: - data_gyro.roll_ = *(float*)(makeblock_response_msg+4 + 5*value); - ret = 0; - break; - case 0x1: - data_gyro.pitch_ = *(float*)(makeblock_response_msg+4 + 5*value); - ret = 0; - break; - case 0x2: - data_gyro.yaw_ = *(float*)(makeblock_response_msg+4 + 5*value); - ret = 0; - break; - case 0x3: - data_gyro.ang_x_ = *(float*)(makeblock_response_msg+4 + 5*value); - ret = 0; - break; - case 0x4: - data_gyro.ang_y_ = *(float*)(makeblock_response_msg+4 + 5*value); - ret = 0; - break; - case 0x5: - data_gyro.ang_z_ = *(float*)(makeblock_response_msg+4 + 5*value); - ret = 0; - break; - case 0x6: - data_gyro.lin_x_ = *(float*)(makeblock_response_msg+4 + 5*value); - ret = 0; - break; - case 0x7: - data_gyro.lin_y_ = *(float*)(makeblock_response_msg+4 + 5*value); - ret = 0; - break; - case 0x8: - data_gyro.lin_z_ = *(float*)(makeblock_response_msg+4 + 5*value); - ret = 0; - break; - - default: - ret = -1; - break; - } - } + data_gyro.roll_ = *(float*)(makeblock_response_msg+4 + 5*0); + data_gyro.pitch_ = *(float*)(makeblock_response_msg+4 + 5*1); + + // this check that the value is not error 0 and the value is not above 2Pi + // it should be between pi and -pi + // I don't know why but it happens only for the yaw.. + if(fabs( *(float*)(makeblock_response_msg+4 + 5*2)) > 0.000001 && fabs( *(float*)(makeblock_response_msg+4 + 5*2) < 6.28)){ + data_gyro.yaw_ = *(float*)(makeblock_response_msg+4 + 5*2); } + + data_gyro.ang_x_ = *(float*)(makeblock_response_msg+4 + 5*3); + data_gyro.ang_y_ = *(float*)(makeblock_response_msg+4 + 5*4); + data_gyro.ang_z_ = *(float*)(makeblock_response_msg+4 + 5*5); + + data_gyro.lin_x_ = *(float*)(makeblock_response_msg+4 + 5*6); + data_gyro.lin_y_ = *(float*)(makeblock_response_msg+4 + 5*7); + data_gyro.lin_z_ = *(float*)(makeblock_response_msg+4 + 5*8); + gyro_new_data = 1; piUnlock (IMU_MUTEX) ; + // printf(" after : %f \n", data_gyro.yaw_); } @@ -370,6 +331,18 @@ int is_ultrasonic_new_data(char port){ return new_data; } +int get_orientation(float * ypr){ + int ret = 0x0; + piLock (IMU_MUTEX) ; + ret = 0x1; + ypr[0] = data_gyro.yaw_; + ypr[1] = data_gyro.pitch_; + ypr[2] = data_gyro.roll_; + gyro_new_data = 0x0; + piUnlock (IMU_MUTEX) ; + return ret; +} + float get_gyro_roll(){ piLock (IMU_MUTEX) ; float x = data_gyro.roll_; From 363e0c87d50f12479e1b1e7573d02a74daffb46f Mon Sep 17 00:00:00 2001 From: Florent Chretien Date: Wed, 28 Jun 2023 03:00:18 +0200 Subject: [PATCH 2/3] add fonction to check valid values --- src/serial_megapi.c | 50 +++++++++++++++++++++++++++++++++++++-------- 1 file changed, 41 insertions(+), 9 deletions(-) diff --git a/src/serial_megapi.c b/src/serial_megapi.c index 6e004df..584d41b 100644 --- a/src/serial_megapi.c +++ b/src/serial_megapi.c @@ -134,6 +134,10 @@ int init_uss(const int fd, char port){ return send_data(fd, uss_msg, HEADER_MSG_SIZE + USS_MSG_SIZE); } +int check_valid_value(float value_to_check, float epsilon, float max){ + return (fabs(value_to_check) > epsilon && fabs(value_to_check) < max); +} + int decode_data(){ int ret = -1; if(0xff == makeblock_response_msg[0] && 0x55 == makeblock_response_msg[1] @@ -283,23 +287,51 @@ int decode_data(){ ){ ret = 0; piLock (IMU_MUTEX) ; - data_gyro.roll_ = *(float*)(makeblock_response_msg+4 + 5*0); - data_gyro.pitch_ = *(float*)(makeblock_response_msg+4 + 5*1); + // data_gyro.roll_ = *(float*)(makeblock_response_msg+4 + 5*0); + // data_gyro.pitch_ = *(float*)(makeblock_response_msg+4 + 5*1); // this check that the value is not error 0 and the value is not above 2Pi // it should be between pi and -pi // I don't know why but it happens only for the yaw.. - if(fabs( *(float*)(makeblock_response_msg+4 + 5*2)) > 0.000001 && fabs( *(float*)(makeblock_response_msg+4 + 5*2) < 6.28)){ + // if(fabs( *(float*)(makeblock_response_msg+4 + 5*2)) > 0.000001 && fabs( *(float*)(makeblock_response_msg+4 + 5*2) < 6.28)){ + // data_gyro.yaw_ = *(float*)(makeblock_response_msg+4 + 5*2); + // } + + float epsilon = 0.000001; + float max_angle = 6.28; + float max_speed = 6.28; + float max_accelration = 12; + if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*0), epsilon, max_angle)){ + data_gyro.roll_ = *(float*)(makeblock_response_msg+4 + 5*0); + } + if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*1), epsilon, max_angle)){ + data_gyro.pitch_ = *(float*)(makeblock_response_msg+4 + 5*1); + } + if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*2), epsilon, max_angle)){ data_gyro.yaw_ = *(float*)(makeblock_response_msg+4 + 5*2); } - data_gyro.ang_x_ = *(float*)(makeblock_response_msg+4 + 5*3); - data_gyro.ang_y_ = *(float*)(makeblock_response_msg+4 + 5*4); - data_gyro.ang_z_ = *(float*)(makeblock_response_msg+4 + 5*5); - data_gyro.lin_x_ = *(float*)(makeblock_response_msg+4 + 5*6); - data_gyro.lin_y_ = *(float*)(makeblock_response_msg+4 + 5*7); - data_gyro.lin_z_ = *(float*)(makeblock_response_msg+4 + 5*8); + if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*3), epsilon, max_speed)){ + data_gyro.ang_x_ = *(float*)(makeblock_response_msg+4 + 5*3); + } + if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*4), epsilon, max_speed)){ + data_gyro.ang_y_ = *(float*)(makeblock_response_msg+4 + 5*4); + } + if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*5), epsilon, max_speed)){ + data_gyro.ang_z_ = *(float*)(makeblock_response_msg+4 + 5*5); + + } + + if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*6), epsilon, max_accelration)){ + data_gyro.lin_x_ = *(float*)(makeblock_response_msg+4 + 5*6); + } + if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*7), epsilon, max_accelration)){ + data_gyro.lin_y_ = *(float*)(makeblock_response_msg+4 + 5*7); + } + if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*8), epsilon, max_accelration)){ + data_gyro.lin_z_ = *(float*)(makeblock_response_msg+4 + 5*8); + } gyro_new_data = 1; piUnlock (IMU_MUTEX) ; From 6309600519e37e551d3da560453edce22b78c6e5 Mon Sep 17 00:00:00 2001 From: Florent Chretien Date: Thu, 29 Jun 2023 01:27:14 +0200 Subject: [PATCH 3/3] Add Other_Lock add new functions for uss protected update check valid_values --- include/serial_megapi.h | 2 ++ src/serial_megapi.c | 53 +++++++++++++++++++++++++++++++++++++---- 2 files changed, 51 insertions(+), 4 deletions(-) diff --git a/include/serial_megapi.h b/include/serial_megapi.h index d00b641..4f52548 100644 --- a/include/serial_megapi.h +++ b/include/serial_megapi.h @@ -76,6 +76,7 @@ int is_motor_new_data(); int is_ultrasonic_new_data(char port); int get_orientation(float * ypr); +int get_all_imu_infos(float * ypr, float * ang_vel, float * lin_acc); float get_gyro_roll(); float get_gyro_pitch(); float get_gyro_yaw(); @@ -88,6 +89,7 @@ float get_gyro_linear_z(); int get_motor_position(int port); float get_motor_speed(int port); +int get_uss_cm_safe(float * value_cm, int port); float get_uss_cm(int port); // int request_gyro_all_axes(const int fd); diff --git a/src/serial_megapi.c b/src/serial_megapi.c index 584d41b..17bd6cf 100644 --- a/src/serial_megapi.c +++ b/src/serial_megapi.c @@ -153,6 +153,7 @@ int decode_data(){ case MOTOR_DEV_ID & 0xf: piLock (MOTOR_MUTEX) ; + piLock (OTHER_MUTEX) ; // ext_id_motor = ((motor<<4)+MOTOR_DEV_ID)&0xff; int motor = ((ext_id - MOTOR_DEV_ID) >> 4); if(DATA_TYPE_FLOAT == data_type){ @@ -166,21 +167,25 @@ int decode_data(){ // printf("\nmotor pos decoder/ %d : %d", motor, encoders_pos[motor-1]); ret = 0; } + piUnlock (OTHER_MUTEX); piUnlock (MOTOR_MUTEX); break; case USS_DEV_ID: piLock (USS_MUTEX) ; + piLock (OTHER_MUTEX) ; int port = ((ext_id & 0xf0) >> 4); if(DATA_TYPE_FLOAT == data_type){ data_uss[port-1-4].distance_cm = *(float*)(makeblock_response_msg+4); data_uss[port-1-4].new_data = 0x1; ret = 0; } + piUnlock (OTHER_MUTEX) ; piUnlock (USS_MUTEX) ; break; case GYRO_DEV_ID: piLock (IMU_MUTEX) ; + piLock (OTHER_MUTEX) ; char axis = ((ext_id & 0xf0) >> 4) - GYRO_PORT; if(DATA_TYPE_FLOAT == data_type){ if(GYRO_AXE_X == axis){ @@ -195,6 +200,7 @@ int decode_data(){ } } + piUnlock (OTHER_MUTEX) ; piUnlock (IMU_MUTEX) ; break; @@ -213,6 +219,7 @@ int decode_data(){ switch (dev_id) { case GYRO_DEV_ID: piLock (IMU_MUTEX) ; + piLock (OTHER_MUTEX) ; for(int axis = 1; axis < 4; axis++){ if(DATA_TYPE_FLOAT == data_type[axis - 1]){ if(GYRO_AXE_X == axis){ @@ -227,6 +234,7 @@ int decode_data(){ } } ret = 0; + piUnlock (OTHER_MUTEX) ; piUnlock (IMU_MUTEX) ; break; @@ -242,6 +250,7 @@ int decode_data(){ char data_type[4] = {makeblock_response_msg[3], makeblock_response_msg[8], makeblock_response_msg[13], makeblock_response_msg[18]}; piLock (MOTOR_MUTEX) ; + piLock (OTHER_MUTEX) ; int motor = ((ext_id & 0xf0) >> 4); char motor_1 = motor & 0x3; char motor_2 = (motor & 0xc) >> 2; @@ -279,6 +288,7 @@ int decode_data(){ } } motor_new_data = 1; + piUnlock (OTHER_MUTEX) ; piUnlock (MOTOR_MUTEX) ; } else if ( @@ -287,6 +297,7 @@ int decode_data(){ ){ ret = 0; piLock (IMU_MUTEX) ; + piLock (OTHER_MUTEX) ; // data_gyro.roll_ = *(float*)(makeblock_response_msg+4 + 5*0); // data_gyro.pitch_ = *(float*)(makeblock_response_msg+4 + 5*1); @@ -300,7 +311,7 @@ int decode_data(){ float epsilon = 0.000001; float max_angle = 6.28; float max_speed = 6.28; - float max_accelration = 12; + float max_acceleration = 12; if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*0), epsilon, max_angle)){ data_gyro.roll_ = *(float*)(makeblock_response_msg+4 + 5*0); } @@ -323,17 +334,18 @@ int decode_data(){ } - if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*6), epsilon, max_accelration)){ + if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*6), epsilon, max_acceleration)){ data_gyro.lin_x_ = *(float*)(makeblock_response_msg+4 + 5*6); } - if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*7), epsilon, max_accelration)){ + if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*7), epsilon, max_acceleration)){ data_gyro.lin_y_ = *(float*)(makeblock_response_msg+4 + 5*7); } - if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*8), epsilon, max_accelration)){ + if(check_valid_value(*(float*)(makeblock_response_msg+4 + 5*8), epsilon, max_acceleration)){ data_gyro.lin_z_ = *(float*)(makeblock_response_msg+4 + 5*8); } gyro_new_data = 1; + piUnlock (OTHER_MUTEX) ; piUnlock (IMU_MUTEX) ; // printf(" after : %f \n", data_gyro.yaw_); @@ -375,6 +387,27 @@ int get_orientation(float * ypr){ return ret; } +int get_all_imu_infos(float * ypr, float * ang_vel, float * lin_acc){ + int ret = 0x0; + piLock (IMU_MUTEX) ; + ret = 0x1; + + ypr[0] = data_gyro.yaw_; + ypr[1] = data_gyro.pitch_; + ypr[2] = data_gyro.roll_; + + ang_vel[0] = data_gyro.ang_x_; + ang_vel[1] = data_gyro.ang_y_; + ang_vel[2] = data_gyro.ang_z_; + + lin_acc[0] = data_gyro.lin_x_; + lin_acc[1] = data_gyro.lin_y_; + lin_acc[2] = data_gyro.lin_z_; + gyro_new_data = 0x0; + piUnlock (IMU_MUTEX) ; + return ret; +} + float get_gyro_roll(){ piLock (IMU_MUTEX) ; float x = data_gyro.roll_; @@ -468,6 +501,16 @@ float get_motor_speed(int port){ // return encoders_speed[port-1]; } +int get_uss_cm_safe(float * value_cm, int port){ + int ret = 0x0; + piLock (USS_MUTEX); + ret = 0x1; + (*value_cm) = data_uss[port-1-4].distance_cm; + data_uss[port-1-4].new_data = 0x0; + piUnlock (USS_MUTEX); + return ret; +} + float get_uss_cm(int port){ piLock (USS_MUTEX); float distance_cm = data_uss[port-1-4].distance_cm; @@ -536,6 +579,7 @@ int request_uss(const int fd, char port){ void receive_msg(int fd){ int ii = 0; + piLock (OTHER_MUTEX); while (serialDataAvail (fd)){ makeblock_response_msg[ii] = serialGetchar (fd); // printf (" -> %3d", makeblock_response_msg[ii]) ; @@ -549,6 +593,7 @@ void receive_msg(int fd){ ii++; } + piUnlock (OTHER_MUTEX); if(ii >= MAKEBLOCK_MSG_SIZE ){ // printf ("\ndata_received of size: %d", ii) ;