diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 724e913015..3bedca3890 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -313,10 +313,12 @@ AP_Mount::calc_GPS_target_angle(struct Location *target) _roll_control_angle = 0; _pitch_control_angle = atan2(GPS_vector_z, target_distance); _yaw_control_angle = atan2(GPS_vector_x, GPS_vector_y); +/* // Converts +/- 180 into 0-360. if(_yaw_control_angle<0){ _yaw_control_angle += 2*M_PI; } +*/ } /// Stabilizes mount relative to the Earth's frame @@ -365,13 +367,6 @@ AP_Mount::stabilize() } } -// This function is needed to let the HIL code compile -long -AP_Mount::rc_map(RC_Channel_aux* rc_ch) -{ - return (rc_ch->radio_in - rc_ch->radio_min) * (rc_ch->angle_max - rc_ch->angle_min) / (rc_ch->radio_max - rc_ch->radio_min) + rc_ch->angle_min; -} - // For testing and development. Called in the medium loop. void AP_Mount::debug_output() diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index a7194524e8..5f71d526d8 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -64,7 +64,6 @@ private: // internal methods void calc_GPS_target_angle(struct Location *target); void stabilize(); - long rc_map(RC_Channel_aux* rc_ch); //members AP_AHRS *_ahrs; ///< Rotation matrix from earth to plane.