AntennaTracker/mode_servotest.cpp

   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 bool ModeServoTest::set_servo(uint8_t servo_num, uint16_t pwm)
  12 {
  13     // convert servo_num from 1~2 to 0~1 range
  14     servo_num--;
  15
  16     // exit immediately if servo_num is invalid
  17     if (servo_num != CH_YAW && servo_num != CH_PITCH) {
  18         return false;
  19     }
  20
  21     // set yaw servo pwm and send output to servo
  22     if (servo_num == CH_YAW) {
  23         SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_yaw, pwm);
  24         SRV_Channels::constrain_pwm(SRV_Channel::k_tracker_yaw);
  25     }
  26
  27     // set pitch servo pwm and send output to servo
  28     if (servo_num == CH_PITCH) {
  29         SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_pitch, pwm);
  30         SRV_Channels::constrain_pwm(SRV_Channel::k_tracker_pitch);
  31     }
  32
  33     SRV_Channels::calc_pwm();
  34     SRV_Channels::output_ch_all();
  35
  36     // return success
  37     return true;
  38 }