void hoverPitchCntrl(void)
{
union longww pitchAccum;
if (flags._.pitch_feedback)
{
pitchAccum.WW = (__builtin_mulss(-rmat[7] , omegagyro[0])
- __builtin_mulss(rmat[6] , omegagyro[1])) << 1;
pitchrate = pitchAccum._.W1;
int16_t elevInput = (udb_flags._.radio_on == 1) ?
REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, udb_pwIn[ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]) : 0;
int16_t manualPitchOffset = elevInput * (int16_t)(RMAX/600);
int32_t pitchToWP;
if (flags._.GPS_steering)
{
pitchToWP = (tofinish_line > HOVER_NAV_MAX_PITCH_RADIUS) ?
HOVERPTOWP : (HOVERPTOWP / HOVER_NAV_MAX_PITCH_RADIUS * tofinish_line);
}
else
{
pitchToWP = 0;
}
pitchAccum.WW = __builtin_mulsu(rmat[8] + HOVERPOFFSET - pitchToWP + manualPitchOffset , hoverpitchgain)
+ __builtin_mulus(hoverpitchkd , pitchrate);
}
else
{
pitchAccum.WW = 0;
}
pitch_control = (int32_t)pitchAccum._.W1;
}
void normalRollCntrl(void)
{
union longww rollAccum = { 0 };
union longww gyroRollFeedback;
union longww gyroYawFeedback;
fractional omegaAccum2;
if (!canStabilizeInverted() || !desired_behavior._.inverted)
{
omegaAccum2 = omegaAccum[2];
}
else
{
omegaAccum2 = -omegaAccum[2];
}
#ifdef TestGains
state_flags._.pitch_feedback = 1;
#endif
if (settings._.RollStabilizaionAilerons && state_flags._.pitch_feedback)
{
gyroRollFeedback.WW = - __builtin_mulus(rollkd, rotationRateError[1]);
rollAccum.WW -= __builtin_mulsu(tiltError[1], rollkp);
rollAccum.WW += __builtin_mulsu(desiredRotationRateRadians[1], rollkpfdfwd);
}
else
{
gyroRollFeedback.WW = 0;
}
if (settings._.YawStabilizationAileron && state_flags._.pitch_feedback)
{
gyroYawFeedback.WW = - __builtin_mulus(yawkdail, omegaAccum2);
}
else
{
gyroYawFeedback.WW = 0;
}
roll_control = (int32_t)rollAccum._.W1 + (int32_t)gyroRollFeedback._.W1 + (int32_t)gyroYawFeedback._.W1;
// Servo reversing is handled in servoMix.c
}
static int16_t relativeWingLoading(int16_t wingLoad, uint16_t airSpeed)
{
// wingLoad is(2**16)*((wing_load / mass*gravity) / 16)
// stallSpeed is the stall speed in centimeters per second
// airSpeed is the air speed in centimeters per second
uint16_t stallSpeed = STALL_SPEED_CM_SEC;
int16_t result = 0;
uint32_t long_unsigned_accum;
union longww long_signed_accum;
uint16_t unsigned_accum;
// if airspeed is less than or equal to stall speed, return zero
if (airSpeed <= stallSpeed)
{
return 0;
}
long_unsigned_accum = (uint32_t)stallSpeed;
long_unsigned_accum = long_unsigned_accum << 16; //(2**16)*V0, 32 bits unsigned
unsigned_accum = __builtin_divud(long_unsigned_accum, airSpeed); //(2**16)*(V0/V), 16 bits unsigned
long_unsigned_accum = __builtin_muluu(unsigned_accum, unsigned_accum); //(2**32)*(V0/V)**2, 32 bits unsigned
unsigned_accum = long_unsigned_accum >> 16; //(2**16)*(V0/V)**2, 16 bits unsigned
long_signed_accum.WW = __builtin_mulus(unsigned_accum, wingLoad); //(2**32)*(a/16g)*(V0/V)**2, 32 bits unsigned
if (abs(long_signed_accum._.W1) < 4095)
{
long_signed_accum.WW = long_signed_accum.WW << 3; // multiply by 8
result = long_signed_accum._.W1;
}
else
{
if (wingLoad > 0)
{
result = 32767;
}
else
{
result = -32767;
}
}
return result;
}
void hoverYawCntrl(void)
{
union longww yawAccum;
union longww gyroYawFeedback;
if (flags._.pitch_feedback)
{
gyroYawFeedback.WW = __builtin_mulus(hoveryawkd, omegaAccum[2]);
int16_t yawInput = (udb_flags._.radio_on == 1) ? REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, udb_pwIn[RUDDER_INPUT_CHANNEL] - udb_pwTrim[RUDDER_INPUT_CHANNEL]) : 0;
int16_t manualYawOffset = yawInput * (int16_t)(RMAX/2000);
yawAccum.WW = __builtin_mulsu(rmat[6] + HOVERYOFFSET + manualYawOffset, hoveryawkp);
}
else
{
gyroYawFeedback.WW = 0;
yawAccum.WW = 0;
}
yaw_control = (int32_t)yawAccum._.W1 - (int32_t)gyroYawFeedback._.W1;
}
void hoverRollCntrl(void)
{
int16_t rollNavDeflection;
union longww gyroRollFeedback;
if (state_flags._.pitch_feedback)
{
if (settings._.AileronNavigation && state_flags._.GPS_steering)
{
rollNavDeflection = (tofinish_line > hover.HoverNavMaxPitchRadius/2) ? navigate_determine_deflection('h') : 0;
}
else
{
rollNavDeflection = 0;
}
gyroRollFeedback.WW = __builtin_mulus(hoverrollkd , omegaAccum[1]);
}
else
{
rollNavDeflection = 0;
gyroRollFeedback.WW = 0;
}
roll_control = rollNavDeflection -(int32_t)gyroRollFeedback._.W1;
}
void dead_reckon(void)
{
int16_t air_speed_x, air_speed_y, air_speed_z;
union longww accum;
union longww energy;
if (dcm_flags._.dead_reckon_enable == 1) // wait for startup of GPS
{
// integrate the accelerometers to update IMU velocity
IMUintegralAccelerationx.WW += __builtin_mulss(((int16_t)(ACCEL2DELTAV)), accelEarth[0]);
IMUintegralAccelerationy.WW += __builtin_mulss(((int16_t)(ACCEL2DELTAV)), accelEarth[1]);
IMUintegralAccelerationz.WW += __builtin_mulss(((int16_t)(ACCEL2DELTAV)), accelEarth[2]);
// integrate IMU velocity to update the IMU location
IMUlocationx.WW += (__builtin_mulss(((int16_t)(VELOCITY2LOCATION)), IMUintegralAccelerationx._.W1)>>4);
IMUlocationy.WW += (__builtin_mulss(((int16_t)(VELOCITY2LOCATION)), IMUintegralAccelerationy._.W1)>>4);
IMUlocationz.WW += (__builtin_mulss(((int16_t)(VELOCITY2LOCATION)), IMUintegralAccelerationz._.W1)>>4);
if (dead_reckon_clock > 0)
// apply drift adjustments only while valid GPS data is in force.
// This is done with a countdown clock that gets reset each time new data comes in.
{
dead_reckon_clock --;
IMUintegralAccelerationx.WW += __builtin_mulss(DR_FILTER_GAIN, velocityErrorEarth[0]);
IMUintegralAccelerationy.WW += __builtin_mulss(DR_FILTER_GAIN, velocityErrorEarth[1]);
IMUintegralAccelerationz.WW += __builtin_mulss(DR_FILTER_GAIN, velocityErrorEarth[2]);
IMUlocationx.WW += __builtin_mulss(DR_FILTER_GAIN, locationErrorEarth[0]);
IMUlocationy.WW += __builtin_mulss(DR_FILTER_GAIN, locationErrorEarth[1]);
IMUlocationz.WW += __builtin_mulss(DR_FILTER_GAIN, locationErrorEarth[2]);
IMUvelocityx.WW = IMUintegralAccelerationx.WW +
__builtin_mulus(ONE_OVER_TAU, 100*locationErrorEarth[0]);
IMUvelocityy.WW = IMUintegralAccelerationy.WW +
__builtin_mulus(ONE_OVER_TAU, 100*locationErrorEarth[1]);
IMUvelocityz.WW = IMUintegralAccelerationz.WW +
__builtin_mulus(ONE_OVER_TAU, 100*locationErrorEarth[2]);
}
else // GPS has gotten disconnected
{
yaw_drift_reset();
dcm_flags._.gps_history_valid = 0; // restart GPS history variables
IMUvelocityx.WW = IMUintegralAccelerationx.WW;
IMUvelocityy.WW = IMUintegralAccelerationy.WW;
IMUvelocityz.WW = IMUintegralAccelerationz.WW;
}
if (gps_nav_valid() && (dcm_flags._.reckon_req == 1))
{
// compute error indications and restart the dead reckoning clock to apply them
dcm_flags._.reckon_req = 0;
dead_reckon_clock = DR_PERIOD;
locationErrorEarth[0] = GPSlocation.x - IMUlocationx._.W1;
locationErrorEarth[1] = GPSlocation.y - IMUlocationy._.W1;
locationErrorEarth[2] = GPSlocation.z - IMUlocationz._.W1;
velocityErrorEarth[0] = GPSvelocity.x - IMUintegralAccelerationx._.W1;
velocityErrorEarth[1] = GPSvelocity.y - IMUintegralAccelerationy._.W1;
velocityErrorEarth[2] = GPSvelocity.z - IMUintegralAccelerationz._.W1;
}
}
void normalPitchCntrl(void)
{
union longww pitchAccum;
int16_t rtlkick;
// int16_t aspd_adj;
// fractional aspd_err, aspd_diff;
fractional rmat6;
fractional rmat7;
fractional rmat8;
if (!canStabilizeInverted() || current_orientation != F_INVERTED)
{
rmat6 = rmat[6];
rmat7 = rmat[7];
rmat8 = rmat[8];
}
else
{
rmat6 = -rmat[6];
rmat7 = -rmat[7];
rmat8 = -rmat[8];
pitchAltitudeAdjust = -pitchAltitudeAdjust - INVNPITCH;
}
navElevMix = 0;
if (flags._.pitch_feedback)
{
if (RUDDER_OUTPUT_CHANNEL != CHANNEL_UNUSED && RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED) {
pitchAccum.WW = __builtin_mulsu(rmat6 , rudderElevMixGain) << 1;
pitchAccum.WW = __builtin_mulss(pitchAccum._.W1 ,
REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, udb_pwTrim[RUDDER_INPUT_CHANNEL] - udb_pwOut[RUDDER_OUTPUT_CHANNEL])) << 3;
navElevMix += pitchAccum._.W1;
}
pitchAccum.WW = __builtin_mulsu(rmat6 , rollElevMixGain) << 1;
pitchAccum.WW = __builtin_mulss(pitchAccum._.W1 , rmat[6]) >> 3;
navElevMix += pitchAccum._.W1;
}
pitchAccum.WW = (__builtin_mulss(rmat8 , omegagyro[0])
- __builtin_mulss(rmat6 , omegagyro[2])) << 1;
pitchrate = pitchAccum._.W1;
if (!udb_flags._.radio_on && flags._.GPS_steering)
{
rtlkick = RTLKICK;
}
else
{
rtlkick = 0;
}
#if(GLIDE_AIRSPEED_CONTROL == 1)
fractional aspd_pitch_adj = gliding_airspeed_pitch_adjust();
#endif
if (PITCH_STABILIZATION && flags._.pitch_feedback)
{
#if(GLIDE_AIRSPEED_CONTROL == 1)
pitchAccum.WW = __builtin_mulsu(rmat7 - rtlkick + aspd_pitch_adj + pitchAltitudeAdjust, pitchgain)
+ __builtin_mulus(pitchkd , pitchrate);
#else
pitchAccum.WW = __builtin_mulsu(rmat7 - rtlkick + pitchAltitudeAdjust, pitchgain)
+ __builtin_mulus(pitchkd , pitchrate);
#endif
}
else
{
pitchAccum.WW = 0;
}
pitch_control = (int32_t)pitchAccum._.W1 + navElevMix;
}
void normalYawCntrl(void)
{
int16_t yawNavDeflection;
union longww rollStabilization;
union longww gyroYawFeedback;
int16_t ail_rud_mix;
#ifdef TestGains
flags._.GPS_steering = 0; // turn off navigation
flags._.pitch_feedback = 1; // turn on stabilization
#endif
if (RUDDER_NAVIGATION && flags._.GPS_steering)
{
yawNavDeflection = determine_navigation_deflection('y');
if (canStabilizeInverted() && current_orientation == F_INVERTED)
{
yawNavDeflection = -yawNavDeflection;
}
}
else
{
yawNavDeflection = 0;
}
if (YAW_STABILIZATION_RUDDER && flags._.pitch_feedback)
{
gyroYawFeedback.WW = __builtin_mulus(yawkdrud, omegaAccum[2]);
}
else
{
gyroYawFeedback.WW = 0;
}
rollStabilization.WW = 0; // default case is no roll rudder stabilization
if (ROLL_STABILIZATION_RUDDER && flags._.pitch_feedback)
{
if (!desired_behavior._.inverted && !desired_behavior._.hover) // normal
{
rollStabilization.WW = __builtin_mulsu(rmat[6], rollkprud);
}
else if (desired_behavior._.inverted) // inverted
{
rollStabilization.WW = - __builtin_mulsu(rmat[6], rollkprud);
}
rollStabilization.WW -= __builtin_mulus(rollkdrud, omegaAccum[1]);
}
if (flags._.pitch_feedback)
{
int16_t ail_offset = (udb_flags._.radio_on) ? (udb_pwIn[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]) : 0;
ail_rud_mix = MANUAL_AILERON_RUDDER_MIX * REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, ail_offset);
if (canStabilizeInverted() && current_orientation == F_INVERTED) ail_rud_mix = -ail_rud_mix;
}
else
{
ail_rud_mix = 0;
}
yaw_control = (int32_t)yawNavDeflection
- (int32_t)gyroYawFeedback._.W1
+ (int32_t)rollStabilization._.W1
+ ail_rud_mix;
// Servo reversing is handled in servoMix.c
}
