Skip to content

Commit b2f9082

Browse files
IanMurphy-RockfortIanMurphy-Rockfort
committed
Added torque limit.
1 parent ce5c7a1 commit b2f9082

3 files changed

Lines changed: 39 additions & 16 deletions

File tree

‎FS-AI_API/fs-ai_api.c‎

Lines changed: 32 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -712,9 +712,16 @@ void fs_ai_api_ai2vcu_set_data(fs_ai_api_ai2vcu *data) {
712712
float t_AI2VCU_FRONT_BRAKE_PRESS_REQUEST_pct = 0;
713713
float t_AI2VCU_REAR_BRAKE_PRESS_REQUEST_pct = 0;
714714

715-
float t_FRONT_AXLE_TORQUE_MAX = (0.1f*VCU2AI_FRONT_AXLE_TORQUE_MAX_raw);
716-
float t_REAR_AXLE_TORQUE_MAX = (0.1f*VCU2AI_REAR_AXLE_TORQUE_MAX_raw);
717-
float t_STEER_ANGLE_MAX = (0.1f*VCU2AI_STEER_ANGLE_MAX_raw);
715+
float t_FRONT_AXLE_TORQUE_MAX_Nm = (0.1f*VCU2AI_FRONT_AXLE_TORQUE_MAX_raw);
716+
float t_REAR_AXLE_TORQUE_MAX_Nm = (0.1f*VCU2AI_REAR_AXLE_TORQUE_MAX_raw);
717+
float t_STEER_ANGLE_MAX_deg = (0.1f*VCU2AI_STEER_ANGLE_MAX_raw);
718+
719+
uint16_t t_fastest_wheel_rpm = 0;
720+
721+
if(VCU2AI_FL_WHEEL_SPEED_rpm > t_fastest_wheel_rpm) { t_fastest_wheel_rpm = VCU2AI_FL_WHEEL_SPEED_rpm; }
722+
if(VCU2AI_FR_WHEEL_SPEED_rpm > t_fastest_wheel_rpm) { t_fastest_wheel_rpm = VCU2AI_FR_WHEEL_SPEED_rpm; }
723+
if(VCU2AI_RL_WHEEL_SPEED_rpm > t_fastest_wheel_rpm) { t_fastest_wheel_rpm = VCU2AI_RL_WHEEL_SPEED_rpm; }
724+
if(VCU2AI_RR_WHEEL_SPEED_rpm > t_fastest_wheel_rpm) { t_fastest_wheel_rpm = VCU2AI_RR_WHEEL_SPEED_rpm; }
718725

719726
t_AI2VCU_MISSION_STATUS = data->AI2VCU_MISSION_STATUS;
720727
t_AI2VCU_DIRECTION_REQUEST = data->AI2VCU_DIRECTION_REQUEST;
@@ -728,20 +735,38 @@ void fs_ai_api_ai2vcu_set_data(fs_ai_api_ai2vcu *data) {
728735
t_AI2VCU_FRONT_BRAKE_PRESS_REQUEST_pct = data->AI2VCU_BRAKE_PRESS_REQUEST_pct;
729736
t_AI2VCU_REAR_BRAKE_PRESS_REQUEST_pct = data->AI2VCU_BRAKE_PRESS_REQUEST_pct;
730737

738+
// additional torque limit to maintain constant electrical power and minimise risk of over-current trip
739+
if(t_fastest_wheel_rpm > 700) {
740+
if(t_FRONT_AXLE_TORQUE_MAX_Nm > 50.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 50.0f; }
741+
if(t_REAR_AXLE_TORQUE_MAX_Nm > 50.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 50.0f; }
742+
} else if(t_fastest_wheel_rpm > 600) {
743+
if(t_FRONT_AXLE_TORQUE_MAX_Nm > 85.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 85.0f; }
744+
if(t_REAR_AXLE_TORQUE_MAX_Nm > 85.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 85.0f; }
745+
} else if(t_fastest_wheel_rpm > 500) {
746+
if(t_FRONT_AXLE_TORQUE_MAX_Nm > 100.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 100.0f; }
747+
if(t_REAR_AXLE_TORQUE_MAX_Nm > 100.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 100.0f; }
748+
} else if(t_fastest_wheel_rpm > 400) {
749+
if(t_FRONT_AXLE_TORQUE_MAX_Nm > 120.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 120.0f; }
750+
if(t_REAR_AXLE_TORQUE_MAX_Nm > 120.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 120.0f; }
751+
} else if(t_fastest_wheel_rpm > 300) {
752+
if(t_FRONT_AXLE_TORQUE_MAX_Nm > 150.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 150.0f; }
753+
if(t_REAR_AXLE_TORQUE_MAX_Nm > 150.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 150.0f; }
754+
}
755+
731756
// validate the 'float' requests
732-
if(t_AI2VCU_STEER_ANGLE_REQUEST_deg > t_STEER_ANGLE_MAX) { t_AI2VCU_STEER_ANGLE_REQUEST_deg = t_STEER_ANGLE_MAX; }
733-
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); }
757+
if(t_AI2VCU_STEER_ANGLE_REQUEST_deg > t_STEER_ANGLE_MAX_deg) { t_AI2VCU_STEER_ANGLE_REQUEST_deg = t_STEER_ANGLE_MAX_deg; }
758+
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); }
734759

