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.Thanks.#define STICK_TO_ANGLE_FACTOR 12.0 // To convert stick position to absolute angles#define YAW_STICK_TO_ANGLE_FACTOR 150.0void read_radio(){int tempThrottle = 0;// Commands from radio Rx// We apply the Radio calibration parameters (from configurator) except for throttlech_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 throttletempThrottle = APM_RC.InputCh(CH_THROTTLE);// throttle safety checkif( motorSafety == 1 ) {if( motorArmed == 1 ) {if( ch_throttle > MIN_THROTTLE + 100 ) { // if throttle is now over MIN..// if throttle has increased suddenly, disarm motorsif( (tempThrottle - ch_throttle) > SAFETY_MAX_THROTTLE_INCREASE ) {motorArmed = 0;}else{ // if it hasn't increased too quickly turn off the safetymotorSafety = 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 throttleSafety_counter++;if( Safety_counter > SAFETY_DELAY ) {motorSafety = 1;Safety_counter = 0;}}else{ // throttle is over MIN so make sure to reset Safety_counterSafety_counter = 0;}// normal throttle filtering. Note: Transmiter calibration not used on throttlech_throttle = channel_filter(tempThrottle, ch_throttle);// Flight modecurrent_state = ch_aux;if (current_state-previousstate>500){modeCounter++;}if (modeCounter == 3){modeCounter=0;AP_mode = AP_NORMAL_MODE; // Normal mode}previousstate = current_state;if(modeCounter==0)flightMode = ACRO_MODE; // Force to Acro mode from radioelseflightMode = STABLE_MODE; // Stable mode (default)// Autopilot mode (only works on Stable mode)if (flightMode == STABLE_MODE){if(modeCounter==2)AP_mode = AP_AUTOMATIC_MODE; // Automatic mode : GPS position hold mode + altitude holdelseAP_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_Xif(flightOrientation) {// For X mode we make a mix in the inputfloat 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 anglescommand_rx_pitch = (ch_pitch-pitch_mid) / STICK_TO_ANGLE_FACTOR;}// YAWif (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 logLog_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,ch_aux,ch_aux2);// Motor arm logicif (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 secondsif (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}}elseArming_counter++;}elseArming_counter=0;// To Disarm motor output : Throttle down and full yaw left for more than 2 secondsif (ch_yaw < 1150) {if (Disarming_counter > DISARM_DELAY){motorArmed = 0;minThrottle = MIN_THROTTLE;}elseDisarming_counter++;}elseDisarming_counter=0;}else{Arming_counter=0;Disarming_counter=0;}}
You need to be a member of diydrones to add comments!
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.
Replies
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