Line data    Source code

       1             : #include "Tracker.h"
       2             : 
       3             : /*
       4             :  * GCS controlled servo test mode
       5             :  */
       6             : 
       7             : /*
       8             :  * set_servo - sets the yaw or pitch servo pwm directly
       9             :  *  servo_num are 1 for yaw, 2 for pitch
      10             :  */
      11           8 : bool ModeServoTest::set_servo(uint8_t servo_num, uint16_t pwm)
      12             : {
      13             :     // convert servo_num from 1~2 to 0~1 range
      14           8 :     servo_num--;
      15             : 
      16             :     // exit immediately if servo_num is invalid
      17           8 :     if (servo_num != CH_YAW && servo_num != CH_PITCH) {
      18           0 :         return false;
      19             :     }
      20             : 
      21           8 :     hal.rcout->cork();
      22             : 
      23             :     // set yaw servo pwm and send output to servo
      24           8 :     if (servo_num == CH_YAW) {
      25           4 :         SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_yaw, pwm);
      26           4 :         SRV_Channels::constrain_pwm(SRV_Channel::k_tracker_yaw);
      27             :     }
      28             : 
      29             :     // set pitch servo pwm and send output to servo
      30           8 :     if (servo_num == CH_PITCH) {
      31           4 :         SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_pitch, pwm);
      32           4 :         SRV_Channels::constrain_pwm(SRV_Channel::k_tracker_pitch);
      33             :     }
      34             : 
      35           8 :     SRV_Channels::calc_pwm();
      36           8 :     SRV_Channels::output_ch_all();
      37             : 
      38           8 :     hal.rcout->push();
      39             : 
      40             :     // return success
      41           8 :     return true;
      42             : }