Unverified Commit bf6d1236 authored by Adam Valt's avatar Adam Valt
Browse files

Implement steering curve for servo control

parent b36046fe

src/firmware/source/config.c

+21 −3
Original line number Diff line number Diff line
@@ -26,11 +26,29 @@ void set_regl(uint16_t duty_us) { 
	config_struct.regl.pulse_width_us = duty_us;

}



void set_servo(uint16_t duty_us) {

uint16_t real_duty_us = 1500;



uint16_t steering_curve(uint16_t input, float a) {

    float x = ((int32_t)input - 1500) / 256.0f;



    // polynome: y = a*x^3 + (1 - a)*x

    float y = a * x * x * x + (1.0f - a) * x;



    int32_t output = (int32_t)(1500 + y * 256.0f);



    if (output < MIN_SERVO_DUTY_US) output = MIN_SERVO_DUTY_US;

    if (output > MAX_SERVO_DUTY_US) output = MAX_SERVO_DUTY_US;



    return (uint16_t)output;

}



void set_servo(uint16_t requested_duty_us) {

	// new car: https://www.kavanrc.com/cs/item/go-10s41mg-0-075s-60-10-5kg-cm-157511

	custom_assert(duty_us >= MIN_SERVO_DUTY_US && duty_us <= MAX_SERVO_DUTY_US);

	SCT0->MATCHREL[2] = duty_us;

	config_struct.servo.pulse_width_us = duty_us;



	real_duty_us = steering_curve(requested_duty_us, 0.7);

	SCT0->MATCHREL[2] = real_duty_us;

	config_struct.servo.pulse_width_us = real_duty_us;

}



void handle_received_packet() {