static void applyMulticopterPositionController(uint32_t currentTime)
{
static uint32_t previousTimePositionUpdate; // Occurs @ GPS update rate
static uint32_t previousTimeUpdate; // Occurs @ looptime rate
uint32_t deltaMicros = currentTime - previousTimeUpdate;
previousTimeUpdate = currentTime;
bool bypassPositionController;
// We should passthrough rcCommand is adjusting position in GPS_ATTI mode
bypassPositionController = (posControl.navConfig->flags.user_control_mode == NAV_GPS_ATTI) && posControl.flags.isAdjustingPosition;
// If last call to controller was too long in the past - ignore it (likely restarting position controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
previousTimeUpdate = currentTime;
previousTimePositionUpdate = currentTime;
resetMulticopterPositionController();
return;
}
// Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
// and pilots input would be passed thru to PID controller
if (posControl.flags.hasValidPositionSensor) {
// If we have new position - update velocity and acceleration controllers
if (posControl.flags.horizontalPositionDataNew) {
uint32_t deltaMicrosPositionUpdate = currentTime - previousTimePositionUpdate;
previousTimePositionUpdate = currentTime;
if (!bypassPositionController) {
// Update position controller
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
updatePositionVelocityController_MC();
updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX);
}
else {
resetMulticopterPositionController();
}
}
// Indicate that information is no longer usable
posControl.flags.horizontalPositionDataConsumed = 1;
}
}
else {
/* No position data, disable automatic adjustment, rcCommand passthrough */
posControl.rcAdjustment[PITCH] = 0;
posControl.rcAdjustment[ROLL] = 0;
bypassPositionController = true;
}
if (!bypassPositionController) {
rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH]);
rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL]);
}
}
void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, uint32_t currentTime)
{
int16_t pitchCorrection = 0; // >0 climb, <0 dive
int16_t rollCorrection = 0; // >0 right, <0 left
int16_t throttleCorrection = 0; // raw throttle
int16_t minThrottleCorrection = posControl.navConfig->fw_min_throttle - posControl.navConfig->fw_cruise_throttle;
int16_t maxThrottleCorrection = posControl.navConfig->fw_max_throttle - posControl.navConfig->fw_cruise_throttle;
// Mix Pitch/Roll/Throttle
if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
pitchCorrection += posControl.rcAdjustment[PITCH];
throttleCorrection += DECIDEGREES_TO_DEGREES(posControl.rcAdjustment[PITCH]) * posControl.navConfig->fw_pitch_to_throttle;
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
}
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
pitchCorrection += ABS(posControl.rcAdjustment[ROLL]) * (posControl.navConfig->fw_roll_to_pitch / 100.0f);
rollCorrection += posControl.rcAdjustment[ROLL];
}
// Speed controller - only apply in POS mode
if (navStateFlags & NAV_CTL_POS) {
throttleCorrection += applyFixedWingMinSpeedController(currentTime);
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
}
// Limit and apply
if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
// PITCH angle is measured in opposite direction ( >0 - dive, <0 - climb)
pitchCorrection = constrain(pitchCorrection, -DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_dive_angle), DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_climb_angle));
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, posControl.pidProfile->max_angle_inclination[FD_PITCH]);
}
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
rollCorrection = constrain(rollCorrection, -DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_bank_angle), DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_bank_angle));
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, posControl.pidProfile->max_angle_inclination[FD_ROLL]);
// Calculate coordinated turn rate based on velocity and banking angle
if (posControl.actualState.velXY >= 300.0f) {
float targetYawRateDPS = RADIANS_TO_DEGREES(tan_approx(DECIDEGREES_TO_RADIANS(-rollCorrection)) * GRAVITY_CMSS / posControl.actualState.velXY);
rcCommand[YAW] = pidRateToRcCommand(targetYawRateDPS, currentControlRateProfile->rates[FD_YAW]);
}
else {
rcCommand[YAW] = 0;
}
}
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
uint16_t correctedThrottleValue = constrain(posControl.navConfig->fw_cruise_throttle + throttleCorrection, posControl.navConfig->fw_min_throttle, posControl.navConfig->fw_max_throttle);
rcCommand[THROTTLE] = constrain(correctedThrottleValue, posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle);
}
}
void applyFixedWingLaunchController(timeUs_t currentTimeUs)
{
// Called at PID rate
if (launchState.launchDetected) {
bool applyLaunchIdleLogic = true;
if (launchState.motorControlAllowed) {
// If launch detected we are in launch procedure - control airplane
const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime);
// If user moves the stick - finish the launch
if ((ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband)) {
launchState.launchFinished = true;
}
// Abort launch after a pre-set time
if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_timeout) {
launchState.launchFinished = true;
}
// Motor control enabled
if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) {
// Don't apply idle logic anymore
applyLaunchIdleLogic = false;
// Increase throttle gradually over `launch_motor_spinup_time` milliseconds
if (navConfig()->fw.launch_motor_spinup_time > 0) {
const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time);
const uint16_t minIdleThrottle = MAX(motorConfig()->minthrottle, navConfig()->fw.launch_idle_throttle);
rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
0.0f, navConfig()->fw.launch_motor_spinup_time,
minIdleThrottle, navConfig()->fw.launch_throttle);
}
else {
rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
}
}
}
if (applyLaunchIdleLogic) {
// Launch idle logic - low throttle and zero out PIDs
applyFixedWingLaunchIdleLogic();
}
}
else {
// We are waiting for launch - update launch detector
updateFixedWingLaunchDetector(currentTimeUs);
// Launch idle logic - low throttle and zero out PIDs
applyFixedWingLaunchIdleLogic();
}
// Control beeper
if (!launchState.launchFinished) {
beeper(BEEPER_LAUNCH_MODE_ENABLED);
}
// Lock out controls
rcCommand[ROLL] = 0;
rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = 0;
}
