void calibrate_delay_loop(void)
{
L4_KernelInterfacePage_t *kip = L4_GetKernelInterface();
L4_Time_t readp = { .raw = kip->ClockInfo.X.ReadPrecision };
clock_step = time_in_us(readp);
if(clock_step > 1000000) {
printf("kernel reports scheduleprecision of %u µs (too high!)\n",
clock_step);
abort();
}
hz = 1000000 / clock_step;
printf("calibrating delay loop... (hz %u, clock_step %u)\n",
hz, clock_step);
iters_per_tick = measure(15);
assert(iters_per_tick > 0);
printf(" %lu.%02lu BogoMIPS (%lu iters / tick)\n",
iters_per_tick / (500000 / hz),
(iters_per_tick / (5000 / hz)) % 100,
iters_per_tick);
#if 0
/* testing. */
for(int i=1; i<=25; i++) {
L4_Clock_t start = L4_SystemClock();
do {
delay_loop(200);
} while(L4_SystemClock().raw < start.raw + 1);
int ticks = i * 2;
delay_loop(iters_per_tick * ticks);
L4_Clock_t end = L4_SystemClock();
if(start.raw + ticks != end.raw) {
printf("wanted to wait for %d ticks, waited for %d\n",
ticks, (int)(end.raw - start.raw));
}
}
#endif
}
/**
* Update
* DESCRIPTION: Update servo commands to track to set set course
* When engaged, this function should be called often enough to keep
* the autopilot from drifting off course.
* PRE-CONDITIONS: One or more servos engaged, flying, valid sensor data
* POST-CONDITIONS: Servos have latest corrections set in.
* EXCEPTIONS THROWN: NO_SERVOS, NOT_FLYING, NO_AHRS, NO_CDI, NO_GSI
* EXCEPTIONS HANDLED:
* */
void autopilot::Update (void)
{
float dt; // seconds since last update
unsigned curtime;
float force_error;
static int i = 0;
if (hw == NULL)
ThrowException (NO_SERVOS);
/**** Record the current time to comput delta time between updates *****/
curtime = time_in_us ();
if (last_update == 0)
dt = 1.0 / 1000000.0;
else
dt = (curtime - last_update) / 1000000.0;
last_update = curtime;
if (last_update == 0)
last_update = 1;
if (roll_engaged)
{
float heading_error;
float target_roll_angle;
float roll_angle_error;
float target_roll_angle_prime;
float roll_angle_prime_error;
if (TheAHRS->good == FALSE)
ThrowException (NO_AHRS);
switch (mode)
{
case AM_ILS:
case AM_VOR:
{
float desired_cdi_prime, cdi_prime_error;
if (TheNAVNeedles->cdi_good == FALSE)
ThrowException (NO_CDI);
desired_cdi_prime = cdi_prime_amplifier * TheNAVNeedles->cdi;
cdi_prime_error = desired_cdi_prime - TheNAVNeedles->cdi_prime;
ils_desired_heading += cdi_heading_amplifier * cdi_prime_error * dt;
if (ils_desired_heading < 0)
ils_desired_heading += 360;
else if (ils_desired_heading > 360)
ils_desired_heading -= 360;
// Now that desired heading is computed, fall through to manual
desired_heading = (int) roundf (ils_desired_heading);
}
case AM_MANUAL:
heading_error = (M_PI * desired_heading / 180) - TheAHRS->heading_angle;
break;
}
// Normalize heading error
if (heading_error < -M_PI)
heading_error += 2*M_PI;
if (heading_error > M_PI)
heading_error -= 2*M_PI;
// First, compute desired roll angle from heading error
// roll_angle_amplifier may change to be a function of ground speed
// in order to produce standard rate turns
target_roll_angle = roll_angle_amplifier * heading_error;
LIMIT_VARIABLE(target_roll_angle,min_roll_angle,max_roll_angle);
// Second, compute desired roll rate from desired roll angle and actual
roll_angle_error = target_roll_angle - TheAHRS->roll_angle;
target_roll_angle_prime = roll_angle_prime_amplifier * roll_angle_error;
LIMIT_VARIABLE(target_roll_angle_prime,min_roll_angle_prime,max_roll_angle_prime);
roll_angle_prime_error = target_roll_angle_prime - TheAHRS->roll_angle_prime;
// Last, compute aileron force/deflection from desired roll rate and actual
force_error = roll_force_amplifier * roll_angle_prime_error * dt;
aileron_force += force_error;
LIMIT_VARIABLE(aileron_force,min_aileron_force,max_aileron_force);
hw->update_aileron_servo ((int) roundf(aileron_force), 0);
//printf ("aileron = %f, ", aileron_force);
}
if (pitch_engaged)
{
float pitch_error, target_pitch_prime;
if (TheAirspeed->good == FALSE)
ThrowException (NO_AIRSPEED);
if (TheAltitude->good == FALSE)
ThrowException (NO_ALTITUDE);
if (TheAHRS->good == FALSE)
ThrowException (NO_AHRS);
if (TheAltitude->alt > (int)desired_altitude)
desired_airspeed = descent_airspeed;
else
desired_airspeed = climb_airspeed;
switch (mode)
{
case AM_ILS:
if (desired_airspeed == 0)
{
float desired_gsi_prime, gsi_prime_error;
// TODO: keep airspeed within limits
if (TheNAVNeedles->gsi_good == FALSE)
ThrowException (NO_GSI);
desired_gsi_prime = gsi_prime_amplifier * TheNAVNeedles->gsi;
gsi_prime_error = desired_gsi_prime - TheNAVNeedles->gsi_prime;
pitch_error = gsi_pitch_amplifier * gsi_prime_error * dt;
break;
}
// If desired airspeed is non 0, this is a localizer approach and we
// should descend to MDA at the descent airspeed, so just fall through to
// manual pitch control
case AM_VOR:
case AM_MANUAL:
int working_airspeed = desired_airspeed;
// Find whether the primary instrument is AI or airspeed
if ((abs(desired_altitude - TheAltitude->alt) > 100) &&
(abs(desired_airspeed - TheAirspeed->as) < 10))
{
control_airspeed:
int airspeed_error;
float desired_airspeed_prime, airspeed_prime_error;
// Primary instrument is airspeed indicator
if (++calls_to_pitch >= pitch_duty_cycle)
{
calls_to_pitch = 0;
airspeed_error = working_airspeed - TheAirspeed->as;
desired_airspeed_prime = as_prime_amplifier * airspeed_error;
airspeed_prime_error = desired_airspeed_prime - TheAirspeed->as_prime;
desired_pitch += as_pitch_amplifier * airspeed_prime_error * dt * pitch_duty_cycle;
}
} else {
int altitude_error;
float desired_altitude_prime, altitude_prime_error;
// Primary instrument is altimeter
// But first, check if we're in airspeed limits
if (TheAirspeed->as < min_airspeed)
{
working_airspeed = min_airspeed;
goto control_airspeed;
} else if (TheAirspeed->as > max_airspeed)
{
working_airspeed = max_airspeed;
goto control_airspeed;
}
altitude_error = (desired_altitude - TheAltitude->alt);
desired_altitude_prime = alt_prime_amplifier * altitude_error;
LIMIT_VARIABLE(desired_altitude_prime,min_vsi,max_vsi);
pitch_error = target_pitch_prime - TheAHRS->pitch_angle_prime;
if ((++calls_to_pitch >= pitch_duty_cycle) &&
(abs(pitch_error) < 2 * M_PI / 180))
{
calls_to_pitch = 0;
altitude_prime_error = desired_altitude_prime - TheAltitude->alt_prime;
desired_pitch += alt_pitch_amplifier *
altitude_prime_error * dt * pitch_duty_cycle;
}
}
break;
}
LIMIT_VARIABLE(desired_pitch,
min_pitch_angle,max_pitch_angle);
target_pitch_prime = pitch_prime_amplifier * (desired_pitch - TheAHRS-> pitch_angle);
pitch_error = target_pitch_prime - TheAHRS->pitch_angle_prime;
//if ((i++ % 50) == 0)
// printf ("desired_pitch = %f, actual pitch = %f, tpap = %f, pap = %f\n",
// desired_pitch * 180 / M_PI,
// TheAHRS->pitch_angle * 180 / M_PI,
// target_pitch_prime,
// TheAHRS->pitch_angle_prime);
force_error = pitch_error * pitch_force_amplifier * dt;
//printf ("desired_pitch = %f, force_error = %f\n",
// 180 * desired_pitch / M_PI,
// force_error);
elevator_force += force_error;
LIMIT_VARIABLE(elevator_force,min_elevator_force,max_elevator_force);
hw->update_elevator_servo ((int) roundf(elevator_force), 0);
}
if (rudder_engaged)
{
if (TheAHRS->good == FALSE)
ThrowException (NO_AHRS);
rudder_force += rudder_force_amplifier * TheAHRS->yaw_angle;
LIMIT_VARIABLE(rudder_force,min_rudder_force,max_rudder_force);
hw->update_rudder_servo ((int) roundf(rudder_force), 0);
//printf ("rudder = %f, ", rudder_force);
}
//if ((pitch_engaged) || (roll_engaged) || (rudder_engaged))
// putchar ('\n');
}
