[gentlenav] r3647 committed - branch: MatrixPilot_wjp_helicalTurns: completed support for delta wing...

[gent...@googlecode.com]

Revision: 3647
Author: wprem...@gmail.com
Date: Tue Jun 2 12:13:26 2015 UTC
Log: branch: MatrixPilot_wjp_helicalTurns: completed support for delta
wing and V tail airframe types
https://code.google.com/p/gentlenav/source/detail?r=3647

Modified:
/branches/MatrixPilot_wjp_helicalTurns/MatrixPilot/config_tests.c
/branches/MatrixPilot_wjp_helicalTurns/MatrixPilot/helicalTurnCntrl.c
/branches/MatrixPilot_wjp_helicalTurns/MatrixPilot/servoMix.c

=======================================
--- /branches/MatrixPilot_wjp_helicalTurns/MatrixPilot/config_tests.c Sat
May 30 19:58:18 2015 UTC
+++ /branches/MatrixPilot_wjp_helicalTurns/MatrixPilot/config_tests.c Tue
Jun 2 12:13:26 2015 UTC
@@ -243,11 +243,6 @@
#endif // ANGLE_OF_ATTACK_INVERTED
#endif // INVERTED_NEUTRAL_PITCH

-// at this time, the only airframes supported by helical turn controls are
standard and delta wing
-#if ( AIRFRAME_TYPE == AIRFRAME_VTAIL )
- #error("Helical controls does not yet support AIRFRAME_VTAIL, it will
soon."
-#endif
-
#if ( AIRFRAME_TYPE == AIRFRAME_HELI )
#error("Helical controls does not support AIRFRAME_HELI."
-#endif
+#endif // AIRFRAME_HELI
=======================================
--- /branches/MatrixPilot_wjp_helicalTurns/MatrixPilot/helicalTurnCntrl.c
Sat May 30 19:58:18 2015 UTC
+++ /branches/MatrixPilot_wjp_helicalTurns/MatrixPilot/helicalTurnCntrl.c
Tue Jun 2 12:13:26 2015 UTC
@@ -220,17 +220,15 @@
if ( airSpeed < MINIMUM_AIRSPEED ) airSpeed = 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)
- if ( AILERON_INPUT_CHANNEL != CHANNEL_UNUSED ) // compiler is smart
about this
+ // use the aileron channel if it is available, otherwise use rudder
+ if ( AILERON_INPUT_CHANNEL != CHANNEL_UNUSED )
{
steeringInput = udb_pwIn[ AILERON_INPUT_CHANNEL ] - udb_pwTrim[
AILERON_INPUT_CHANNEL ] ;
steeringInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED,
steeringInput) ;
@@ -247,9 +245,28 @@
#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)>>1 ;
+ }
+ 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)>>1)) ;
@@ -259,6 +276,9 @@
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 ((AILERON_NAVIGATION||RUDDER_NAVIGATION) && flags._.GPS_steering)
=======================================
--- /branches/MatrixPilot_wjp_helicalTurns/MatrixPilot/servoMix.c Sat May
30 19:58:18 2015 UTC
+++ /branches/MatrixPilot_wjp_helicalTurns/MatrixPilot/servoMix.c Tue Jun
2 12:13:26 2015 UTC
@@ -44,100 +44,143 @@
else
pwManual[temp] = udb_pwTrim[temp];
}
-
-

