32. How to #monitor the #Voltage and #Mode of #UAV Battery

We replaced some variables in ardustation programming in telemtry.c and then monitored it in HappyKill More GCS program in the Serial Data:

// The Ardupilot GroundStation protocol is mostly documented here:

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

///////////////////////// battery health testing code

if((udb_analogInputs[0].value<batt_min_value) && (mode ==3))

{

BATT_FLAG =1;

}

//////////////////////////////////////////////////////

if(mode<3)

{

BATT_FLAG =0;

}

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,DS

T:%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, udb_analogInputs[0].value, (float)(voltage_milis.BB / 

100.0), 

(int16_t)((udb_pwOut[THROTTLE_OUTPUT_CHANNEL] - 

udb_pwTrim[THROTTLE_OUTPUT_CHANNEL])/20),

earth_roll, earth_pitch,

(BATT_FLAG+1) // So that flag is not discarded if it is 0

) ;

}

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 ;

}

These can be viewed in the serial data screen of the Happy Kill More GCS program.

31. Further modifications in #Software for Battery Monitoring

We realized that by merely doing the flag int_flightplan 1 in telemetry.c will not direct our program in Return To Land Mode. We discovered in the programming that RTL mode can only be achieved when two conditions are met:

1. Program/drone is in Auto-Pilot Mode, for this we need our gps to be locked &
2. failSafePulses or the pulses coming from the transmitter are 0 i.e. we have lost the radio signal

This can be seen from the following program in states.c file:

/////// Come home state, entered when the radio signal is lost, and gps is locked.

void ent_returnS()

{

flags._.GPS_steering = 1 ;

flags._.pitch_feedback = 1 ;

flags._.altitude_hold_throttle = (ALTITUDEHOLD_WAYPOINT == AH_FULL) ;

flags._.altitude_hold_pitch = (ALTITUDEHOLD_WAYPOINT == AH_FULL || ALTITUDEHOLD_WAYPOINT == AH_PITCH_ONLY) ;

#if ( FAILSAFE_TYPE == FAILSAFE_RTL )

init_flightplan( 1 ) ;

#elif ( FAILSAFE_TYPE == FAILSAFE_MAIN_FLIGHTPLAN )

if ( stateS != &waypointS )

{

init_flightplan( 0 ) ; // Only reset non-rtl waypoints if not already following waypoints

}

The condition to control the failSafePulses is mentioned in the background.c file:

if ( ( failSafePulses == 0 ) || ( noisePulses > MAX_NOISE_RATE ) )

{

if (udb_flags._.radio_on == 1) {

udb_flags._.radio_on = 0 ;

udb_callback_radio_did_turn_off() ;

}

LED_GREEN = LED_OFF ;

noisePulses = 0 ; // reset count of noise pulses

}

else

{

udb_flags._.radio_on = 1 ;

LED_GREEN = LED_ON ;

}

failSafePulses = 0 ;

}

Mode is mentioned in telemetry.c file and can be controlled from the receiver:

void serial_output_8hz( void )

{

uint16_t mode ;

struct relative2D matrix_accum ;

union longbbbb accum ;

int16_t desired_dir_deg ;  // desired_dir converted to a bearing (0-360)

int32_t earth_pitch ; // pitch in binary angles ( 0-255 is 360 degreres)

int32_t earth_roll ; // roll of the plane with respect to earth frame

//int32_t earth_yaw ; // yaw with respect to earth frame

accum.WW  = ( desired_dir * BYTECIR_TO_DEGREE ) + 32768 ;

desired_dir_deg  = accum._.W1 - 90 ; // "Convert UAV DevBoad Earth" to Compass Bearing

if ( desired_dir_deg < 0 ) desired_dir_deg += 360 ; 

if (flags._.GPS_steering == 0 && flags._.pitch_feedback == 0)

mode = 1 ;

else if (flags._.GPS_steering == 0 && flags._.pitch_feedback == 1)

mode = 2 ;

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

mode = 3 ;

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

mode = 0 ;

else

mode = 99 ; // Unknown

Hence to meet the above two conditions we created a flag BATT_FLAG which we defined in telemetry.c and declared as extern in defines.h file. When BATT_FLAG is set to 1 (in telemetry.c file) the int_flightplan  will become 0 (in background.c file) and thus enable out RTL Mode.

In telemetry.c file:

#define batt_min_value 16000

int BATT_FLAG =0;

if((udb_analogInputs[0].value<batt_min_value) && (mode = =3)) // if battery value goes below 16000 and is in Auto-Pilot Mode 

{

BATT_FLAG =1;

}

//////////////////////////////////////////////////////

if(mode<3) // if in Manual or Stabilized Mode then will never make BATT_FLAG 1 and int_flightplan 0

{

BATT_FLAG =0;

}

In background.c file:

// check to see if at least one valid pulse has been received,

// and also that the noise rate has not been exceeded

if(BATT_FLAG = =1)

{

failSafePulses =0;

}

if ( ( failSafePulses = = 0 ) || ( noisePulses > MAX_NOISE_RATE ) )

{

if (udb_flags._.radio_on == 1) {

udb_flags._.radio_on = 0 ;

udb_callback_radio_did_turn_off() ;

}

LED_GREEN = LED_OFF ;

noisePulses = 0 ; // reset count of noise pulses

}

else

{

udb_flags._.radio_on = 1 ;

LED_GREEN = LED_ON ;

}

failSafePulses = 0 ;

}

Here if we change from Auto-Pilot Mode to any other Mode then our BATT_FLAG will become 0 and our failSafePulses will start receiving real time signals from the transmitter on the drone.

How we monitored our changes, I will mention that in my next post.

22. Tip2 for #UAV

Always debug (i.e. check for values in a particular part of the program) your program in a PIC controller testing kit with MPLab 8.9, because UDB4 Devboard does not provide the facility to debug, i.e it can't be debugged with PICKit 2. This will help you as you will discover in the next post on this blog. (Wink! Wink!)

1. You may double click on the left hand side of the line where you want to debug. A letter 'B' in red circle will appear at that point. Change the mode of the MPLab setup to Debug (originally its Release). 
2. After the changes above you may build your program.
3. Connect the PICKit 2 with the testing board and program it.
4. Next go to View - Locals. A window will appear which will show you values at the breakpoints set by you every time you Run the debugging.