/* * update_manual - runs the manual controller * called at 50hz while control_mode is 'MANUAL' */ void Tracker::update_manual(void) { if ((enum ServoType)g.servo_pitch_type.get() == SERVO_TYPE_CR) { int16_t radio_in = constrain_int16(RC_Channels::rc_channel(CH_PITCH)->get_radio_in(), RC_Channels::rc_channel(CH_PITCH)->get_radio_min(), RC_Channels::rc_channel(CH_PITCH)->get_radio_max()); //Check for Radio center to lock angle //Check to see if we should lock the pitch angle check_manual_pitch_limits(radio_in); if (pitch_lock) { float pitch = pitch_lock_angle + g.pitch_trim; update_pitch_servo(pitch); } else { SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_pitch, radio_in); SRV_Channels::constrain_pwm(SRV_Channel::k_tracker_pitch); } } else { SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_pitch, constrain_int16(RC_Channels::rc_channel(CH_PITCH)->get_radio_in(), RC_Channels::rc_channel(CH_PITCH)->get_radio_min(), RC_Channels::rc_channel(CH_PITCH)->get_radio_max())); SRV_Channels::constrain_pwm(SRV_Channel::k_tracker_pitch); } // copy yaw and pitch input to output SRV_Channels::set_output_pwm(SRV_Channel::k_tracker_yaw, RC_Channels::rc_channel(CH_YAW)->get_radio_in()); SRV_Channels::constrain_pwm(SRV_Channel::k_tracker_yaw); SRV_Channels::calc_pwm(); SRV_Channels::output_ch_all(); }
/* * update_auto - runs the auto controller * called at 50hz while control_mode is 'AUTO' */ void Tracker::update_auto(void) { // exit immediately if we do not have a valid vehicle position if (!vehicle.location_valid) { return; } float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100); // target yaw in centidegrees float pitch = constrain_float(nav_status.pitch+g.pitch_trim, -90, 90) * 100; // target pitch in centidegrees bool direction_reversed = get_ef_yaw_direction(); calc_angle_error(pitch, yaw, direction_reversed); float bf_pitch; float bf_yaw; convert_ef_to_bf(pitch, yaw, bf_pitch, bf_yaw); // only move servos if target is at least distance_min away if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min)) { update_pitch_servo(bf_pitch); update_yaw_servo(bf_yaw); } }