// Standard airplane airframe
// Mix roll_control into ailerons
// Mix pitch_control into elevators
// Mix yaw control and waggle into rudder
#if (AIRFRAME_TYPE == AIRFRAME_STANDARD)
-
- // Apply boosts if in a stabilized mode
- if (udb_flags._.radio_on && flags._.pitch_feedback)
- {
- pwManual[AILERON_INPUT_CHANNEL] = udb_pwTrim[AILERON_INPUT_CHANNEL] ;
// in fly by wire or navigate mode, manual input is calculated as part of
turn control
- pwManual[ELEVATOR_INPUT_CHANNEL] += ((pwManual[ELEVATOR_INPUT_CHANNEL]
- udb_pwTrim[ELEVATOR_INPUT_CHANNEL]) * elevatorbgain) >> 3;
- pwManual[RUDDER_INPUT_CHANNEL] += ((pwManual[RUDDER_INPUT_CHANNEL] -
udb_pwTrim[RUDDER_INPUT_CHANNEL]) * rudderbgain) >> 3;
- }
- temp = pwManual[AILERON_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, roll_control + waggle);
- udb_pwOut[AILERON_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ // Apply boosts to elevator and rudder if in a controlled mode
+ // It does not matter whether the radio is on or not
+ if ( flags._.pitch_feedback )
+ {
+ pwManual[AILERON_INPUT_CHANNEL] = udb_pwTrim[AILERON_INPUT_CHANNEL] ; //
in fly by wire or navigate mode, manual input is accounted for in the turn
control
+ pwManual[ELEVATOR_INPUT_CHANNEL] += ((pwManual[ELEVATOR_INPUT_CHANNEL] -
udb_pwTrim[ELEVATOR_INPUT_CHANNEL]) * elevatorbgain) >> 3;
+ pwManual[RUDDER_INPUT_CHANNEL] += ((pwManual[RUDDER_INPUT_CHANNEL] -
udb_pwTrim[RUDDER_INPUT_CHANNEL]) * rudderbgain) >> 3;
+ }
+ temp = pwManual[AILERON_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, roll_control + waggle);
+ udb_pwOut[AILERON_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

- udb_pwOut[AILERON_SECONDARY_OUTPUT_CHANNEL] = 3000 +
- REVERSE_IF_NEEDED(AILERON_SECONDARY_CHANNEL_REVERSED,
udb_pwOut[AILERON_OUTPUT_CHANNEL] - 3000);
+ udb_pwOut[AILERON_SECONDARY_OUTPUT_CHANNEL] =
udb_pwTrim[AILERON_INPUT_CHANNEL] +
+ REVERSE_IF_NEEDED(AILERON_SECONDARY_CHANNEL_REVERSED,
udb_pwOut[AILERON_OUTPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]);

- temp = pwManual[ELEVATOR_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_control);
- udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ temp = pwManual[ELEVATOR_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_control);
+ udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

- temp = pwManual[RUDDER_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, yaw_control - waggle);
- udb_pwOut[RUDDER_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ temp = pwManual[RUDDER_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, yaw_control - waggle);
+ udb_pwOut[RUDDER_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

- if (pwManual[THROTTLE_INPUT_CHANNEL] == 0)
- {
- udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = 0;
- }
- else
- {
- temp = pwManual[THROTTLE_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, throttle_control);
- udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
- }
-#endif
+ if (pwManual[THROTTLE_INPUT_CHANNEL] == 0)
+ {
+ udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = 0;
+ }
+ else
+ {
+ temp = pwManual[THROTTLE_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, throttle_control);
+ udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ }
+#endif // AIRFRAME_STANDARD

// V-Tail airplane airframe
// Mix roll_control and waggle into ailerons
// Mix pitch_control and yaw_control into both elevator and rudder
#if (AIRFRAME_TYPE == AIRFRAME_VTAIL)
- int32_t vtail_yaw_control =
REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED, yaw_control);
+ int16_t rudderInput ;
+ int16_t elevatorInput ;
+ int16_t pitchInput ;
+ int16_t yawInput ;
+ int16_t pitchCommand ;
+ int16_t yawCommand ;
+ int32_t vtail_yaw_control ;

- temp = pwManual[AILERON_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, roll_control + waggle);
- udb_pwOut[AILERON_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ // Unmix the vtail
+ rudderInput = REVERSE_IF_NEEDED( RUDDER_CHANNEL_REVERSED , ( pwManual[
RUDDER_INPUT_CHANNEL ] - udb_pwTrim[ RUDDER_INPUT_CHANNEL ])) ;
+ elevatorInput = REVERSE_IF_NEEDED( ELEVATOR_CHANNEL_REVERSED , (
pwManual[ ELEVATOR_INPUT_CHANNEL ] - udb_pwTrim[ ELEVATOR_INPUT_CHANNEL
])) ;
+ pitchInput = ((rudderInput+elevatorInput)>>1) ;
+ yawInput = ((-rudderInput+elevatorInput)>>1) ;
+
+ if (flags._.pitch_feedback)
+ {
+ // Apply boost in FBW or navigate mode
+ pitchCommand = ( ( elevatorbgain + 8 ) * pitchInput ) >> 3 ;
+ yawCommand = ( ( rudderbgain + 8 ) * yawInput ) >> 3 ;
+ // in fly by wire or navigate mode, manual input is accounted for in the
turn control
+ pwManual[AILERON_INPUT_CHANNEL] = udb_pwTrim[AILERON_INPUT_CHANNEL] ;
+ }
+ else
+ {
+ pitchCommand = pitchInput ;
+ yawCommand = yawInput ;
+ }
+
+ vtail_yaw_control = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED,
yaw_control);
+
+ // In fly by wire mode, ailerons are controlled indirectly by helical
turn control
+ temp = pwManual[AILERON_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, roll_control + waggle);
+ udb_pwOut[AILERON_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

- // Reverse the polarity of the secondary aileron if necessary
- udb_pwOut[AILERON_SECONDARY_OUTPUT_CHANNEL] = 3000 +
- REVERSE_IF_NEEDED(AILERON_SECONDARY_CHANNEL_REVERSED,
udb_pwOut[AILERON_OUTPUT_CHANNEL] - 3000);
+ // Reverse the polarity of the secondary aileron if necessary
+ udb_pwOut[AILERON_SECONDARY_OUTPUT_CHANNEL] =
udb_pwTrim[AILERON_INPUT_CHANNEL] +
+ REVERSE_IF_NEEDED(AILERON_SECONDARY_CHANNEL_REVERSED,
udb_pwOut[AILERON_OUTPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]);

- temp = pwManual[ELEVATOR_INPUT_CHANNEL] +
- REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_control +
vtail_yaw_control);
- udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ temp = udb_pwTrim[ELEVATOR_INPUT_CHANNEL] +
+ REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitchCommand +
pitch_control + yawCommand + vtail_yaw_control);
+ udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

- temp = pwManual[RUDDER_INPUT_CHANNEL] +
- REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, pitch_control -
vtail_yaw_control);
- udb_pwOut[RUDDER_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ temp = udb_pwTrim[RUDDER_INPUT_CHANNEL] +
+ REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, pitchCommand + pitch_control
- yawCommand - vtail_yaw_control);
+ udb_pwOut[RUDDER_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

- if (pwManual[THROTTLE_INPUT_CHANNEL] == 0)
- {
- udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = 0;
- }
- else
- {
- temp = pwManual[THROTTLE_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, throttle_control);
- udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
- }
-#endif
+ if (pwManual[THROTTLE_INPUT_CHANNEL] == 0)
+ {
+ udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = 0;
+ }
+ else
+ {
+ temp = pwManual[THROTTLE_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, throttle_control);
+ udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
+ }
+#endif // AIRFRAME_VTAIL

// Delta-Wing airplane airframe
// Mix roll_control, pitch_control, and waggle into aileron and elevator
// Mix rudder_control into rudder
#if (AIRFRAME_TYPE == AIRFRAME_DELTA)
- if (udb_flags._.radio_on && flags._.pitch_feedback)
+ int16_t aileronInput ;
+ int16_t elevatorInput ;
+ int16_t pitchInput ;
+ int16_t rollInput ;
+ int16_t pitchCommand ;
+ int16_t rollCommand ;
+ int32_t delta_roll_control ;
+ // unmix the inputs, note this will produce zeros during radio off
+ aileronInput = REVERSE_IF_NEEDED( AILERON_CHANNEL_REVERSED , ( pwManual[
AILERON_INPUT_CHANNEL ] - udb_pwTrim[ AILERON_INPUT_CHANNEL ])) ;
+ elevatorInput = REVERSE_IF_NEEDED( ELEVATOR_CHANNEL_REVERSED , (
pwManual[ ELEVATOR_INPUT_CHANNEL ] - udb_pwTrim[ ELEVATOR_INPUT_CHANNEL
])) ;
+ pitchInput = (elevatorInput+aileronInput)>>1 ;
+ rollInput = (elevatorInput-aileronInput)>>1 ;
+
+ if ( flags._.pitch_feedback )
+ {
+ pitchCommand = ( ( elevatorbgain + 8 ) * pitchInput ) >> 3 ;
+ pwManual[RUDDER_INPUT_CHANNEL] += ((pwManual[RUDDER_INPUT_CHANNEL] -
udb_pwTrim[RUDDER_INPUT_CHANNEL]) * rudderbgain) >> 3;
+ rollCommand = 0 ;
+ }
+ else
{
-
- 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
])) ;
- int16_t pitchInput = (((elevatorInput+aileronInput)>>1)*elevatorbgain
)>>3 ;
+ pitchCommand = pitchInput ;
+ rollCommand = rollInput ;
+ }

