void motorsSendValues(void) {
int i;
for (i = 0; i < MOTORS_NUM; i++)
if (motorsData.active[i]) {
// ensure motor output is constrained
motorsData.value[i] = constrainInt(motorsData.value[i], 0, MOTORS_SCALE);
// PWM
if (i < PWM_NUM_PORTS && motorsData.pwm[i]) {
if (supervisorData.state & STATE_ARMED)
*motorsData.pwm[i]->ccr = constrainInt((float)motorsData.value[i] * (p[MOT_MAX] - p[MOT_MIN]) / MOTORS_SCALE + p[MOT_MIN], p[MOT_START], p[MOT_MAX]);
else
*motorsData.pwm[i]->ccr = 0;
}
// CAN
else if (motorsData.can[i]) {
motorsCheckCanStatus(i);
if (supervisorData.state & STATE_ARMED)
// convert to 16 bit
*motorsData.canPtrs[i] = constrainInt(motorsData.value[i], MOTORS_SCALE * 0.1f, MOTORS_SCALE)<<4;
else
*motorsData.canPtrs[i] = 0;
}
}
motorsCanSendGroups();
}
void motorsSendValues(void) {
int i;
for (int j = 0; j < motorsData.numActive; ++j) {
i = motorsData.activeList[j];
// ensure motor output is constrained
motorsData.value[i] = constrainInt(motorsData.value[i], 0, MOTORS_SCALE);
// PWM
if (i < PWM_NUM_PORTS && motorsData.pwm[i]) {
if (supervisorData.state & STATE_ARMED) {
#ifdef HAS_ONBOARD_ESC
if (MOTORS_ESC_TYPE == ESC_TYPE_ONBOARD_PWM)
*motorsData.pwm[i]->ccr = constrainInt((float)motorsData.value[i] * (ONBOARD_ESC_PWM_MAX - ONBOARD_ESC_PWM_MIN) / MOTORS_SCALE + ONBOARD_ESC_PWM_MIN, ONBOARD_ESC_PWM_START, ONBOARD_ESC_PWM_MAX);
else
#endif
*motorsData.pwm[i]->ccr = constrainInt((float)motorsData.value[i] * (p[MOT_MAX] - p[MOT_MIN]) / MOTORS_SCALE + p[MOT_MIN], p[MOT_START], p[MOT_MAX]);
} else
*motorsData.pwm[i]->ccr = 0;
}
// CAN
else if (motorsData.can[i]) {
motorsCheckCanStatus(i);
if (supervisorData.state & STATE_ARMED)
// convert to 16 bit
*motorsData.canPtrs[i] = constrainInt(motorsData.value[i], MOTORS_SCALE * 0.1f, MOTORS_SCALE)<<4;
else
*motorsData.canPtrs[i] = 0;
}
}
motorsCanSendGroups();
}
// thrust in gram-force
void motorsSendThrust(void) {
float value, v;
int i;
for (int j = 0; j < motorsData.numActive; ++j) {
i = motorsData.activeList[j];
value = motorsThrust2Value(motorsData.thrust[i]);
// using open-loop PWM ESC?
if (i < PWM_NUM_PORTS && motorsData.pwm[i] && (uint8_t)p[MOT_ESC_TYPE] != ESC_TYPE_ESC32) {
// preload the request to accelerate setpoint changes
if (motorsData.oldValues[i] != value) {
v = (value - motorsData.oldValues[i]);
// increase
if (v > 0.0f)
value += v * MOTORS_COMP_PRELOAD_PTERM;
// decrease
else
value += v * MOTORS_COMP_PRELOAD_PTERM * MOTORS_COMP_PRELOAD_NFACT;
// slowly follow setpoint
motorsData.oldValues[i] += v * MOTORS_COMP_PRELOAD_TAU;
}
// adjust for voltage factor
value *= motorsVFactor();
}
motorsData.value[i] = constrainInt(value * MOTORS_SCALE / p[MOT_VALUE_SCAL], 0, MOTORS_SCALE);
}
motorsSendValues();
}
void motorsPwmToAll(float pwmValue) {
int i;
for (int j = 0; j < motorsData.numActive; ++j) {
i = motorsData.activeList[j];
if (i < PWM_NUM_PORTS && motorsData.pwm[i])
*motorsData.pwm[i]->ccr = constrainInt(pwmValue, p[MOT_MIN], p[MOT_MAX]);
}
}
int ppmDataAvailable(radioInstance_t *r) {
int i;
if (ppmData.frameParsed) {
ppmData.frameParsed = 0;
for (i = 0; i < ppmData.numberChannels; ++i) {
if (&RADIO_THROT == &r->channels[i]) {
r->channels[i] = constrainInt((int)((ppmData.channels[i] - p[PPM_THROT_LOW])*5 / p[PPM_SCALER]), PPM_THROT_MIN, PPM_THROT_MAX);
} else {
r->channels[i] = constrainInt((int)((ppmData.channels[i] - p[PPM_CHAN_MID])*5 / p[PPM_SCALER]), PPM_CHAN_MIN, PPM_CHAN_MAX);
}
}
return 1;
} else {
return 0;
}
}
void motorsCommands(float throtCommand, float pitchCommand, float rollCommand, float ruddCommand) {
float throttle;
float voltageFactor;
float value;
float nominalBatVolts;
int i;
// throttle limiter to prevent control saturation
throttle = constrainFloat(throtCommand - motorsData.throttleLimiter, 0.0f, MOTORS_SCALE);
// calculate voltage factor
if (p[CTRL_BATT_COMP > 0.0]) {
nominalBatVolts = MOTORS_CELL_VOLTS*analogData.batCellCount;
voltageFactor = 1.0f + (nominalBatVolts - analogData.vIn) / nominalBatVolts * p[CTRL_BATT_COMP];
}
// calculate and set each motor value
for (i = 0; i < MOTORS_NUM; i++) {
if (motorsData.active[i]) {
motorsPowerStruct_t *d = &motorsData.distribution[i];
value = 0.0f;
value += (throttle * d->throttle * 0.01f);
value += (pitchCommand * d->pitch * 0.01f);
value += (rollCommand * d->roll * 0.01f);
value += (ruddCommand * d->yaw * 0.01f);
if (p[CTRL_BATT_COMP > 0.0]) {
value *= voltageFactor;
}
// check for over throttle
if (value >= MOTORS_SCALE) {
motorsData.throttleLimiter += MOTORS_THROTTLE_LIMITER;
}
motorsData.value[i] = constrainInt(value, 0, MOTORS_SCALE);
}
}
motorsSendValues();
// decay throttle limit
motorsData.throttleLimiter = constrainFloat(motorsData.throttleLimiter - MOTORS_THROTTLE_LIMITER, 0.0f, MOTORS_SCALE/4);
motorsData.pitch = pitchCommand;
motorsData.roll = rollCommand;
motorsData.yaw = ruddCommand;
motorsData.throttle = throttle;
}
// thrust in gram-force
void motorsSendThrust(void) {
float value;
#ifdef HAS_ONBOARD_ESC
float nominalBatVolts, voltageFactor;
#endif
int i;
for (i = 0; i < MOTORS_NUM; i++) {
if (motorsData.active[i]) {
value = motorsThrust2Value(motorsData.thrust[i]);
#ifdef HAS_ONBOARD_ESC
if (motorsData.pwm[i]) {
// preload the request to accelerate setpoint changes
if (motorsData.oldValues[i] != value) {
float v = (value - motorsData.oldValues[i]);
// increase
if (v > 0.0f) {
value += v * MOTORS_COMP_PRELOAD_PTERM;
}
// decrease
else {
value += v * MOTORS_COMP_PRELOAD_PTERM * MOTORS_COMP_PRELOAD_NFACT;
}
// slowly follow setpoint
motorsData.oldValues[i] += v * MOTORS_COMP_PRELOAD_TAU;
}
// battery voltage compensation
if (p[CTRL_BATT_COMP] > 0.0) {
nominalBatVolts = MOTORS_CELL_VOLTS * analogData.batCellCount;
voltageFactor = 1.0f + (nominalBatVolts - analogData.vIn) / nominalBatVolts * p[CTRL_BATT_COMP];
value *= voltageFactor;
}
}
#endif
motorsData.value[i] = constrainInt(value * MOTORS_SCALE / p[MOT_VALUE_SCAL], 0, MOTORS_SCALE);
}
}
motorsSendValues();
}
// thrust in gram-force
void motorsSendThrust(void) {
float value;
#ifdef HAS_ONBOARD_ESC
float nominalBatVolts, voltageFactor;
#endif
int i;
for (i = 0; i < MOTORS_NUM; i++) {
if (motorsData.active[i]) {
value = motorsThrust2Value(motorsData.thrust[i]);
#ifdef HAS_ONBOARD_ESC
if (motorsData.pwm[i]) {
// preload the request to accelerate setpoint changes
if (motorsData.oldValues[i] != value) {
float v = (value - motorsData.oldValues[i]);
// increase
if (v > 0.0f)
value += v * MOTORS_COMP_PRELOAD_PTERM;
// decrease
else
value += v * MOTORS_COMP_PRELOAD_PTERM * MOTORS_COMP_PRELOAD_NFACT;
// slowly follow setpoint
motorsData.oldValues[i] += v * MOTORS_COMP_PRELOAD_TAU;
}
// battery voltage compensation
nominalBatVolts = MOTORS_CELL_VOLTS * analogData.batCellCount;
//Express voltage command as fraction of battery volts & prevent possible division by 0 during startup
if (analogData.vIn > nominalBatVolts*0.5f)
value /= analogData.vIn;
}
#else
value /= p[MOT_VALUE_SCAL];
#endif
motorsData.value[i] = constrainInt(value * MOTORS_SCALE, 0, MOTORS_SCALE);//scale output rpm or voltage from 0 to MOTORS_SCALE
}
}
motorsSendValues();
}
void motorsCommands(float throtCommand, float pitchCommand, float rollCommand, float ruddCommand) {
float throttle;
float value;
int i;
// throttle limiter to prevent control saturation
throttle = constrainFloat(throtCommand - motorsData.throttleLimiter, 0.0f, MOTORS_SCALE);
// calculate and set each motor value
for (int j = 0; j < motorsData.numActive; ++j) {
i = motorsData.activeList[j];
motorsPowerStruct_t *d = &motorsData.distribution[i];
value = 0.0f;
value += (throttle * d->throttle * 0.01f);
value += (pitchCommand * d->pitch * 0.01f);
value += (rollCommand * d->roll * 0.01f);
value += (ruddCommand * d->yaw * 0.01f);
// adjust for voltage factor
value *= motorsVFactor();
// check for over throttle
if (value >= MOTORS_SCALE)
motorsData.throttleLimiter += MOTORS_THROTTLE_LIMITER;
motorsData.value[i] = constrainInt(value, 0, MOTORS_SCALE);
}
motorsSendValues();
// decay throttle limit
motorsData.throttleLimiter = constrainFloat(motorsData.throttleLimiter - MOTORS_THROTTLE_LIMITER, 0.0f, MOTORS_SCALE/4);
motorsData.pitch = pitchCommand;
motorsData.roll = rollCommand;
motorsData.yaw = ruddCommand;
motorsData.throttle = throttle;
}
void supervisorTaskCode(void *unused) {
unsigned long lastAqCounter = 0; // used for idle time calc
uint32_t count = 0;
AQ_NOTICE("Supervisor task started\n");
// wait for ADC vIn data
while (analogData.batCellCount == 0)
yield(100);
supervisorData.vInLPF = analogData.vIn;
if (analogData.extAmp > 0.0f)
supervisorData.currentSenseValPtr = &analogData.extAmp;
else if (canSensorsData.values[CAN_SENSORS_PDB_BATA] > 0.0f)
supervisorData.currentSenseValPtr = &canSensorsData.values[CAN_SENSORS_PDB_BATA];
else
supervisorData.aOutLPF = SUPERVISOR_INVALID_AMPSOUT_VALUE;
if (supervisorData.currentSenseValPtr)
supervisorData.aOutLPF = *supervisorData.currentSenseValPtr;
while (1) {
yield(1000/SUPERVISOR_RATE);
if (supervisorData.state & STATE_CALIBRATION) {
int i;
// try to indicate completion percentage
i = constrainInt(20*((calibData.percentComplete)/(100.0f/3.0f)), 1, 21);
if (i > 20)
digitalHi(supervisorData.readyLed);
else if (!(count % i))
digitalTogg(supervisorData.readyLed);
#ifdef SUPERVISOR_DEBUG_PORT
i = constrainInt(20*((calibData.percentComplete-100.0f/3.0f)/(100.0f/3.0f)), 1, 21);
if (i > 20)
digitalHi(supervisorData.debugLed);
else if (!(count % i))
digitalTogg(supervisorData.debugLed);
#endif //SUPERVISOR_DEBUG_PORT
i = constrainInt(20*((calibData.percentComplete-100.0f/3.0f*2.0f)/(100.0f/3.0f)), 1, 21);
if (i > 20)
digitalHi(supervisorData.gpsLed);
else if (!(count % i))
digitalTogg(supervisorData.gpsLed);
// user looking to go back to DISARMED mode?
if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500) {
if (!supervisorData.armTime) {
supervisorData.armTime = timerMicros();
}
else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
supervisorDisarm();
supervisorData.armTime = 0;
}
}
else {
supervisorData.armTime = 0;
}
} // end if calibrating
else if (supervisorData.state & STATE_DISARMED) {
#ifdef SUPERVISOR_DEBUG_PORT
// 0.5 Hz blink debug LED if config file could be found on uSD card
if (supervisorData.configRead && (!(count % SUPERVISOR_RATE))) {
// only for first 15s
if (timerMicros() > 15000000) {
supervisorData.configRead = 0;
digitalLo(supervisorData.debugLed);
}
else {
digitalTogg(supervisorData.debugLed);
}
}
#endif // SUPERVISOR_DEBUG_PORT
// 1 Hz blink if disarmed, 5 Hz if writing to uSD card
if (!(count % ((supervisorData.diskWait) ? SUPERVISOR_RATE/10 : SUPERVISOR_RATE/2)))
digitalTogg(supervisorData.readyLed);
// Attempt to arm if throttle down and yaw full right for 2sec.
if (RADIO_VALID && RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD > +500) {
if (!supervisorData.armTime) {
supervisorData.armTime = timerMicros();
}
else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
supervisorArm();
supervisorData.armTime = 0;
}
}
else {
supervisorData.armTime = 0;
}
// various functions
if (RADIO_VALID && RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500) {
if (!supervisorData.stickCmdTimer) {
supervisorData.stickCmdTimer = timerMicros();
} else if ((timerMicros() - supervisorData.stickCmdTimer) > SUPERVISOR_STICK_CMD_TIME) {
#ifdef HAS_DIGITAL_IMU
// tare function (lower left)
if (RADIO_ROLL < -500 && RADIO_PITCH > +500) {
supervisorTare();
supervisorData.stickCmdTimer = 0;
}
else
#endif // HAS_DIGITAL_IMU
// config write (upper right)
if (RADIO_ROLL > +500 && RADIO_PITCH < -500) {
supervisorLEDsOn();
configSaveParamsToFlash();
#ifdef DIMU_HAVE_EEPROM
dIMURequestCalibWrite();
#endif
supervisorLEDsOff();
supervisorData.stickCmdTimer = 0;
}
// calibration mode (upper left)
else if (RADIO_ROLL < -500 && RADIO_PITCH < -500) {
supervisorCalibrate();
supervisorData.stickCmdTimer = 0;
}
// clear waypoints (lower right with WP Record switch active)
else if (RADIO_ROLL > 500 && RADIO_PITCH > 500 && rcIsSwitchActive(NAV_CTRL_WP_REC)) {
navClearWaypoints();
supervisorData.stickCmdTimer = 0;
}
} // end stick timer check
}
// no stick commands detected
else
supervisorData.stickCmdTimer = 0;
} // end if disarmed
else if (supervisorData.state & STATE_ARMED) {
// Disarm only if in manual mode
if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500 && navData.mode == NAV_STATUS_MANUAL) {
if (!supervisorData.armTime) {
supervisorData.armTime = timerMicros();
}
else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
supervisorDisarm();
supervisorData.armTime = 0;
}
}
else {
supervisorData.armTime = 0;
}
}
// end of calibrating/disarmed/armed mode checks
// radio loss
if ((supervisorData.state & STATE_FLYING) && (navData.mode < NAV_STATUS_MISSION || (supervisorData.state & STATE_RADIO_LOSS2))) {
// regained?
if (RADIO_QUALITY > 1.0f) {
supervisorData.lastGoodRadioMicros = timerMicros();
if (supervisorData.state & STATE_RADIO_LOSS1)
AQ_NOTICE("Warning: radio signal regained\n");
navData.spvrModeOverride = 0;
supervisorData.state &= ~(STATE_RADIO_LOSS1 | STATE_RADIO_LOSS2);
}
// loss 1
else if (!(supervisorData.state & STATE_RADIO_LOSS1) && (timerMicros() - supervisorData.lastGoodRadioMicros) > SUPERVISOR_RADIO_LOSS1) {
supervisorData.state |= STATE_RADIO_LOSS1;
AQ_NOTICE("Warning: Radio loss stage 1 detected\n");
// hold position
navData.spvrModeOverride = NAV_STATUS_POSHOLD;
RADIO_PITCH = 0; // center sticks
RADIO_ROLL = 0;
RADIO_RUDD = 0;
RADIO_THROT = RADIO_MID_THROTTLE; // center throttle
}
// loss 2
else if (!(supervisorData.state & STATE_RADIO_LOSS2) && (timerMicros() - supervisorData.lastGoodRadioMicros) > SUPERVISOR_RADIO_LOSS2) {
supervisorData.state |= STATE_RADIO_LOSS2;
AQ_NOTICE("Warning: Radio loss stage 2! Initiating recovery.\n");
// only available with GPS
if (navData.navCapable) {
navMission_t *wp;
uint8_t wpi = 0;
uint8_t fsOption = (uint8_t)p[SPVR_FS_RAD_ST2];
navClearWaypoints();
wp = navGetWaypoint(wpi++);
if (fsOption > SPVR_OPT_FS_RAD_ST2_LAND && navCalcDistance(gpsData.lat, gpsData.lon, navData.homeLeg.targetLat, navData.homeLeg.targetLon) > SUPERVISOR_HOME_POS_DETECT_RADIUS) {
float targetAltitude;
// ascend
if (fsOption == SPVR_OPT_FS_RAD_ST2_ASCEND && ALTITUDE < navData.homeLeg.targetAlt + p[SPVR_FS_ADD_ALT]) {
// the home leg's altitude is recorded without pressure offset
targetAltitude = navData.homeLeg.targetAlt + p[SPVR_FS_ADD_ALT] + navData.presAltOffset;
wp->type = NAV_LEG_GOTO;
wp->relativeAlt = 0;
wp->targetAlt = targetAltitude;
wp->targetLat = gpsData.lat;
wp->targetLon = gpsData.lon;
wp->targetRadius = SUPERVISOR_HOME_ALT_DETECT_MARGIN;
wp->maxHorizSpeed = navData.homeLeg.maxHorizSpeed;
wp->maxVertSpeed = navData.homeLeg.maxVertSpeed;
wp->poiHeading = navData.homeLeg.poiHeading;
wp->loiterTime = 0;
wp->poiAltitude = 0.0f;
wp = navGetWaypoint(wpi++);
}
else {
// the greater of our current altitude or home's altitude
targetAltitude = ((ALTITUDE > navData.homeLeg.targetAlt) ? ALTITUDE : navData.homeLeg.targetAlt) + navData.presAltOffset;
}
// go home with previously determined altitude
wp->type = NAV_LEG_GOTO;
wp->relativeAlt = 0;
wp->targetAlt = targetAltitude;
wp->targetLat = navData.homeLeg.targetLat;
wp->targetLon = navData.homeLeg.targetLon;
wp->targetRadius = SUPERVISOR_HOME_POS_DETECT_RADIUS;
wp->maxHorizSpeed = navData.homeLeg.maxHorizSpeed;
wp->maxVertSpeed = navData.homeLeg.maxVertSpeed;
wp->poiHeading = navData.homeLeg.poiHeading;
wp->loiterTime = 0;
wp->poiAltitude = 0.0f;
wp = navGetWaypoint(wpi++);
// decend to home
wp->type = NAV_LEG_HOME;
wp->targetRadius = SUPERVISOR_HOME_POS_DETECT_RADIUS;
wp->loiterTime = 0;
wp->poiAltitude = 0.0f;
wp = navGetWaypoint(wpi++);
}
// land
wp->type = NAV_LEG_LAND;
wp->maxVertSpeed = NAV_DFLT_LND_SPEED;
wp->maxHorizSpeed = 0.0f;
wp->poiAltitude = 0.0f;
// go
navData.missionLeg = 0;
navData.tempMissionLoaded = 1;
navData.spvrModeOverride = NAV_STATUS_MISSION;
}
// no GPS, slow decent in PH mode
else {
navData.spvrModeOverride = NAV_STATUS_POSHOLD;
RADIO_PITCH = 0; // center sticks
RADIO_ROLL = 0;
RADIO_RUDD = 0;
RADIO_THROT = RADIO_MID_THROTTLE * 3 / 4; // 1/4 max decent
}
}
}
// end radio loss check
// calculate idle time
supervisorData.idlePercent = (counter - lastAqCounter) * minCycles * 100.0f / ((1e6f / SUPERVISOR_RATE) * rccClocks.SYSCLK_Frequency / 1e6f);
lastAqCounter = counter;
// smooth vIn readings
supervisorData.vInLPF += (analogData.vIn - supervisorData.vInLPF) * (0.1f / SUPERVISOR_RATE);
// smooth current flow readings, if any
if (supervisorData.currentSenseValPtr)
supervisorData.aOutLPF += (*supervisorData.currentSenseValPtr - supervisorData.aOutLPF) * (0.1f / SUPERVISOR_RATE);
//calculate remaining battery % based on configured low batt stg 2 level -- ASSumes 4.2v/cell maximum
supervisorData.battRemainingPrct = (supervisorData.vInLPF - p[SPVR_LOW_BAT2] * analogData.batCellCount) / ((4.2f - p[SPVR_LOW_BAT2]) * analogData.batCellCount) * 100;
// low battery
if (!(supervisorData.state & STATE_LOW_BATTERY1) && supervisorData.vInLPF < (p[SPVR_LOW_BAT1]*analogData.batCellCount)) {
supervisorData.state |= STATE_LOW_BATTERY1;
AQ_NOTICE("Warning: Low battery stage 1\n");
// TODO: something
}
else if (!(supervisorData.state & STATE_LOW_BATTERY2) && supervisorData.vInLPF < (p[SPVR_LOW_BAT2]*analogData.batCellCount)) {
supervisorData.state |= STATE_LOW_BATTERY2;
AQ_NOTICE("Warning: Low battery stage 2\n");
// TODO: something
}
// end battery level checks
supervisorSetSystemStatus();
if (supervisorData.state & STATE_FLYING) {
// count flight time in seconds
supervisorData.flightTime += (1.0f / SUPERVISOR_RATE);
// rapidly flash Ready LED if we are critically low on power
if (supervisorData.state & STATE_LOW_BATTERY2)
digitalTogg(supervisorData.readyLed);
}
else if (supervisorData.state & STATE_ARMED) {
digitalHi(supervisorData.readyLed);
}
#ifdef SUPERVISOR_DEBUG_PORT
// DEBUG LED to indicate radio RX state
if (!supervisorData.configRead && RADIO_INITIALIZED && supervisorData.state != STATE_CALIBRATION) {
// packet received in the last 50ms?
if (RADIO_VALID) {
digitalHi(supervisorData.debugLed);
}
else {
if (RADIO_BINDING)
digitalTogg(supervisorData.debugLed);
else
digitalLo(supervisorData.debugLed);
}
}
#endif
count++;
#ifdef USE_SIGNALING
signalingEvent();
#endif
}
}
void supervisorTaskCode(void *unused) {
uint32_t count = 0;
AQ_NOTICE("Supervisor task started\n");
// wait for ADC vIn data
while (analogData.batCellCount == 0)
yield(100);
supervisorCreateSOCTable();
supervisorData.vInLPF = analogData.vIn;
supervisorData.soc = 100.0f;
while (1) {
yield(1000/SUPERVISOR_RATE);
if (supervisorData.state & STATE_CALIBRATION) {
int i;
// try to indicate completion percentage
i = constrainInt(20*((calibData.percentComplete)/(100.0f/3.0f)), 1, 21);
if (i > 20)
digitalHi(supervisorData.readyLed);
else if (!(count % i))
digitalTogg(supervisorData.readyLed);
#ifdef SUPERVISOR_DEBUG_PORT
i = constrainInt(20*((calibData.percentComplete-100.0f/3.0f)/(100.0f/3.0f)), 1, 21);
if (i > 20)
digitalHi(supervisorData.debugLed);
else if (!(count % i))
digitalTogg(supervisorData.debugLed);
#endif
i = constrainInt(20*((calibData.percentComplete-100.0f/3.0f*2.0f)/(100.0f/3.0f)), 1, 21);
if (i > 20)
digitalHi(supervisorData.gpsLed);
else if (!(count % i))
digitalTogg(supervisorData.gpsLed);
// user looking to go back to DISARMED mode?
if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500) {
if (!supervisorData.armTime) {
supervisorData.armTime = timerMicros();
}
else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
supervisorDisarm();
supervisorData.armTime = 0;
}
}
else {
supervisorData.armTime = 0;
}
}
else if (supervisorData.state & STATE_DISARMED) {
#ifdef SUPERVISOR_DEBUG_PORT
// 0.5 Hz blink debug LED if config file could be found on uSD card
if (supervisorData.configRead && (!(count % SUPERVISOR_RATE))) {
// only for first 15s
if (timerMicros() > 15000000) {
supervisorData.configRead = 0;
digitalLo(supervisorData.debugLed);
}
else {
digitalTogg(supervisorData.debugLed);
}
}
#endif
// 1 Hz blink if disarmed, 5 Hz if writing to uSD card
if (!(count % ((supervisorData.diskWait) ? SUPERVISOR_RATE/10 : SUPERVISOR_RATE/2)))
digitalTogg(supervisorData.readyLed);
// Arm if all the switches are in default (startup positions) - flaps down, aux2 centered
if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD > +500 && RADIO_FLAPS < -250 && !navData.homeActionFlag && navData.headFreeMode == NAV_HEADFREE_OFF) {
if (!supervisorData.armTime) {
supervisorData.armTime = timerMicros();
}
else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
supervisorArm();
supervisorData.armTime = 0;
}
}
else {
supervisorData.armTime = 0;
}
// various functions
if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500) {
#ifdef HAS_DIGITAL_IMU
// tare function (lower left)
if (RADIO_ROLL < -500 && RADIO_PITCH > +500) {
supervisorTare();
}
#endif
// config write (upper right)
if (RADIO_VALID && RADIO_ROLL > +500 && RADIO_PITCH < -500) {
supervisorLEDsOn();
configFlashWrite();
#ifdef DIMU_HAVE_EEPROM
dIMURequestCalibWrite();
#endif
supervisorLEDsOff();
}
// calibration mode (upper left)
if (RADIO_ROLL < -500 && RADIO_PITCH < -500) {
supervisorCalibrate();
}
}
}
else if (supervisorData.state & STATE_ARMED) {
// Disarm only if in manual mode
if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500 && navData.mode == NAV_STATUS_MANUAL) {
if (!supervisorData.armTime) {
supervisorData.armTime = timerMicros();
}
else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
supervisorDisarm();
supervisorData.armTime = 0;
}
}
else {
supervisorData.armTime = 0;
}
}
// radio loss
if ((supervisorData.state & STATE_FLYING) && (navData.mode < NAV_STATUS_MISSION || (supervisorData.state & STATE_RADIO_LOSS2))) {
// regained?
if (RADIO_QUALITY > 1.0f) {
supervisorData.lastGoodRadioMicros = timerMicros();
if (supervisorData.state & STATE_RADIO_LOSS1)
AQ_NOTICE("Warning: radio signal regained\n");
supervisorData.state &= ~(STATE_RADIO_LOSS1 | STATE_RADIO_LOSS2);
}
// loss 1
else if (!(supervisorData.state & STATE_RADIO_LOSS1) && (timerMicros() - supervisorData.lastGoodRadioMicros) > SUPERVISOR_RADIO_LOSS1) {
supervisorData.state |= STATE_RADIO_LOSS1;
AQ_NOTICE("Warning: Radio loss stage 1 detected\n");
// hold position
RADIO_FLAPS = 0; // position hold
RADIO_AUX2 = 0; // normal home mode
RADIO_PITCH = 0; // center sticks
RADIO_ROLL = 0;
RADIO_RUDD = 0;
RADIO_THROT = RADIO_MID_THROTTLE; // center throttle
}
// loss 2
else if (!(supervisorData.state & STATE_RADIO_LOSS2) && (timerMicros() - supervisorData.lastGoodRadioMicros) > SUPERVISOR_RADIO_LOSS2) {
supervisorData.state |= STATE_RADIO_LOSS2;
AQ_NOTICE("Warning: Radio loss stage 2! Initiating recovery mission.\n");
// only available with GPS
if (navData.navCapable) {
navMission_t *wp;
uint8_t wpi = 0;
uint8_t fsOption = (uint8_t)p[SPVR_FS_RAD_ST2];
navClearWaypoints();
wp = navGetWaypoint(wpi++);
if (fsOption > SPVR_OPT_FS_RAD_ST2_LAND && navCalcDistance(gpsData.lat, gpsData.lon, navData.homeLeg.targetLat, navData.homeLeg.targetLon) > SUPERVISOR_HOME_POS_DETECT_RADIUS) {
float targetAltitude;
// ascend
if (fsOption == SPVR_OPT_FS_RAD_ST2_ASCEND && ALTITUDE < navData.homeLeg.targetAlt + p[SPVR_FS_ADD_ALT]) {
// the home leg's altitude is recorded without pressure offset
targetAltitude = navData.homeLeg.targetAlt + p[SPVR_FS_ADD_ALT] + navData.presAltOffset;
wp->type = NAV_LEG_GOTO;
wp->relativeAlt = 0;
wp->targetAlt = targetAltitude;
wp->targetLat = gpsData.lat;
wp->targetLon = gpsData.lon;
wp->targetRadius = SUPERVISOR_HOME_ALT_DETECT_MARGIN;
wp->maxHorizSpeed = navData.homeLeg.maxHorizSpeed;
wp->maxVertSpeed = navData.homeLeg.maxVertSpeed;
wp->poiHeading = navData.homeLeg.poiHeading;
wp->loiterTime = 0;
wp->poiAltitude = 0.0f;
wp = navGetWaypoint(wpi++);
}
else {
// the greater of our current altitude or home's altitude
targetAltitude = ((ALTITUDE > navData.homeLeg.targetAlt) ? ALTITUDE : navData.homeLeg.targetAlt) + navData.presAltOffset;
}
// go home with previously determined altitude
wp->type = NAV_LEG_GOTO;
wp->relativeAlt = 0;
wp->targetAlt = targetAltitude;
wp->targetLat = navData.homeLeg.targetLat;
wp->targetLon = navData.homeLeg.targetLon;
wp->targetRadius = SUPERVISOR_HOME_POS_DETECT_RADIUS;
wp->maxHorizSpeed = navData.homeLeg.maxHorizSpeed;
wp->maxVertSpeed = navData.homeLeg.maxVertSpeed;
wp->poiHeading = navData.homeLeg.poiHeading;
wp->loiterTime = 0;
wp->poiAltitude = 0.0f;
wp = navGetWaypoint(wpi++);
// decend to home
wp->type = NAV_LEG_HOME;
wp->targetRadius = SUPERVISOR_HOME_POS_DETECT_RADIUS;
wp->loiterTime = 0;
wp->poiAltitude = 0.0f;
wp = navGetWaypoint(wpi++);
}
// land
wp->type = NAV_LEG_LAND;
wp->maxVertSpeed = p[NAV_LANDING_VEL];
wp->maxHorizSpeed = 0.0f;
wp->poiAltitude = 0.0f;
// go
navData.missionLeg = 0;
RADIO_FLAPS = 500; // mission mode
}
// no GPS, slow decent in PH mode
else {
RADIO_FLAPS = 0; // position hold
RADIO_AUX2 = 0; // normal home mode
RADIO_PITCH = 0; // center sticks
RADIO_ROLL = 0;
RADIO_RUDD = 0;
RADIO_THROT = RADIO_MID_THROTTLE * 3 / 4; // 1/4 max decent
}
}
}
// smooth vIn readings
supervisorData.vInLPF += (analogData.vIn - supervisorData.vInLPF) * (0.1f / SUPERVISOR_RATE);
// determine battery state of charge
supervisorData.soc = supervisorSOCTableLookup(supervisorData.vInLPF);
// low battery
if (!(supervisorData.state & STATE_LOW_BATTERY1) && supervisorData.vInLPF < (p[SPVR_LOW_BAT1]*analogData.batCellCount)) {
supervisorData.state |= STATE_LOW_BATTERY1;
AQ_NOTICE("Warning: Low battery stage 1\n");
// TODO: something
}
else if (!(supervisorData.state & STATE_LOW_BATTERY2) && supervisorData.vInLPF < (p[SPVR_LOW_BAT2]*analogData.batCellCount)) {
supervisorData.state |= STATE_LOW_BATTERY2;
AQ_NOTICE("Warning: Low battery stage 2\n");
// TODO: something
}
if (supervisorData.state & STATE_FLYING) {
// count flight time in seconds
supervisorData.flightTime += (1.0f / SUPERVISOR_RATE);
// calculate remaining flight time
if (supervisorData.soc < 99.0f) {
supervisorData.flightSecondsAvg += (supervisorData.flightTime / (100.0f - supervisorData.soc) - supervisorData.flightSecondsAvg) * (0.01f / SUPERVISOR_RATE);
supervisorData.flightTimeRemaining = supervisorData.flightSecondsAvg * supervisorData.soc;
}
else {
supervisorData.flightSecondsAvg = supervisorData.flightTime;
supervisorData.flightTimeRemaining = 999.9f * 60.0f; // unknown
}
// rapidly flash Ready LED if we are critically low on power
if (supervisorData.state & STATE_LOW_BATTERY2)
digitalTogg(supervisorData.readyLed);
}
else if (supervisorData.state & STATE_ARMED) {
digitalHi(supervisorData.readyLed);
}
#ifdef SUPERVISOR_DEBUG_PORT
// DEBUG LED to indicate radio RX state
if (!supervisorData.configRead && RADIO_INITIALIZED && supervisorData.state != STATE_CALIBRATION) {
// packet received in the last 50ms?
if (RADIO_VALID) {
digitalHi(supervisorData.debugLed);
}
else {
if (RADIO_BINDING)
digitalTogg(supervisorData.debugLed);
else
digitalLo(supervisorData.debugLed);
}
}
#endif
count++;
#ifdef USE_SIGNALING
signalingEvent();
#endif
}
}
void gimbalUpdate(void) {
uint16_t pwm;
float tilt, pitch;
if (timerMicros() < 5e6f)
return;
// calculate manual/PoI tilt angle override
if (p[GMBL_TILT_PORT]) {
tilt = RADIO_AUX3 + navData.poiAngle * GMBL_DEGREES_TO_PW * p[GMBL_SCAL_PITCH];
// smooth
if (tilt != gimbalData.tilt)
gimbalData.tilt -= (gimbalData.tilt - tilt) * p[GMBL_SLEW_RATE];
}
// stabilized pitch axis output
if (gimbalData.pitchPort) {
pitch = -AQ_PITCH * GMBL_DEGREES_TO_PW * p[GMBL_SCAL_PITCH];
// stabilization AND manual/PoI override output to pitchPort
if (p[GMBL_TILT_PORT] == p[GMBL_PITCH_PORT])
pitch += gimbalData.tilt;
pwm = constrainInt(pitch + p[GMBL_NTRL_PITCH], p[GMBL_PWM_MIN_PT], p[GMBL_PWM_MAX_PT]);
*gimbalData.pitchPort->ccr = pwm;
}
// unstabilized cameara tilt (pitch) output with manual and PoI override
if (gimbalData.tiltPort) {
pwm = constrainInt(gimbalData.tilt + p[GMBL_NTRL_PITCH], p[GMBL_PWM_MIN_PT], p[GMBL_PWM_MAX_PT]);
*gimbalData.tiltPort->ccr = pwm;
}
// stabilized roll axis output
if (gimbalData.rollPort) {
if (p[GMBL_ROLL_EXPO])
pwm = constrainInt((-AQ_ROLL * (fabsf(AQ_ROLL) / (100 / p[GMBL_ROLL_EXPO]))) * GMBL_DEGREES_TO_PW * p[GMBL_SCAL_ROLL] + p[GMBL_NTRL_ROLL], p[GMBL_PWM_MIN_RL], p[GMBL_PWM_MAX_RL]);
else
pwm = constrainInt(-AQ_ROLL * GMBL_DEGREES_TO_PW * p[GMBL_SCAL_ROLL] + p[GMBL_NTRL_ROLL], p[GMBL_PWM_MIN_RL], p[GMBL_PWM_MAX_RL]);
*gimbalData.rollPort->ccr = pwm;
}
// trigger output, manual/passthrough or automatic modes
if (gimbalData.triggerPort) {
pwm = RADIO_GEAR + GMBL_TRIG_NTRL_PWM;
// manual trigger active
if (RADIO_GEAR > p[GMBL_TRIG_CH_NEU] + p[CTRL_DEAD_BAND] || RADIO_GEAR < p[GMBL_TRIG_CH_NEU] - p[CTRL_DEAD_BAND]) {
gimbalData.trigger = 0; // cancel automatic trigger, if any
gimbalData.triggerTimer = 0;
if (!gimbalData.triggerLogVal) // first activation after channel had returned to neutral
gimbalData.triggerLogVal = ++gimbalData.triggerCount;
}
// automated trigger detection if trigger channel at neutral position
else {
// if trigger is not already active
if (!gimbalData.triggerTimer) {
gimbalData.triggerLogVal = 0; // trigger should be off at this point (auto or manual)
if (!gimbalData.trigger && (supervisorData.state & STATE_FLYING)) {
if (p[GMBL_TRIG_TIME] && timerMicros() - gimbalData.triggerLastTime >= p[GMBL_TRIG_TIME] * 1e6) {
gimbalData.trigger = 1;
gimbalData.triggerLastTime = timerMicros();
AQ_NOTICE("Time trigger activated.\n");
}
else if (p[GMBL_TRIG_DIST] && (gimbalData.triggerLastLat != gpsData.lat || gimbalData.triggerLastLon != gpsData.lon) &&
navCalcDistance(gimbalData.triggerLastLat, gimbalData.triggerLastLon, gpsData.lat, gpsData.lon) >= p[GMBL_TRIG_DIST]) {
gimbalData.trigger = 1;
gimbalData.triggerLastLat = gpsData.lat;
gimbalData.triggerLastLon = gpsData.lon;
AQ_NOTICE("Distance trigger activated.\n");
}
}
if (gimbalData.trigger) {
gimbalData.triggerTimer = timerMicros();
gimbalData.triggerLogVal = ++gimbalData.triggerCount;
gimbalData.trigger = 0;
}
}
// if trigger is active, keep correct pwm output until timeout
if (gimbalData.triggerTimer) {
if (timerMicros() - gimbalData.triggerTimer > p[GMBL_TRIG_ON_TIM] * 1e3)
gimbalData.triggerTimer = 0;
else
pwm = p[GMBL_TRIG_ON_PWM];
}
} // end auto trigger
pwm = constrainInt(pwm, GMBL_TRIG_MIN_PWM, GMBL_TRIG_MAX_PWM);
*gimbalData.triggerPort->ccr = pwm;
}
// simple passthrough output of any channel
if (gimbalData.passthroughPort) {
pwm = constrainInt(radioData.channels[(int)(p[GMBL_PSTHR_CHAN]-1)] + GMBL_TRIG_NTRL_PWM, GMBL_TRIG_MIN_PWM, GMBL_TRIG_MAX_PWM);
*gimbalData.passthroughPort->ccr = pwm;
}
}
void controlTaskCode(void *unused) {
float yaw;
float throttle;
float ratesDesired[3];
uint16_t overrides[3];
#ifdef USE_QUATOS
float quatDesired[4];
float ratesActual[3];
#else
float pitch, roll;
float pitchCommand, rollCommand, ruddCommand;
#endif // USE_QUATOS
AQ_NOTICE("Control task started\n");
// disable all axes' rate overrides
overrides[0] = 0;
overrides[1] = 0;
overrides[2] = 0;
while (1) {
// wait for work
CoWaitForSingleFlag(imuData.dRateFlag, 0);
// this needs to be done ASAP with the freshest of data
if (supervisorData.state & STATE_ARMED) {
if (RADIO_THROT > p[CTRL_MIN_THROT] || navData.mode > NAV_STATUS_MANUAL) {
supervisorThrottleUp(1);
// are we in altitude hold mode?
if (navData.mode > NAV_STATUS_MANUAL) {
// override throttle with nav's request
throttle = pidUpdate(navData.altSpeedPID, navData.holdSpeedAlt, -VELOCITYD) * MOTORS_SCALE / RADIO_MID_THROTTLE;
// don't allow negative throttle to be built up
if (navData.altSpeedPID->iState < 0.0f) {
navData.altSpeedPID->iState = 0.0f;
}
} else {
throttle = ((uint32_t)RADIO_THROT - p[CTRL_MIN_THROT]) * MOTORS_SCALE / RADIO_MID_THROTTLE * p[CTRL_FACT_THRO];
}
// limit
throttle = constrainInt(throttle, 1, MOTORS_SCALE);
// if motors are not yet running, use this heading as hold heading
if (motorsData.throttle == 0) {
navData.holdHeading = AQ_YAW;
controlData.yaw = navData.holdHeading;
// Reset all PIDs
pidZeroIntegral(controlData.pitchRatePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.rollRatePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.yawRatePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.pitchAnglePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.rollAnglePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.yawAnglePID, 0.0f, 0.0f);
// also set this position as hold position
if (navData.mode == NAV_STATUS_POSHOLD) {
navUkfSetHereAsPositionTarget();
}
}
// constrict nav (only) yaw rates
yaw = compassDifference(controlData.yaw, navData.holdHeading);
yaw = constrainFloat(yaw, -p[CTRL_NAV_YAW_RT]/400.0f, +p[CTRL_NAV_YAW_RT]/400.0f);
controlData.yaw = compassNormalize(controlData.yaw + yaw);
// DVH overrides direct user pitch / roll requests
if (navData.mode != NAV_STATUS_DVH) {
controlData.userPitchTarget = RADIO_PITCH * p[CTRL_FACT_PITC];
controlData.userRollTarget = RADIO_ROLL * p[CTRL_FACT_ROLL];
} else {
controlData.userPitchTarget = 0.0f;
controlData.userRollTarget = 0.0f;
}
// navigation requests
if (navData.mode > NAV_STATUS_ALTHOLD) {
controlData.navPitchTarget = navData.holdTiltN;
controlData.navRollTarget = navData.holdTiltE;
} else {
controlData.navPitchTarget = 0.0f;
controlData.navRollTarget = 0.0f;
}
// manual rate cut through for yaw
if (RADIO_RUDD > p[CTRL_DEAD_BAND] || RADIO_RUDD < -p[CTRL_DEAD_BAND]) {
// fisrt remove dead band
if (RADIO_RUDD > p[CTRL_DEAD_BAND]) {
ratesDesired[2] = (RADIO_RUDD - p[CTRL_DEAD_BAND]);
} else {
ratesDesired[2] = (RADIO_RUDD + p[CTRL_DEAD_BAND]);
}
// calculate desired rate based on full stick scale
ratesDesired[2] = ratesDesired[2] * p[CTRL_MAN_YAW_RT] * DEG_TO_RAD * (1.0f / 700.0f);
// keep up with actual craft heading
controlData.yaw = AQ_YAW;
navData.holdHeading = AQ_YAW;
// request override
overrides[2] = CONTROL_MIN_YAW_OVERRIDE;
} else {
// currently overriding?
if (overrides[2] > 0) {
// request zero rate
ratesDesired[2] = 0.0f;
// follow actual craft heading
controlData.yaw = AQ_YAW;
navData.holdHeading = AQ_YAW;
// decrease override timer
overrides[2]--;
}
}
#ifdef USE_QUATOS
// determine which frame of reference to control from
if (navData.mode <= NAV_STATUS_ALTHOLD)
// craft frame - manual
{
eulerToQuatYPR(quatDesired, controlData.yaw, controlData.userPitchTarget, controlData.userRollTarget);
} else
// world frame - autonomous
{
eulerToQuatRPY(quatDesired, controlData.navRollTarget, controlData.navPitchTarget, controlData.yaw);
}
// reset controller on startup
if (motorsData.throttle == 0) {
quatDesired[0] = UKF_Q1;
quatDesired[1] = UKF_Q2;
quatDesired[2] = UKF_Q3;
quatDesired[3] = UKF_Q4;
quatosReset(quatDesired);
}
ratesActual[0] = IMU_DRATEX + UKF_GYO_BIAS_X;
ratesActual[1] = IMU_DRATEY + UKF_GYO_BIAS_Y;
ratesActual[2] = IMU_DRATEZ + UKF_GYO_BIAS_Z;
quatos(&UKF_Q1, quatDesired, ratesActual, ratesDesired, overrides);
quatosPowerDistribution(throttle);
motorsSendThrust();
motorsData.throttle = throttle;
#else
// smooth
controlData.userPitchTarget = utilFilter3(controlData.userPitchFilter, controlData.userPitchTarget);
controlData.userRollTarget = utilFilter3(controlData.userRollFilter, controlData.userRollTarget);
// smooth
controlData.navPitchTarget = utilFilter3(controlData.navPitchFilter, controlData.navPitchTarget);
controlData.navRollTarget = utilFilter3(controlData.navRollFilter, controlData.navRollTarget);
// rotate nav's NE frame of reference to our craft's local frame of reference
pitch = controlData.navPitchTarget * navUkfData.yawCos - controlData.navRollTarget * navUkfData.yawSin;
roll = controlData.navRollTarget * navUkfData.yawCos + controlData.navPitchTarget * navUkfData.yawSin;
// combine nav & user requests (both are already smoothed)
controlData.pitch = pitch + controlData.userPitchTarget;
controlData.roll = roll + controlData.userRollTarget;
if (p[CTRL_PID_TYPE] == 0) {
// pitch angle
pitchCommand = pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH);
// rate
pitchCommand += pidUpdate(controlData.pitchRatePID, 0.0f, IMU_DRATEY);
// roll angle
rollCommand = pidUpdate(controlData.rollAnglePID, controlData.roll, AQ_ROLL);
// rate
rollCommand += pidUpdate(controlData.rollRatePID, 0.0f, IMU_DRATEX);
} else if (p[CTRL_PID_TYPE] == 1) {
// pitch rate from angle
pitchCommand = pidUpdate(controlData.pitchRatePID, pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH), IMU_DRATEY);
// roll rate from angle
rollCommand = pidUpdate(controlData.rollRatePID, pidUpdate(controlData.rollAnglePID, controlData.roll, AQ_ROLL), IMU_DRATEX);
}
else if (p[CTRL_PID_TYPE] == 2) {
// pitch angle
pitchCommand = pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH);
// rate
pitchCommand += pidUpdate(controlData.pitchRatePID, 0.0f, IMU_DRATEY);
int axis = 0; // ROLL
float PID_P = 3.7;
float PID_I = 0.031;
float PID_D = 23.0;
float error, errorAngle, AngleRateTmp, RateError, delta, deltaSum;
float PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
static int16_t lastGyro[3] = { 0, 0, 0 };
static float delta1[3], delta2[3];
static float errorGyroI[3] = { 0, 0, 0 }, errorAngleI[2] = { 0, 0 };
static float lastError[3] = { 0, 0, 0 }, lastDTerm[3] = { 0, 0, 0 }; // pt1 element http://www.multiwii.com/forum/viewtopic.php?f=23&t=2624;
static int16_t axisPID[3];
static float rollPitchRate = 0.0;
static float newpidimax = 0.0;
float dT;
uint8_t ANGLE_MODE = 0;
uint8_t HORIZON_MODE = 0;
uint16_t cycleTime = IMU_LASTUPD -
controlData.lastUpdate; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
if ((ANGLE_MODE || HORIZON_MODE)) { // MODE relying on ACC
errorAngle = constrainFloat(2.0f * (float)controlData.roll, -500.0f, +500.0f) - AQ_ROLL;
}
if (!ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = (float)(rollPitchRate + 27) * (float)controlData.roll *
0.0625f; // AngleRateTmp = ((int32_t) (cfg.rollPitchRate + 27) * rcCommand[axis]) >> 4;
if (HORIZON_MODE) {
AngleRateTmp += PID_I * errorAngle *
0.0390625f; //increased by x10 //0.00390625f AngleRateTmp += (errorAngle * (float)cfg.I8[PIDLEVEL]) >> 8;
}
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = PID_P * errorAngle * 0.0223214286f; // AngleRateTmp = (errorAngle * (float)cfg.P8[PIDLEVEL]) >> 4; * LevelPprescale;
}
RateError = AngleRateTmp - IMU_DRATEX;
PTerm = PID_P * RateError * 0.0078125f;
errorGyroI[axis] += PID_I * RateError * (float)cycleTime / 2048.0f;
errorGyroI[axis] = constrainFloat(errorGyroI[axis], -newpidimax, newpidimax);
ITerm = errorGyroI[axis] / 8192.0f;
delta = RateError - lastError[axis];
lastError[axis] = RateError;
delta = delta * 16383.75f / (float)cycleTime;
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = PID_D * deltaSum * 0.00390625f;
axisPID[axis] = PTerm + ITerm + DTerm;
rollCommand = AngleRateTmp;
}
else {
pitchCommand = 0.0f;
rollCommand = 0.0f;
ruddCommand = 0.0f;
}
// yaw rate override?
if (overrides[2] > 0)
// manual yaw rate
{
ruddCommand = pidUpdate(controlData.yawRatePID, ratesDesired[2], IMU_DRATEZ);
} else
// seek a 0 deg difference between hold heading and actual yaw
{
ruddCommand = pidUpdate(controlData.yawRatePID, pidUpdate(controlData.yawAnglePID, 0.0f, compassDifference(controlData.yaw, AQ_YAW)),
IMU_DRATEZ);
}
rollCommand = constrainFloat(rollCommand, -p[CTRL_MAX], p[CTRL_MAX]);
pitchCommand = constrainFloat(pitchCommand, -p[CTRL_MAX], p[CTRL_MAX]);
ruddCommand = constrainFloat(ruddCommand, -p[CTRL_MAX], p[CTRL_MAX]);
motorsCommands(throttle, pitchCommand, rollCommand, ruddCommand);
#endif
}
// no throttle input
else {
supervisorThrottleUp(0);
motorsOff();
}
}
// not armed
else {
motorsOff();
}
controlData.lastUpdate = IMU_LASTUPD;
controlData.loops++;
}
}