This is just something that has been bothering me lately. I will start at the very beginning. My quad flies great in stable mode, but I have one little problem. I only have 5 channels on my receiver right now, so I decided a little code tweak should solve the problem. I would just make channel 5 toggle acro, then stable, then gps, and start over. Tested it out in the house all seemed well, so time for test flight. I took it out and got it into stable mode it was flying great.

Then gps mode...
This is where the problem started, and my first big mistake. I hadn't tested gps mode before. It decided that it would rapidly find 20' that apparently it thought it needed. Then it decided to rapidly drift 20' to the right. Of course, I was trying to counter act this but in gps mode it was completely unresponsive to throttle or directional controls. Then I tried toggling out of gps mode, but it was too late. It found a tree.

My question really boils down to does anyone know what happened in gps mode? Why did it not respond at all to throttle inputs or directional? The code that was modified was the NG code. Below is the radio library that I edited. The only real change was in flight mode selection. Please feel free to look it over and tell me if that is what really screwed stuff up. I haven't looked at the firmware in the gps, so it could be that it was just old and needs to be updated too.


#define STICK_TO_ANGLE_FACTOR 12.0 // To convert stick position to absolute angles

void read_radio()
int tempThrottle = 0;

// Commands from radio Rx
// We apply the Radio calibration parameters (from configurator) except for throttle
ch_roll = channel_filter(APM_RC.InputCh(CH_ROLL) * ch_roll_slope + ch_roll_offset, ch_roll);
ch_pitch = channel_filter(APM_RC.InputCh(CH_PITCH) * ch_pitch_slope + ch_pitch_offset, ch_pitch);
ch_yaw = channel_filter(APM_RC.InputCh(CH_RUDDER) * ch_yaw_slope + ch_yaw_offset, ch_yaw);
ch_aux = APM_RC.InputCh(CH_5) * ch_aux_slope + ch_aux_offset;
ch_aux2 = APM_RC.InputCh(CH_5) * ch_aux2_slope + ch_aux2_offset;

// special checks for throttle
tempThrottle = APM_RC.InputCh(CH_THROTTLE);

// throttle safety check
if( motorSafety == 1 ) {
if( motorArmed == 1 ) {
if( ch_throttle > MIN_THROTTLE + 100 ) { // if throttle is now over MIN..
// if throttle has increased suddenly, disarm motors
if( (tempThrottle - ch_throttle) > SAFETY_MAX_THROTTLE_INCREASE ) {
motorArmed = 0;
}else{ // if it hasn't increased too quickly turn off the safety
motorSafety = 0;
}else if( ch_throttle < MIN_THROTTLE + 100 ) { // Safety logic: hold throttle low for more than 1 second, safety comes on which stops sudden increases in throttle
if( Safety_counter > SAFETY_DELAY ) {
motorSafety = 1;
Safety_counter = 0;
}else{ // throttle is over MIN so make sure to reset Safety_counter
Safety_counter = 0;
// normal throttle filtering. Note: Transmiter calibration not used on throttle
ch_throttle = channel_filter(tempThrottle, ch_throttle);

// Flight mode

current_state = ch_aux;
if (current_state-previousstate>500){
if (modeCounter == 3){
AP_mode = AP_NORMAL_MODE; // Normal mode
previousstate = current_state;

flightMode = ACRO_MODE; // Force to Acro mode from radio
flightMode = STABLE_MODE; // Stable mode (default)

// Autopilot mode (only works on Stable mode)
if (flightMode == STABLE_MODE)
AP_mode = AP_AUTOMATIC_MODE; // Automatic mode : GPS position hold mode + altitude hold
AP_mode = AP_NORMAL_MODE; // Normal mode

if (flightMode==STABLE_MODE) // IN STABLE MODE we convert stick positions to absoulte angles
// In Stable mode stick position defines the desired angle in roll, pitch and yaw
// #ifdef FLIGHT_MODE_X
if(flightOrientation) {
// For X mode we make a mix in the input
float aux_roll = (ch_roll-roll_mid) / STICK_TO_ANGLE_FACTOR;
float aux_pitch = (ch_pitch-pitch_mid) / STICK_TO_ANGLE_FACTOR;
command_rx_roll = aux_roll - aux_pitch;
command_rx_pitch = aux_roll + aux_pitch;
} else {
command_rx_roll = (ch_roll-roll_mid) / STICK_TO_ANGLE_FACTOR; // Convert stick position to absolute angles
command_rx_pitch = (ch_pitch-pitch_mid) / STICK_TO_ANGLE_FACTOR;

// YAW
if (abs(ch_yaw-yaw_mid)>6) // Take into account a bit of "dead zone" on yaw
command_rx_yaw += (ch_yaw-yaw_mid) / YAW_STICK_TO_ANGLE_FACTOR;
command_rx_yaw = Normalize_angle(command_rx_yaw); // Normalize angle to [-180,180]

// Write Radio data to DataFlash log

// Motor arm logic
if (ch_throttle < (MIN_THROTTLE + 100)) {
control_yaw = 0;
command_rx_yaw = ToDeg(yaw);

// Arm motor output : Throttle down and full yaw right for more than 2 seconds
if (ch_yaw > 1850) {
if (Arming_counter > ARM_DELAY){
if(ch_throttle > 800) {
motorArmed = 1;
minThrottle = MIN_THROTTLE+60; // A minimun value for mantain a bit if throttle

// To Disarm motor output : Throttle down and full yaw left for more than 2 seconds
if (ch_yaw < 1150) {
if (Disarming_counter > DISARM_DELAY){
motorArmed = 0;
minThrottle = MIN_THROTTLE;

Views: 185

Reply to This

Replies to This Discussion

did you had GPS lock?

do you have magneto installed and activated? is it calibrated properly?


in current code it does not respond to directions in GPS mode

It did have gps lock. I do have the magneto installed and activated. I dont think I calibrated the magneto's offsets could that have made it respond erratically?
yes ;)
thanks. I will see what happens when I calibrate the magneto declination and offsets. How different is the 1.6 firmware on the mtek gps from the 1.4? I honestly dont know which one is loaded, but I dont know if its worth the trouble to even find out and upgrade if needed.

Reply to Discussion



Season Two of the Trust Time Trial (T3) Contest 
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here

© 2020   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service