- int32_t delta_roll_control =
REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED, roll_control);
+ delta_roll_control = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED,
roll_control);

temp = udb_pwTrim[AILERON_INPUT_CHANNEL] +
- REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, -delta_roll_control +
pitchInput + pitch_control - waggle);
+ REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, -rollCommand
-delta_roll_control + pitchCommand + pitch_control - waggle);
udb_pwOut[AILERON_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

temp = udb_pwTrim[ELEVATOR_INPUT_CHANNEL] +
- REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, delta_roll_control +
pitchInput + pitch_control + waggle);
+ REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, rollCommand +
delta_roll_control + pitchCommand + pitch_control + waggle);
udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);

- pwManual[RUDDER_INPUT_CHANNEL] += ((pwManual[RUDDER_INPUT_CHANNEL] -
udb_pwTrim[RUDDER_INPUT_CHANNEL]) * rudderbgain) >> 3;
-
temp = pwManual[RUDDER_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, yaw_control - waggle);
udb_pwOut[RUDDER_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
@@ -151,16 +194,7 @@
temp = pwManual[THROTTLE_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, throttle_control);
udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
}
- }
- else
- {
- udb_pwOut[AILERON_OUTPUT_CHANNEL] = udb_servo_pulsesat(
pwManual[AILERON_INPUT_CHANNEL] -
REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED , waggle ) );
- udb_pwOut[RUDDER_OUTPUT_CHANNEL] = udb_servo_pulsesat(
pwManual[RUDDER_INPUT_CHANNEL] -
REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED , waggle ) );
- udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = pwManual[THROTTLE_INPUT_CHANNEL] ;
- udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(
pwManual[ELEVATOR_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED , waggle ) ) ;
-
- }
-#endif
+#endif // AIRFRAME_DELTA

// Helicopter airframe
// Mix half of roll_control and half of pitch_control into aileron
channels