Permalink
Browse files

Adding support for checkgains in hover and removing manual offset fro…

…m pitchcontrol
  • Loading branch information...
1 parent dd70401 commit a2fd1f87a42b3ec940503eb8f8a23c53eaa50150 @cglusky committed Jan 1, 2012
Binary file not shown.
Oops, something went wrong.
Oops, something went wrong.
@@ -482,7 +482,7 @@
#define HOVER_PITCH_OFFSET 0.0 // + leans towards top, - leans towards bottom
#define HOVER_YAWKP 1.0 //0.2
#define HOVER_YAWKD 0.5 //0.25
-#define HOVER_YAW_OFFSET 0.0 // Testing non zero values
+#define HOVER_YAW_OFFSET 0.0 // ***TODO Test non zero values for control mixing for VTOL
#define HOVER_PITCH_TOWARDS_WP 0.0 //30.0 ***TODO Test for VTOL with nav mode
#define HOVER_NAV_MAX_PITCH_RADIUS 20
@@ -144,18 +144,24 @@ void normalPitchCntrl(void)
void hoverPitchCntrl(void)
{
union longww pitchAccum ;
+ long pitchToWP ; //***TODO Not same type or naming convention as roll: int rollNavDeflection
+
+#ifdef TestGains
+ flags._.GPS_steering = 0 ; // turn off navigation
+ flags._.pitch_feedback = 1 ; //turn on stabilization
+#endif
if ( flags._.pitch_feedback )
{
pitchAccum.WW = ( __builtin_mulss( -rmat[7] , omegagyro[0] )
- __builtin_mulss( rmat[6] , omegagyro[1] )) << 1 ;
pitchrate = pitchAccum._.W1 ;
+ /* Manual offset should achieved in servoMix?
int elevInput = ( udb_flags._.radio_on == 1 ) ? REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, udb_pwIn[ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]) : 0 ;
int manualPitchOffset = elevInput * (int)(RMAX/600);
-
- long pitchToWP ;
-
+ */
+
if ( flags._.GPS_steering )
{
pitchToWP = (tofinish_line > HOVER_NAV_MAX_PITCH_RADIUS) ? HOVERPTOWP : (HOVERPTOWP / HOVER_NAV_MAX_PITCH_RADIUS * tofinish_line) ;
@@ -165,7 +171,8 @@ void hoverPitchCntrl(void)
pitchToWP = 0 ;
}
- pitchAccum.WW = __builtin_mulss( rmat[8] + HOVERPOFFSET - pitchToWP + manualPitchOffset , hoverpitchgain )
+ //pitchAccum.WW = __builtin_mulss( rmat[8] + HOVERPOFFSET - pitchToWP + manualPitchOffset , hoverpitchgain )
+ pitchAccum.WW = __builtin_mulss( rmat[8] + HOVERPOFFSET - pitchToWP , hoverpitchgain ) //Removed manualPitchOffset
+ __builtin_mulss( hoverpitchkd , pitchrate ) ;
}
else
@@ -81,16 +81,14 @@ void normalRollCntrl(void)
#ifdef TestGains
flags._.GPS_steering = 0 ; // turn off navigation
+ flags._.pitch_feedback = 1 ; // turn on stabilization
#endif
+
if ( AILERON_NAVIGATION && flags._.GPS_steering )
{
rollAccum._.W1 = determine_navigation_deflection( 'a' ) ;
}
-#ifdef TestGains
- flags._.pitch_feedback = 1 ;
-#endif
-
if ( ROLL_STABILIZATION_AILERONS && flags._.pitch_feedback )
{
gyroRollFeedback.WW = __builtin_mulss( rollkd , omegaAccum[1] ) ;
@@ -121,6 +119,11 @@ void hoverRollCntrl(void)
{
int rollNavDeflection ;
union longww gyroRollFeedback ;
+
+#ifdef TestGains
+ flags._.GPS_steering = 0 ; // turn off navigation
+ flags._.pitch_feedback = 1 ; //turn on stabilization
+#endif
if ( flags._.pitch_feedback )
{
@@ -182,12 +182,12 @@ void servoMix( void )
// Mix rudder_control into rudder
#if ( AIRFRAME_TYPE == AIRFRAME_VTOL )
long vtol_elevon_roll_control = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED, roll_control) ;
- long vtol_rudderon_roll_control = REVERSE_IF_NEEDED(RUDDERON_VTOL_SURFACES_REVERSED, roll_control) ;
+ long vtol_rudderon_roll_control = REVERSE_IF_NEEDED(RUDDERON_VTOL_SURFACES_REVERSED, roll_control) ;
temp = pwManual[AILERON_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, -vtol_elevon_roll_control/2 + pitch_control) ;
udb_pwOut[AILERON_OUTPUT_CHANNEL] = udb_servo_pulsesat( temp ) ;
-
+
temp = pwManual[ELEVATOR_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, vtol_elevon_roll_control/2 + pitch_control) ;
udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat( temp ) ;
@@ -21,7 +21,7 @@
#include "defines.h"
-#define HOVERYOFFSET ((long)(HOVER_YAW_OFFSET*(RMAX/57.3))) //***TODO Why is this here?
+#define HOVERYOFFSET ((long)(HOVER_YAW_OFFSET*(RMAX/57.3))) //***TODO Why is this here? Should it be in defines.h?
#if(GAINS_VARIABLE == 0)
const int yawkdrud = YAWKD_RUDDER*SCALEGYRO*RMAX ;
@@ -62,10 +62,11 @@ void normalYawCntrl(void)
union longww gyroYawFeedback ;
int ail_rud_mix ;
-#ifdef TestGains
+ #ifdef TestGains
flags._.GPS_steering = 0 ; // turn off navigation
flags._.pitch_feedback = 1 ; // turn on stabilization
-#endif
+ #endif
+
if ( RUDDER_NAVIGATION && flags._.GPS_steering )
{
yawNavDeflection = determine_navigation_deflection( 'y' ) ;
@@ -127,16 +128,28 @@ void hoverYawCntrl(void)
{
union longww yawAccum ;
union longww gyroYawFeedback ;
+
+ #ifdef TestGains
+ flags._.GPS_steering = 0 ; // turn off navigation
+ flags._.pitch_feedback = 1 ; // turn on stabilization
+ #endif
if ( flags._.pitch_feedback )
{
gyroYawFeedback.WW = __builtin_mulss( hoveryawkd , omegaAccum[2] ) ;
+
+ /***TODO Check this with others
+ **This was causing 2nd rudder output to = 0
+ **Seems manualYawoffset should be handled in servoMix already?
+ **Tested to work in manual and stabilized mode on VFO1
+
+ int yawInput = ( udb_flags._.radio_on == 1 ) ? REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, udb_pwIn[RUDDER_INPUT_CHANNEL] - udb_pwTrim[RUDDER_INPUT_CHANNEL]) : 0 ;
+ int manualYawOffset = yawInput * (int)(RMAX/2000);
- //int yawInput = ( udb_flags._.radio_on == 1 ) ? REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, udb_pwIn[RUDDER_INPUT_CHANNEL] - udb_pwTrim[RUDDER_INPUT_CHANNEL]) : 0 ;
- //int manualYawOffset = yawInput * (int)(RMAX/2000);
-
- //yawAccum.WW = __builtin_mulss( rmat[6] + HOVERYOFFSET + manualYawOffset , hoveryawkp ) ;
- yawAccum.WW = __builtin_mulss( rmat[6] + HOVERYOFFSET , hoveryawkp ) ;
+ yawAccum.WW = __builtin_mulss( rmat[6] + HOVERYOFFSET + manualYawOffset , hoveryawkp ) ;
+ */
+
+ yawAccum.WW = __builtin_mulss( rmat[6] + HOVERYOFFSET , hoveryawkp ) ; //Removed manualYawOffset
}
else
{

0 comments on commit a2fd1f8

Please sign in to comment.