diff --git a/FS-AI_API/fs-ai_api.c b/FS-AI_API/fs-ai_api.c index 4062624..baffb49 100644 --- a/FS-AI_API/fs-ai_api.c +++ b/FS-AI_API/fs-ai_api.c @@ -712,9 +712,16 @@ void fs_ai_api_ai2vcu_set_data(fs_ai_api_ai2vcu *data) { float t_AI2VCU_FRONT_BRAKE_PRESS_REQUEST_pct = 0; float t_AI2VCU_REAR_BRAKE_PRESS_REQUEST_pct = 0; - float t_FRONT_AXLE_TORQUE_MAX = (0.1f*VCU2AI_FRONT_AXLE_TORQUE_MAX_raw); - float t_REAR_AXLE_TORQUE_MAX = (0.1f*VCU2AI_REAR_AXLE_TORQUE_MAX_raw); - float t_STEER_ANGLE_MAX = (0.1f*VCU2AI_STEER_ANGLE_MAX_raw); + float t_FRONT_AXLE_TORQUE_MAX_Nm = (0.1f*VCU2AI_FRONT_AXLE_TORQUE_MAX_raw); + float t_REAR_AXLE_TORQUE_MAX_Nm = (0.1f*VCU2AI_REAR_AXLE_TORQUE_MAX_raw); + float t_STEER_ANGLE_MAX_deg = (0.1f*VCU2AI_STEER_ANGLE_MAX_raw); + + uint16_t t_fastest_wheel_rpm = 0; + + if(VCU2AI_FL_WHEEL_SPEED_rpm > t_fastest_wheel_rpm) { t_fastest_wheel_rpm = VCU2AI_FL_WHEEL_SPEED_rpm; } + if(VCU2AI_FR_WHEEL_SPEED_rpm > t_fastest_wheel_rpm) { t_fastest_wheel_rpm = VCU2AI_FR_WHEEL_SPEED_rpm; } + if(VCU2AI_RL_WHEEL_SPEED_rpm > t_fastest_wheel_rpm) { t_fastest_wheel_rpm = VCU2AI_RL_WHEEL_SPEED_rpm; } + if(VCU2AI_RR_WHEEL_SPEED_rpm > t_fastest_wheel_rpm) { t_fastest_wheel_rpm = VCU2AI_RR_WHEEL_SPEED_rpm; } t_AI2VCU_MISSION_STATUS = data->AI2VCU_MISSION_STATUS; t_AI2VCU_DIRECTION_REQUEST = data->AI2VCU_DIRECTION_REQUEST; @@ -728,9 +735,27 @@ void fs_ai_api_ai2vcu_set_data(fs_ai_api_ai2vcu *data) { t_AI2VCU_FRONT_BRAKE_PRESS_REQUEST_pct = data->AI2VCU_BRAKE_PRESS_REQUEST_pct; t_AI2VCU_REAR_BRAKE_PRESS_REQUEST_pct = data->AI2VCU_BRAKE_PRESS_REQUEST_pct; + // additional torque limit to maintain constant electrical power and minimise risk of over-current trip + if(t_fastest_wheel_rpm > 700) { + if(t_FRONT_AXLE_TORQUE_MAX_Nm > 50.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 50.0f; } + if(t_REAR_AXLE_TORQUE_MAX_Nm > 50.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 50.0f; } + } else if(t_fastest_wheel_rpm > 600) { + if(t_FRONT_AXLE_TORQUE_MAX_Nm > 85.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 85.0f; } + if(t_REAR_AXLE_TORQUE_MAX_Nm > 85.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 85.0f; } + } else if(t_fastest_wheel_rpm > 500) { + if(t_FRONT_AXLE_TORQUE_MAX_Nm > 100.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 100.0f; } + if(t_REAR_AXLE_TORQUE_MAX_Nm > 100.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 100.0f; } + } else if(t_fastest_wheel_rpm > 400) { + if(t_FRONT_AXLE_TORQUE_MAX_Nm > 120.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 120.0f; } + if(t_REAR_AXLE_TORQUE_MAX_Nm > 120.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 120.0f; } + } else if(t_fastest_wheel_rpm > 300) { + if(t_FRONT_AXLE_TORQUE_MAX_Nm > 150.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 150.0f; } + if(t_REAR_AXLE_TORQUE_MAX_Nm > 150.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 150.0f; } + } + // validate the 'float' requests - if(t_AI2VCU_STEER_ANGLE_REQUEST_deg > t_STEER_ANGLE_MAX) { t_AI2VCU_STEER_ANGLE_REQUEST_deg = t_STEER_ANGLE_MAX; } - if(t_AI2VCU_STEER_ANGLE_REQUEST_deg < (-1.0f*t_STEER_ANGLE_MAX)) { t_AI2VCU_STEER_ANGLE_REQUEST_deg = (-1.0f*t_STEER_ANGLE_MAX); } + if(t_AI2VCU_STEER_ANGLE_REQUEST_deg > t_STEER_ANGLE_MAX_deg) { t_AI2VCU_STEER_ANGLE_REQUEST_deg = t_STEER_ANGLE_MAX_deg; } + if(t_AI2VCU_STEER_ANGLE_REQUEST_deg < (-1.0f*t_STEER_ANGLE_MAX_deg)) { t_AI2VCU_STEER_ANGLE_REQUEST_deg = (-1.0f*t_STEER_ANGLE_MAX_deg); } if(t_AI2VCU_FRONT_MOTOR_SPEED_REQUEST_rpm > MOTOR_MAX_RPM) { t_AI2VCU_FRONT_MOTOR_SPEED_REQUEST_rpm = MOTOR_MAX_RPM; } if(t_AI2VCU_FRONT_MOTOR_SPEED_REQUEST_rpm < 0.0f) { t_AI2VCU_FRONT_MOTOR_SPEED_REQUEST_rpm = 0.0f; } @@ -738,10 +763,10 @@ void fs_ai_api_ai2vcu_set_data(fs_ai_api_ai2vcu *data) { if(t_AI2VCU_REAR_MOTOR_SPEED_REQUEST_rpm > MOTOR_MAX_RPM) { t_AI2VCU_REAR_MOTOR_SPEED_REQUEST_rpm = MOTOR_MAX_RPM; } if(t_AI2VCU_REAR_MOTOR_SPEED_REQUEST_rpm < 0.0f) { t_AI2VCU_REAR_MOTOR_SPEED_REQUEST_rpm = 0.0f; } - if(t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm > t_FRONT_AXLE_TORQUE_MAX) { t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm = t_FRONT_AXLE_TORQUE_MAX; } + if(t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm > t_FRONT_AXLE_TORQUE_MAX_Nm) { t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm = t_FRONT_AXLE_TORQUE_MAX_Nm; } if(t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm < 0.0f) { t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm = 0.0f; } - if(t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm > t_REAR_AXLE_TORQUE_MAX) { t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm = t_REAR_AXLE_TORQUE_MAX; } + if(t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm > t_REAR_AXLE_TORQUE_MAX_Nm) { t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm = t_REAR_AXLE_TORQUE_MAX_Nm; } if(t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm < 0.0f) { t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm = 0.0f; } if(t_AI2VCU_FRONT_BRAKE_PRESS_REQUEST_pct > 100.0f) { t_AI2VCU_FRONT_BRAKE_PRESS_REQUEST_pct = 100.0f; } diff --git a/FS-AI_API_Console/fs-ai_api_console.c b/FS-AI_API_Console/fs-ai_api_console.c index 0bd5726..63886fb 100644 --- a/FS-AI_API_Console/fs-ai_api_console.c +++ b/FS-AI_API_Console/fs-ai_api_console.c @@ -46,7 +46,7 @@ static void *loop_thread(); static fs_ai_api_ai2vcu ai2vcu_data; // assume initialised to zeros static char inputs[10] = "", outputs[80] = " "; static int data = 0; -static int timing_us = 5000; +static int timing_us = 10000; int main(int argc, char** argv) { @@ -154,8 +154,6 @@ static void *loop_thread() { printf(" \r\n"); printf("%s \r\n",outputs); - // printf("\033[27A"); // move cursor back up - // send some data if(HANDSHAKE_RECEIVE_BIT_OFF == vcu2ai_data.VCU2AI_HANDSHAKE_RECEIVE_BIT) { ai2vcu_data.AI2VCU_HANDSHAKE_SEND_BIT = HANDSHAKE_SEND_BIT_OFF; @@ -167,7 +165,7 @@ static void *loop_thread() { fs_ai_api_ai2vcu_set_data(&ai2vcu_data); - // repeat roughly every 5ms + // repeat according to loop timing (default roughly every 10ms) usleep(timing_us); } diff --git a/FS-AI_API_Tester/fs-ai_api_tester.c b/FS-AI_API_Tester/fs-ai_api_tester.c index 2db4980..8f392c4 100644 --- a/FS-AI_API_Tester/fs-ai_api_tester.c +++ b/FS-AI_API_Tester/fs-ai_api_tester.c @@ -78,8 +78,8 @@ int main(int argc, char** argv) { printf("VCU2AI_FR_PULSE_COUNT %5u \r\n",vcu2ai_data.VCU2AI_FR_PULSE_COUNT); printf("VCU2AI_RL_PULSE_COUNT %5u \r\n",vcu2ai_data.VCU2AI_RL_PULSE_COUNT); printf("VCU2AI_RR_PULSE_COUNT %5u \r\n",vcu2ai_data.VCU2AI_RR_PULSE_COUNT); - - // printf("\033[21A"); // move cursor back up + + // TODO: add GPS & IMU data // send some data if(HANDSHAKE_RECEIVE_BIT_OFF == vcu2ai_data.VCU2AI_HANDSHAKE_RECEIVE_BIT) { @@ -90,9 +90,9 @@ int main(int argc, char** argv) { printf("HANDSHAKE_BIT error\r\n"); } + ai2vcu_data.AI2VCU_MISSION_STATUS = MISSION_SELECTED; ai2vcu_data.AI2VCU_DIRECTION_REQUEST = DIRECTION_FORWARD; ai2vcu_data.AI2VCU_ESTOP_REQUEST = ESTOP_NO; - ai2vcu_data.AI2VCU_MISSION_STATUS = MISSION_SELECTED; ai2vcu_data.AI2VCU_STEER_ANGLE_REQUEST_deg = 9.9f; ai2vcu_data.AI2VCU_AXLE_SPEED_REQUEST_rpm = 999.0f; ai2vcu_data.AI2VCU_AXLE_TORQUE_REQUEST_Nm = 99.0f; @@ -100,8 +100,8 @@ int main(int argc, char** argv) { fs_ai_api_ai2vcu_set_data(&ai2vcu_data); - // repeat roughly every 5ms - usleep(5000); + // repeat roughly every 10ms + usleep(10000); } return(0);