/**
* vtol autopilot
* use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure)
* fall back to emergency fallback autopilot to keep minimum amount of flight control
*/
uint8_t VtolFlyController::RunAutoPilot()
{
enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode;
enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode;
uint8_t result = 0;
// decide on behaviour based on settings and system state
if (vtolEmergencyFallbackSwitch) {
returnmode = RETURN_0;
followermode = FOLLOWER_FALLBACK;
} else {
if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
returnmode = RETURN_1;
followermode = FOLLOWER_FALLBACK;
} else {
returnmode = RETURN_RESULT;
followermode = FOLLOWER_REGULAR;
}
}
switch (followermode) {
case FOLLOWER_REGULAR:
{
// horizontal position control PID loop works according to settings in regular mode, allowing integral terms
UpdateVelocityDesired();
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
bool yaw_attitude = true;
float yaw = 0.0f;
switch (vtolPathFollowerSettings->YawControl) {
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
yaw_attitude = false;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
yaw = updateTailInBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
yaw = updateCourseBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
yaw = updatePathBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
yaw = updatePOIBearing();
break;
}
result = UpdateStabilizationDesired(yaw_attitude, yaw);
if (!result) {
if (vtolPathFollowerSettings->FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) {
// switch to emergency follower if follower indicates problems
vtolEmergencyFallbackSwitch = true;
}
}
}
break;
case FOLLOWER_FALLBACK:
{
// fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly
controlNE.UpdatePositionalParameters(1.0f);
UpdateVelocityDesired();
// emergency follower has no return value
UpdateDesiredAttitudeEmergencyFallback();
}
break;
}
switch (returnmode) {
case RETURN_RESULT:
return result;
default:
// returns either 0 or 1 according to enum definition above
return returnmode;
}
}
/**
* Module thread, should not return.
*/
static void AutotuneTask(void *parameters)
{
enum AUTOTUNE_STATE state = AT_INIT;
uint32_t last_update_time = PIOS_Thread_Systime();
float X[AF_NUMX] = {0};
float P[AF_NUMP] = {0};
float noise[3] = {0};
af_init(X,P);
uint32_t last_time = 0.0f;
const uint32_t YIELD_MS = 2;
GyrosConnectCallback(at_new_gyro_data);
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
uint32_t diff_time;
const uint32_t PREPARE_TIME = 2000;
const uint32_t MEASURE_TIME = 60000;
static uint32_t update_counter = 0;
bool doing_ident = false;
bool can_sleep = true;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// Only allow this module to run when autotuning
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
state = AT_INIT;
PIOS_Thread_Sleep(50);
continue;
}
switch(state) {
case AT_INIT:
last_update_time = PIOS_Thread_Systime();
// Only start when armed and flying
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
af_init(X,P);
UpdateSystemIdent(X, NULL, 0.0f, 0, 0);
state = AT_START;
}
break;
case AT_START:
diff_time = PIOS_Thread_Systime() - last_update_time;
// Spend the first block of time in normal rate mode to get airborne
if (diff_time > PREPARE_TIME) {
last_time = PIOS_DELAY_GetRaw();
/* Drain the queue of all current data */
while (circ_queue_read_pos(at_queue)) {
circ_queue_read_completed(at_queue);
}
/* And reset the point spill counter */
update_counter = 0;
at_points_spilled = 0;
state = AT_RUN;
last_update_time = PIOS_Thread_Systime();
}
break;
case AT_RUN:
diff_time = PIOS_Thread_Systime() - last_update_time;
doing_ident = true;
can_sleep = false;
for (int i=0; i<MAX_PTS_PER_CYCLE; i++) {
struct at_queued_data *pt;
/* Grab an autotune point */
pt = circ_queue_read_pos(at_queue);
if (!pt) {
/* We've drained the buffer
* fully. Yay! */
can_sleep = true;
break;
}
/* calculate time between successive
* points */
float dT_s = PIOS_DELAY_DiffuS2(last_time,
pt->raw_time) * 1.0e-6f;
/* This is for the first point, but
* also if we have extended drops */
if (dT_s > 0.010f) {
dT_s = 0.010f;
}
last_time = pt->raw_time;
af_predict(X, P, pt->u, pt->y, dT_s);
// Update uavo every 256 cycles to avoid
// telemetry spam
if (!((update_counter++) & 0xff)) {
UpdateSystemIdent(X, noise, dT_s, update_counter, at_points_spilled);
}
for (uint32_t i = 0; i < 3; i++) {
const float NOISE_ALPHA = 0.9997f; // 10 second time constant at 300 Hz
noise[i] = NOISE_ALPHA * noise[i] + (1-NOISE_ALPHA) * (pt->y[i] - X[i]) * (pt->y[i] - X[i]);
}
/* Free the buffer containing an AT point */
circ_queue_read_completed(at_queue);
}
if (diff_time > MEASURE_TIME) { // Move on to next state
state = AT_FINISHED;
last_update_time = PIOS_Thread_Systime();
}
break;
case AT_FINISHED:
// Wait until disarmed and landed before saving the settings
UpdateSystemIdent(X, noise, 0, update_counter, at_points_spilled);
state = AT_WAITING; // Fall through
case AT_WAITING:
// TODO do this unconditionally on disarm,
// no matter what mode we're in.
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
// Save the settings locally.
UAVObjSave(SystemIdentHandle(), 0);
state = AT_INIT;
}
break;
default:
// Set an alarm or some shit like that
break;
}
// Update based on manual controls
UpdateStabilizationDesired(doing_ident);
if (can_sleep) {
PIOS_Thread_Sleep(YIELD_MS);
}
}
}
