diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index a6b11cde04..f32bc3cdae 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -744,6 +744,12 @@ void fifty_hz_loop() #endif // XXX this should be absorbed into the above, // or be a "GCS fast loop" interface + + // Hack - had to move to 50hz loop to test a theory + if(g.frame_type == TRI_FRAME){ + // servo Yaw + APM_RC.OutputCh(CH_7, g.rc_4.radio_out); + } } @@ -1251,8 +1257,6 @@ void update_alt() } } - - // calculate our altitude if(altitude_sensor == BARO){ current_loc.alt = baro_alt + baro_alt_offset + home.alt; diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 96236c8369..0167ee1791 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -131,9 +131,6 @@ set_servos_4() // this is a compensation for the angle of the yaw motor. Its linear, but should work ok. //motor_out[CH_4] += (float)(abs(g.rc_4.control_in)) * .013; - // servo Yaw - APM_RC.OutputCh(CH_7, g.rc_4.radio_out); - }else if (g.frame_type == HEXAX_FRAME) { //Serial.println("6_FRAME");