void I2C_doneReadMagData(void) { magFieldRaw[0] = (magreg[0]<<8) + magreg[1]; magFieldRaw[1] = (magreg[2]<<8) + magreg[3]; magFieldRaw[2] = (magreg[4]<<8) + magreg[5]; if (magMessage == 7) { udb_magFieldBody[0] = MAG_X_SIGN((__builtin_mulsu((magFieldRaw[MAG_X_AXIS]), magGain[MAG_X_AXIS]))>>14)-(udb_magOffset[0]>>1); udb_magFieldBody[1] = MAG_Y_SIGN((__builtin_mulsu((magFieldRaw[MAG_Y_AXIS]), magGain[MAG_Y_AXIS]))>>14)-(udb_magOffset[1]>>1); udb_magFieldBody[2] = MAG_Z_SIGN((__builtin_mulsu((magFieldRaw[MAG_Z_AXIS]), magGain[MAG_Z_AXIS]))>>14)-(udb_magOffset[2]>>1); if ((abs(udb_magFieldBody[0]) < MAGNETICMAXIMUM) && (abs(udb_magFieldBody[1]) < MAGNETICMAXIMUM) && (abs(udb_magFieldBody[2]) < MAGNETICMAXIMUM)) { if (magnetometer_callback != NULL) { magnetometer_callback(); } else { printf("ERROR: magnetometer_callback function pointer not set\r\n"); } } else { magMessage = 0; // invalid reading, reset the magnetometer } }
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 }
void I2C_doneReadMagData(void) { magFieldRaw[0] = (magreg[0]<<8)+magreg[1] ; magFieldRaw[1] = (magreg[2]<<8)+magreg[3] ; magFieldRaw[2] = (magreg[4]<<8)+magreg[5] ; if ( magMessage == 7 ) { udb_magFieldBody[0] = MAG_X_SIGN((__builtin_mulsu((magFieldRaw[MAG_X_AXIS]), magGain[MAG_X_AXIS] ))>>14)-(udb_magOffset[0]>>1) ; udb_magFieldBody[1] = MAG_Y_SIGN((__builtin_mulsu((magFieldRaw[MAG_Y_AXIS]), magGain[MAG_Y_AXIS] ))>>14)-(udb_magOffset[1]>>1) ; udb_magFieldBody[2] = MAG_Z_SIGN((__builtin_mulsu((magFieldRaw[MAG_Z_AXIS]), magGain[MAG_Z_AXIS] ))>>14)-(udb_magOffset[2]>>1) ; if ( ( abs(udb_magFieldBody[0]) < MAGNETICMAXIMUM ) && ( abs(udb_magFieldBody[1]) < MAGNETICMAXIMUM ) && ( abs(udb_magFieldBody[2]) < MAGNETICMAXIMUM ) ) { udb_magnetometer_callback_data_available(); } else { magMessage = 0 ; // invalid reading, reset the magnetometer } }
// Calculate the required airspeed in cm/s. desiredSpeed is in dm/s int16_t calc_target_airspeed(int16_t desiredSpd) { union longww accum ; int16_t target; accum.WW = __builtin_mulsu ( desiredSpd , 10 ) ; target = accum._.W0 ; if(groundspeed < minimum_groundspeed) target += (minimum_groundspeed - groundspeed); if(target > maximum_airspeed) target = maximum_airspeed; if(target < minimum_airspeed) target = minimum_airspeed; return target; }
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 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 }
void helicalTurnCntrl(void) { union longww accum; int16_t pitchAdjustAngleOfAttack; int16_t rollErrorVector[3]; int16_t rtlkick; int16_t desiredPitch; int16_t steeringInput; int16_t desiredTiltVector[3]; int16_t desiredRotationRateGyro[3]; uint16_t airSpeed; union longww desiredTilt; int16_t desiredPitchVector[2]; int16_t desiredPerpendicularPitchVector[2]; int16_t actualPitchVector[2]; int16_t pitchDot; int16_t pitchCross; int16_t pitchError; int16_t pitchEarthBodyProjection[2]; int16_t angleOfAttack; #ifdef TestGains state_flags._.GPS_steering = 0; // turn off navigation state_flags._.pitch_feedback = 1; // turn on stabilization airSpeed = 981; // for testing purposes, an airspeed is needed #else airSpeed = air_speed_3DIMU; if (airSpeed < TURN_CALC_MINIMUM_AIRSPEED) airSpeed = TURN_CALC_MINIMUM_AIRSPEED; #endif // determine the desired turn rate as the sum of navigation and fly by wire. // this allows the pilot to override navigation if needed. steeringInput = 0 ; // just in case no airframe type is specified or radio is off if (udb_flags._.radio_on == 1) { #if ( (AIRFRAME_TYPE == AIRFRAME_STANDARD) || (AIRFRAME_TYPE == AIRFRAME_GLIDER) ) if (AILERON_INPUT_CHANNEL != CHANNEL_UNUSED) // compiler is smart about this { steeringInput = udb_pwIn[ AILERON_INPUT_CHANNEL ] - udb_pwTrim[ AILERON_INPUT_CHANNEL ]; steeringInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, steeringInput); } else if (RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED) { steeringInput = udb_pwIn[ RUDDER_INPUT_CHANNEL ] - udb_pwTrim[ RUDDER_INPUT_CHANNEL ]; steeringInput = REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, steeringInput); } else { steeringInput = 0; } #endif // AIRFRAME_STANDARD #if (AIRFRAME_TYPE == AIRFRAME_VTAIL) // use aileron channel if it is available, otherwise use rudder if (AILERON_INPUT_CHANNEL != CHANNEL_UNUSED) // compiler is smart about this { steeringInput = udb_pwIn[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]; steeringInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, steeringInput); } else if (RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED) { // unmix the Vtail int16_t rudderInput = REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, (udb_pwIn[ RUDDER_INPUT_CHANNEL] - udb_pwTrim[RUDDER_INPUT_CHANNEL])); int16_t elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, (udb_pwIn[ ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL])); steeringInput = (-rudderInput + elevatorInput); } else { steeringInput = 0; } #endif // AIRFRAME_VTAIL #if (AIRFRAME_TYPE == AIRFRAME_DELTA) // delta wing must have an aileron input, so use that // unmix the elevons int16_t aileronInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, (udb_pwIn[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL])); int16_t elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, (udb_pwIn[ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL])); steeringInput = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED, ((elevatorInput - aileronInput))); #endif // AIRFRAME_DELTA } if (steeringInput > MAX_INPUT) steeringInput = MAX_INPUT; if (steeringInput < - MAX_INPUT) steeringInput = - MAX_INPUT; // note that total steering is the sum of pilot input and waypoint navigation, // so that the pilot always has some say in the matter accum.WW = __builtin_mulsu(steeringInput, turngainfbw) /(2*MAX_INPUT); if ((settings._.AileronNavigation || settings._.RudderNavigation) && state_flags._.GPS_steering) { accum.WW +=(int32_t) navigate_determine_deflection('t'); } if (accum.WW >(int32_t) 2*(int32_t) RMAX - 1) accum.WW =(int32_t) 2*(int32_t) RMAX - 1; if (accum.WW < -(int32_t) 2*(int32_t) RMAX + 1) accum.WW = -(int32_t) 2*(int32_t) RMAX + 1; desiredTurnRateRadians = accum._.W0; // compute the desired tilt from desired turn rate and air speed // range for acceleration is plus minus 4 times gravity // range for turning rate is plus minus 4 radians per second // desiredTilt is the ratio(-rmat[6]/rmat[8]), times RMAX/2 required for the turn // desiredTilt = desiredTurnRate * airSpeed / gravity // desiredTilt = RMAX/2*"real desired tilt" // desiredTurnRate = RMAX/2*"real desired turn rate", desired turn rate in radians per second // airSpeed is air speed centimeters per second // gravity is 981 centimeters per second per second desiredTilt.WW = - __builtin_mulsu(desiredTurnRateRadians, airSpeed); desiredTilt.WW /= GRAVITYCMSECSEC; // limit the lateral acceleration to +- 4 times gravity, total wing loading approximately 4.12 times gravity if (desiredTilt.WW > (int32_t)2 * (int32_t)RMAX - 1) { desiredTilt.WW = (int32_t)2 * (int32_t)RMAX - 1; accum.WW = __builtin_mulsu(-desiredTilt._.W0, GRAVITYCMSECSEC); accum.WW /= airSpeed; desiredTurnRateRadians = accum._.W0; } else if (desiredTilt.WW < -(int32_t)2 * (int32_t)RMAX + 1) { desiredTilt.WW = -(int32_t)2 * (int32_t)RMAX + 1; accum.WW = __builtin_mulsu(-desiredTilt._.W0, GRAVITYCMSECSEC); accum.WW /= airSpeed; desiredTurnRateRadians = accum._.W0; } // Compute the amount of lift needed to perform the desired turn // Tests show that the best estimate of lift is obtained using // actual values of rmat[6] and rmat[8], and the commanded value of their ratio estimatedLift = wingLift(rmat[6], rmat[8], desiredTilt._.W0); // compute angle of attack and elevator trim based on relative wing loading. // relative wing loading is the ratio of wing loading divided by the stall wing loading, as a function of air speed // both angle of attack and trim are computed by a linear approximation as a function of relative loading: // y = (2m)*(x/2) + b, y is either angle of attack or elevator trim. // x is relative wing loading. (x/2 is computed instead of x) // 2m and b are determined from values of angle of attack and trim at stall speed, normal and inverted. // b = (y_normal + y_inverted) / 2. // 2m = (y_normal - y_inverted). // If airspeed is greater than stall speed, compute angle of attack and elevator trim, // otherwise set AoA and trim to zero. if (air_speed_3DIMU > STALL_SPEED_CM_SEC) { // compute "x/2", the relative wing loading relativeLoading = relativeWingLoading(estimatedLift, air_speed_3DIMU); // multiply x/2 by 2m for angle of attack accum.WW = __builtin_mulss(AOA_SLOPE, relativeLoading); // add mx to b angleOfAttack = AOA_OFFSET + accum._.W1; // project angle of attack into the earth frame accum.WW =(__builtin_mulss(angleOfAttack, rmat[8])) << 2; pitchAdjustAngleOfAttack = accum._.W1; // similarly, compute elevator trim accum.WW = __builtin_mulss(ELEVATOR_TRIM_SLOPE, relativeLoading); elevatorLoadingTrim = ELEVATOR_TRIM_OFFSET + accum._.W1; } else { angleOfAttack = 0; pitchAdjustAngleOfAttack = 0; elevatorLoadingTrim = 0; } // SetAofA(angleOfAttack); // removed by helicalTurns // convert desired turn rate from radians/second to gyro units accum.WW = (((int32_t)desiredTurnRateRadians) << 4); // desired turn rate in radians times 16 to provide resolution for the divide to follow accum.WW = accum.WW / RADSTOGYRO; // at this point accum._.W0 has 2 times the required gyro signal for the turn. // compute desired rotation rate vector in body frame, scaling is same as gyro signal VectorScale(3, desiredRotationRateGyro, &rmat[6], accum._.W0); // this operation has side effect of dividing by 2 // compute desired rotation rate vector in body frame, scaling is in RMAX/2*radians/sec VectorScale(3, desiredRotationRateRadians, &rmat[6], desiredTurnRateRadians); // this produces half of what we want VectorAdd(3, desiredRotationRateRadians, desiredRotationRateRadians, desiredRotationRateRadians); // double // incorporate roll into desired tilt vector desiredTiltVector[0] = desiredTilt._.W0; desiredTiltVector[1] = 0; desiredTiltVector[2] = RMAX/2; // the divide by 2 is to account for the RMAX/2 scaling in both tilt and rotation rate vector3_normalize(desiredTiltVector, desiredTiltVector); // make sure tilt vector has magnitude RMAX // incorporate pitch into desired tilt vector // compute return to launch pitch down kick for unpowered RTL if (!udb_flags._.radio_on && state_flags._.GPS_steering) { rtlkick = RTLKICK; } else { rtlkick = 0; } // Compute Matt's glider pitch adjustment #if (GLIDE_AIRSPEED_CONTROL == 1) fractional aspd_pitch_adj = gliding_airspeed_pitch_adjust(); #endif // Compute total desired pitch #if (GLIDE_AIRSPEED_CONTROL == 1) desiredPitch = - rtlkick + aspd_pitch_adj + pitchAltitudeAdjust; #else desiredPitch = - rtlkick + pitchAltitudeAdjust; #endif // Adjustment for inverted flight if (!canStabilizeInverted() || !desired_behavior._.inverted) { // normal flight desiredTiltVector[1] = - desiredPitch - pitchAdjustAngleOfAttack; } else { // inverted flight desiredTiltVector[0] = - desiredTiltVector[0]; desiredTiltVector[1] = - desiredPitch - pitchAdjustAngleOfAttack - INVNPITCH; // only one of the adjustments is not zero desiredTiltVector[2] = - desiredTiltVector[2]; } vector3_normalize(desiredTiltVector, desiredTiltVector); // make sure tilt vector has magnitude RMAX // compute roll error VectorCross(rollErrorVector, &rmat[6], desiredTiltVector); // compute tilt orientation error if (VectorDotProduct(3, &rmat[6], desiredTiltVector) < 0) // more than 90 degree error { vector3_normalize(rollErrorVector, rollErrorVector); // for more than 90 degrees, make the tilt error vector parallel to desired axis, with magnitude RMAX } tiltError[1] = rollErrorVector[1]; // compute pitch error // start by computing the projection of earth frame pitch error to body frame pitchEarthBodyProjection[0] = rmat[6]; pitchEarthBodyProjection[1] = rmat[8]; // normalize the projection vector and compute the cosine of the actual pitch as a side effect actualPitchVector[1] =(int16_t) vector2_normalize(pitchEarthBodyProjection, pitchEarthBodyProjection); // complete the actual pitch vector actualPitchVector[0] = rmat[7]; // compute the desired pitch vector desiredPitchVector[0] = - desiredPitch; desiredPitchVector[1] = RMAX; vector2_normalize(desiredPitchVector, desiredPitchVector); // rotate desired pitch vector by 90 degrees to be able to compute cross product using VectorDot desiredPerpendicularPitchVector[0] = desiredPitchVector[1]; desiredPerpendicularPitchVector[1] = - desiredPitchVector[0]; // compute pitchDot, the dot product of actual and desired pitch vector // (the 2* that appears in several of the following expressions is a result of the Q2.14 format) pitchDot = 2*VectorDotProduct(2, actualPitchVector, desiredPitchVector); // compute pitchCross, the cross product of the actual and desired pitch vector pitchCross = 2*VectorDotProduct(2, actualPitchVector, desiredPerpendicularPitchVector); if (pitchDot > 0) { pitchError = pitchCross; } else { if (pitchCross > 0) { pitchError = RMAX; } else { pitchError = - RMAX; } } // multiply the normalized rmat[6], rmat[8] vector by the pitch error VectorScale(2, pitchEarthBodyProjection, pitchEarthBodyProjection, pitchError); tiltError[0] = 2*pitchEarthBodyProjection[1]; tiltError[2] = - 2*pitchEarthBodyProjection[0]; // compute the rotation rate error vector VectorSubtract(3, rotationRateError, omegaAccum, desiredRotationRateGyro); }