quadrotor/thrust.c

/* thrust.c */

#include "fisqrt.h"
#include "timer.h"

float linearise_thrust(float x)
{
	return fisqrt(fisqrt(x*x*x));
}

void set_thrust(int motor, float x)
{
	timer_set_pwm_value(motor, (int)(linearise_thrust(x) * PWM_MAX));
}