24. Update Switch-out from Auto-Pilot Mode

1. In order to receive indication physically on Dev Board that we have switched out from Auto-Pilot Mode  we use the Blue LED (declared in ConfigUDB4.h file) present on it.

So in servoMix.c file we made the following changes:

void startupfuntion(void)

{

if (startflag == 0)

{

startflag =1;

currentvalue = mychannelvalue;

}

}

void checkmode(void)

{

diff = mychannelvalue-currentvalue;

if((diff>100) || (diff<-100))

//if (diff==1)

{

currentvalue = mychannelvalue;

if(LED_BLUE == 0)

LED_BLUE = 1;

else

LED_BLUE = 0;

if (flags._.GPS_steering == 1 && flags._.pitch_feedback == 1 && udb_flags._.radio_on == 1)

            {

 next_waypoint();

              currentvalue = mychannelvalue;

}

}

}

2. Also we declared our int variable - diff in defines.h file so that it may be used for other purposes too, and removed its static declaration:

extern int diff; //extern helps to access a variable/function defined in a single file and makes it available for all the other files in our program

3. In order to check what difference in servoMix.c file (diff) is coming in the - diff = mychannelvalue-currentvalue;, so that we may check whether vale in our condition - if((diff>100) || (diff<-100)) is within range or not. So we opened telemetry.c file and did the following changes:

// Yaw

// Earth Frame of Reference

// Ardustation does not use yaw in degrees

// matrix_accum.x = rmat[4] ;

// matrix_accum.y = rmat[1] ;

// earth_yaw = rect_to_polar(&matrix_accum) ; // binary angle (0 - 256 = 360 degrees)

// earth_yaw = (earth_yaw * BYTECIR_TO_DEGREE) >> 16 ; // switch polarity, convert to -180 - 180 degrees

// The Ardupilot GroundStation protocol is mostly documented here:

//    http://diydrones.com/profiles/blogs/ardupilot-telemetry-protocol

if (udb_heartbeat_counter % 40 == 0)  // Every 8 runs (5 heartbeat counts per 8Hz)

{

serial_output("!!!LAT:%li,LON:%li,SPD:%.2f,CRT:%.2f,ALT:%li,ALH:%i,CRS:%.2f,BER:%i,WPN:%i,DST:%i,BTV:%.2f***\r\n"

 "+++THH:%i,RLL:%li,PCH:%li,STT:%i,***\r\n",

lat_gps.WW / 10 , long_gps.WW / 10 , (float)(sog_gps.BB / 100.0), (float)(climb_gps.BB / 100.0),

(alt_sl_gps.WW - alt_origin.WW) / 100, desiredHeight, (float)(cog_gps.BB / 100.0), desired_dir_deg,

waypointIndex, diff, (float)(voltage_milis.BB / 100.0), 

(int16_t)((udb_pwOut[THROTTLE_OUTPUT_CHANNEL] - udb_pwTrim[THROTTLE_OUTPUT_CHANNEL])/20),

earth_roll, earth_pitch,

mode

) ;

}

else if (udb_heartbeat_counter % 10 == 0)  // Every 2 runs (5 heartbeat counts per 8Hz)

{

serial_output("+++THH:%i,RLL:%li,PCH:%li,STT:%i,***\r\n",

(int16_t)((udb_pwOut[THROTTLE_OUTPUT_CHANNEL] - udb_pwTrim[THROTTLE_OUTPUT_CHANNEL])/20),

earth_roll, earth_pitch,

mode

) ;

}

return ;

}

We changed to diff from tofinish_line so that in Realterm Software we may see the diff value coming and set our condition accordingly.

4. And finally: What is the meaning of diff?

Diff is the decimal difference in milliseconds that comes between the up and down position of the switch we will use, on the transmitter, to switch-out from Auto-Pilot mode.

23. Switch-out from Auto-Pilot Mode : Another function for #UAV

