diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index d0f01683ef..2a36c6de42 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -129,11 +129,6 @@ void AP_Mount::update_mount_position() } // write the results to the servos -/* - G_RC_AUX(k_mount_roll)->angle_out(_roll_angle); - G_RC_AUX(k_mount_pitch)->angle_out(_pitch_angle); - G_RC_AUX(k_mount_yaw)->angle_out(_yaw_angle); -*/ // closest_limit() takes degrees * 10 units G_RC_AUX(k_mount_roll)->closest_limit(_roll_angle*10); G_RC_AUX(k_mount_pitch)->closest_limit(_pitch_angle*10); diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index d55e98de5c..e7f259ceee 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -83,21 +83,6 @@ RC_Channel_aux::rc_input(float *control_angle, int16_t angle) } } -/// Takes the desired servo angle(deg) and converts to microSeconds for PWM -/// Like this: 45 deg = 2000 us ; -45 deg/1000 us. 1000us/(90*100 deg) = 0.1111111111111 -void -RC_Channel_aux::angle_out(int16_t angle) -{ - if(angle >= angle_max){ - angle = angle_max; - } - if(angle <= angle_min){ - angle = angle_min; - } - // Convert the angle*100 to pwm microseconds. 45 deg = 500 us. - radio_out = (/*_reverse * */ angle * 0.1111111) + 1500; -} - /// map a function to a servo channel and output it void RC_Channel_aux::output_ch(unsigned char ch_nr) diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 36b4e71641..748386f700 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -53,8 +53,6 @@ public: int16_t closest_limit(int16_t angle); - void angle_out(int16_t angle); - void rc_input(float *control_angle, int16_t angle); void output_ch(unsigned char ch_nr);