/*
* 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);
}
}