735760
if(t_AI2VCU_FRONT_MOTOR_SPEED_REQUEST_rpm > MOTOR_MAX_RPM) { t_AI2VCU_FRONT_MOTOR_SPEED_REQUEST_rpm = MOTOR_MAX_RPM; }
736761
if(t_AI2VCU_FRONT_MOTOR_SPEED_REQUEST_rpm < 0.0f) { t_AI2VCU_FRONT_MOTOR_SPEED_REQUEST_rpm = 0.0f; }
737762

738763
if(t_AI2VCU_REAR_MOTOR_SPEED_REQUEST_rpm > MOTOR_MAX_RPM) { t_AI2VCU_REAR_MOTOR_SPEED_REQUEST_rpm = MOTOR_MAX_RPM; }
739764
if(t_AI2VCU_REAR_MOTOR_SPEED_REQUEST_rpm < 0.0f) { t_AI2VCU_REAR_MOTOR_SPEED_REQUEST_rpm = 0.0f; }
740765

741-
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; }
766+
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; }
742767
if(t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm < 0.0f) { t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm = 0.0f; }
743768

744-
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; }
769+
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; }
745770
if(t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm < 0.0f) { t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm = 0.0f; }
746771

747772
if(t_AI2VCU_FRONT_BRAKE_PRESS_REQUEST_pct > 100.0f) { t_AI2VCU_FRONT_BRAKE_PRESS_REQUEST_pct = 100.0f; }

‎FS-AI_API_Console/fs-ai_api_console.c‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ static void *loop_thread();
4646
static fs_ai_api_ai2vcu ai2vcu_data; // assume initialised to zeros
4747
static char inputs[10] = "", outputs[80] = " ";
4848
static int data = 0;
49-
static int timing_us = 5000;
49+
static int timing_us = 10000;
5050

5151

5252
int main(int argc, char** argv) {
@@ -154,8 +154,6 @@ static void *loop_thread() {
154154
printf(" \r\n");
155155
printf("%s \r\n",outputs);
156156

157-
// printf("\033[27A"); // move cursor back up
158-
159157
// send some data
160158
if(HANDSHAKE_RECEIVE_BIT_OFF == vcu2ai_data.VCU2AI_HANDSHAKE_RECEIVE_BIT) {
161159
ai2vcu_data.AI2VCU_HANDSHAKE_SEND_BIT = HANDSHAKE_SEND_BIT_OFF;
@@ -167,7 +165,7 @@ static void *loop_thread() {
167165

168166
fs_ai_api_ai2vcu_set_data(&ai2vcu_data);
169167

170-
// repeat roughly every 5ms
168+
// repeat according to loop timing (default roughly every 10ms)
171169
usleep(timing_us);
172170
}
173171

‎FS-AI_API_Tester/fs-ai_api_tester.c‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -78,8 +78,8 @@ int main(int argc, char** argv) {
7878
printf("VCU2AI_FR_PULSE_COUNT %5u \r\n",vcu2ai_data.VCU2AI_FR_PULSE_COUNT);
7979
printf("VCU2AI_RL_PULSE_COUNT %5u \r\n",vcu2ai_data.VCU2AI_RL_PULSE_COUNT);
8080
printf("VCU2AI_RR_PULSE_COUNT %5u \r\n",vcu2ai_data.VCU2AI_RR_PULSE_COUNT);
81-
82-
// printf("\033[21A"); // move cursor back up
81+
82+
// TODO: add GPS & IMU data
8383

8484
// send some data
8585
if(HANDSHAKE_RECEIVE_BIT_OFF == vcu2ai_data.VCU2AI_HANDSHAKE_RECEIVE_BIT) {
@@ -90,18 +90,18 @@ int main(int argc, char** argv) {
9090
printf("HANDSHAKE_BIT error\r\n");
9191
}
9292

93+
ai2vcu_data.AI2VCU_MISSION_STATUS = MISSION_SELECTED;
9394
ai2vcu_data.AI2VCU_DIRECTION_REQUEST = DIRECTION_FORWARD;
9495
ai2vcu_data.AI2VCU_ESTOP_REQUEST = ESTOP_NO;
95-
ai2vcu_data.AI2VCU_MISSION_STATUS = MISSION_SELECTED;
9696
ai2vcu_data.AI2VCU_STEER_ANGLE_REQUEST_deg = 9.9f;
9797
ai2vcu_data.AI2VCU_AXLE_SPEED_REQUEST_rpm = 999.0f;
9898
ai2vcu_data.AI2VCU_AXLE_TORQUE_REQUEST_Nm = 99.0f;
9999
ai2vcu_data.AI2VCU_BRAKE_PRESS_REQUEST_pct = 99.0f;
100100

101101
fs_ai_api_ai2vcu_set_data(&ai2vcu_data);
102102

103-
// repeat roughly every 5ms
104-
usleep(5000);
103+
// repeat roughly every 10ms
104+
usleep(10000);
105105
}
106106

107107
return(0);

0 commit comments

Comments
 (0)