In our previous programming segment (16. Auto Take-Off and Auto-Landing programming of the #UAV and 6. How are we going to build a more stable Loiter program? - The Idea.) we had made Auto-Pilot program and also an Auto-take off and Auto-landing program for our U.A.V.

Then we thought more. What if due to some reason we want our U.A.V to move out of the Auto-Pilot mode towards the next desired waypoint (like sometimes we may want a quick auto-landing or surveillance of some other area)?. Hence we came up with another addition to the whole program we had made so far.

We decided to use one of the 2 output channels left (rest 5 out total 7 are used for Aileron, Throttle, rudder, Elevator and Mode) of the UDB Devboard to receive a signal from our transmitter so that whenever we change the position of the switch from its initial position (at start-up) i.e. from up to down or down to up, our  DevBoard will switch out of the Auto-Pilot mode.

So in order form a logic, we first tried a simple C program in which if we press any letter different from the one we have put in the memory (through scanf function), then it will display a certain message and if we press the same letter as in the memory then another certain message will appear:


#include <stdio.h>
#include <conio.h>

int main()

{
char in, oc;
in = 'j';
printf("Testing code");
while(1)
{
          printf("input pls");
          oc=getch(); // so that we do not have to press enter every-time we enter something on screen
          //scanf("%c", &oc);
          //printf("yes wrkin");
          if (oc!=in)
             {
                     printf("Matched");
                    if ( (oc == 'j')||(oc == 'h'))
                       {
                                printf("moved to next waypoint");
                                in = oc;
                       }
             }
}
   getch();
   return 0;
}

To implement this idea we went into the servoMix.c file of our program and edited the following:

static int mychannelvalue =0;
static char startflag =0; // will be used to check position of switch at sart-up only once
void startupfuntion(void); //to store the current position of the switch
void checkmode (void); // to check the position of the switch, if changed then switch-out of Auto-Pilot mode
static int currentvalue =0;

// Helicopter airframe
// Mix half of roll_control and half of pitch_control into aileron channels
// Mix full pitch_control into elevator
// Ignore waggle for now
#if ( AIRFRAME_TYPE == AIRFRAME_HELI )
temp = pwManual[AILERON_INPUT_CHANNEL] +
REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, roll_control/2 +

pitch_control/2) ;
udb_pwOut[AILERON_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[AILERON_SECONDARY_OUTPUT_CHANNEL] +
REVERSE_IF_NEEDED(AILERON_SECONDARY_CHANNEL_REVERSED,

-roll_control/2 + pitch_control/2) ;
udb_pwOut[AILERON_SECONDARY_OUTPUT_CHANNEL] = temp ;

temp = pwManual[RUDDER_INPUT_CHANNEL] /*+

REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, 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
mychannelvalue = udb_servo_pulsesat( pwManual[PASSTHROUGH_A_INPUT_CHANNEL] )

;

startupfuntion();
checkmode();
// next_waypoint();      

udb_pwOut[PASSTHROUGH_A_OUTPUT_CHANNEL] = udb_servo_pulsesat(

pwManual[PASSTHROUGH_A_INPUT_CHANNEL] ) ;
udb_pwOut[PASSTHROUGH_B_OUTPUT_CHANNEL] = udb_servo_pulsesat(

pwManual[PASSTHROUGH_B_INPUT_CHANNEL] ) ;
udb_pwOut[PASSTHROUGH_C_OUTPUT_CHANNEL] = udb_servo_pulsesat(

pwManual[PASSTHROUGH_C_INPUT_CHANNEL] ) ;
udb_pwOut[PASSTHROUGH_D_OUTPUT_CHANNEL] = udb_servo_pulsesat(

pwManual[PASSTHROUGH_D_INPUT_CHANNEL] ) ;
}

void cameraServoMix( void )
{
int32_t temp ;
int16_t pwManual[NUM_INPUTS+1] ;

// If radio is off, use udb_pwTrim values instead of the udb_pwIn values
for (temp = 0; temp <= NUM_INPUTS; temp++)
if (udb_flags._.radio_on)
pwManual[temp] = udb_pwIn[temp];
else
pwManual[temp] = udb_pwTrim[temp];

temp = ( pwManual[CAMERA_PITCH_INPUT_CHANNEL] - 3000 ) +

REVERSE_IF_NEEDED(CAMERA_PITCH_CHANNEL_REVERSED,
cam_pitch_servo_pwm_delta ) ;
temp = cam_pitchServoLimit(temp) ;
udb_pwOut[CAMERA_PITCH_OUTPUT_CHANNEL] = udb_servo_pulsesat( temp + 3000 ) ;

temp = ( pwManual[CAMERA_YAW_INPUT_CHANNEL] - 3000 ) +

REVERSE_IF_NEEDED(CAMERA_YAW_CHANNEL_REVERSED,
cam_yaw_servo_pwm_delta ) ;
temp = cam_yawServoLimit(temp) ;
udb_pwOut[CAMERA_YAW_OUTPUT_CHANNEL] = udb_servo_pulsesat( temp + 3000 ) ;
}

void startupfuntion(void)
{
if (startflag == 0)
{
startflag =1;
currentvalue = mychannelvalue;
}
}

void checkmode(void)
{
int diff;
diff = mychannelvalue-currentvalue;
 if(diff>100 || diff<-100)
{
             if (flags._.GPS_steering == 1 && flags._.pitch_feedback == 1 && udb_flags._.radio_on == 1) //to check if in Auto-Pilot mode, else will never go for the function, make sure only Auto-Pilot switch-out
            {
 next_waypoint();
                           currentvalue = mychannelvalue;
}
}
}