Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AntennaTracker: adapted to new SRV_Channel API

Browse Source
This commit is contained in:
Andrew Tridgell 2017-01-07 13:51:56 +11:00
parent 93d6b012c2
commit 3f71fd98a0
8 changed files with 59 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
AntennaTracker/Parameters.cpp
View File

@ -302,15 +302,11 @@ const AP_Param::Info Tracker::var_info[] = {
// @Path: ../libraries/AP_Notify/AP_Notify.cpp
GOBJECT(notify, "NTF_", AP_Notify),
// RC channel
//-----------
// @Group: RC1_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GOBJECT(channel_yaw, "RC1_", RC_Channel),
GOBJECT(rc_channels, "RC", RC_Channels),
// @Group: RC2_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GOBJECT(channel_pitch, "RC2_", RC_Channel),
// @Path: ../libraries/SRV_Channel/SRV_Channel.cpp
GOBJECT(servo_channels, "SERVO", SRV_Channels),
// @Group: SERIAL
// @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp

6
AntennaTracker/Parameters.h
View File

@ -106,10 +106,12 @@ public:
//
// 200 : Radio settings
//
k_param_channel_yaw = 200,
k_param_channel_pitch,
k_param_channel_yaw_old = 200,
k_param_channel_pitch_old,
k_param_pidPitch2Srv,
k_param_pidYaw2Srv,
k_param_rc_channels,
k_param_servo_channels,
//
// 220: Waypoint data

4
AntennaTracker/Tracker.h
View File

@ -128,8 +128,8 @@ private:
/**
antenna control channels
*/
RC_Channel channel_yaw{CH_YAW};
RC_Channel channel_pitch{CH_PITCH};
RC_Channels rc_channels;
SRV_Channels servo_channels;
LowPassFilterFloat yaw_servo_out_filt;
LowPassFilterFloat pitch_servo_out_filt;

12
AntennaTracker/control_manual.cpp
View File

@ -11,10 +11,12 @@
void Tracker::update_manual(void)
{
// copy yaw and pitch input to output
channel_yaw.set_radio_out(constrain_int16(channel_yaw.get_radio_in(), channel_yaw.get_radio_min(), channel_yaw.get_radio_max()));
channel_pitch.set_radio_out(constrain_int16(channel_pitch.get_radio_in(), channel_pitch.get_radio_min(), channel_pitch.get_radio_max()));
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, RC_Channels::rc_channel(CH_YAW)->get_radio_in());
SRV_Channels::constrain_pwm(SRV_Channel::k_steering);
// send output to servos
channel_yaw.output();
channel_pitch.output();
SRV_Channels::set_output_pwm(SRV_Channel::k_elevator, RC_Channels::rc_channel(CH_PITCH)->get_radio_in());
SRV_Channels::constrain_pwm(SRV_Channel::k_elevator);
SRV_Channels::calc_pwm();
SRV_Channels::output_ch_all();
}

11
AntennaTracker/control_servo_test.cpp
View File

@ -25,16 +25,19 @@ bool Tracker::servo_test_set_servo(uint8_t servo_num, uint16_t pwm)
// set yaw servo pwm and send output to servo
if (servo_num == CH_YAW) {
channel_yaw.set_radio_out(constrain_int16(pwm, channel_yaw.get_radio_min(), channel_yaw.get_radio_max()));
channel_yaw.output();
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, pwm);
SRV_Channels::constrain_pwm(SRV_Channel::k_steering);
}
// set pitch servo pwm and send output to servo
if (servo_num == CH_PITCH) {
channel_pitch.set_radio_out(constrain_int16(pwm, channel_pitch.get_radio_min(), channel_pitch.get_radio_max()));
channel_pitch.output();
SRV_Channels::set_output_pwm(SRV_Channel::k_elevator, pwm);
SRV_Channels::constrain_pwm(SRV_Channel::k_elevator);
}
SRV_Channels::calc_pwm();
SRV_Channels::output_ch_all();
// return success
return true;
}

3
AntennaTracker/radio.cpp
View File

@ -5,7 +5,6 @@
void Tracker::read_radio()
{
if (hal.rcin->new_input()) {
channel_yaw.set_pwm(hal.rcin->read(CH_YAW));
channel_pitch.set_pwm(hal.rcin->read(CH_PITCH));
RC_Channels::set_pwm_all();
}
}

51
AntennaTracker/servos.cpp
View File

@ -7,17 +7,18 @@
// init_servos - initialises the servos
void Tracker::init_servos()
{
// setup antenna control PWM channels
channel_yaw.set_angle(g.yaw_range * 100/2); // yaw range is +/- (YAW_RANGE parameter/2) converted to centi-degrees
channel_pitch.set_angle((-g.pitch_min+g.pitch_max) * 100/2); // pitch range is +/- (PITCH_MIN/MAX parameters/2) converted to centi-degrees
SRV_Channels::set_default_function(CH_YAW, SRV_Channel::k_steering);
SRV_Channels::set_default_function(CH_PITCH, SRV_Channel::k_elevator);
// move servos to mid position
channel_yaw.output_trim();
channel_pitch.output_trim();
// yaw range is +/- (YAW_RANGE parameter/2) converted to centi-degrees
SRV_Channels::set_angle(SRV_Channel::k_steering, g.yaw_range * 100/2);
// initialise output to servos
channel_yaw.calc_pwm();
channel_pitch.calc_pwm();
// pitch range is +/- (PITCH_MIN/MAX parameters/2) converted to centi-degrees
SRV_Channels::set_angle(SRV_Channel::k_elevator, (-g.pitch_min+g.pitch_max) * 100/2);
SRV_Channels::output_trim_all();
SRV_Channels::calc_pwm();
SRV_Channels::output_ch_all();
yaw_servo_out_filt.set_cutoff_frequency(SERVO_OUT_FILT_HZ);
pitch_servo_out_filt.set_cutoff_frequency(SERVO_OUT_FILT_HZ);
@ -45,8 +46,8 @@ void Tracker::update_pitch_servo(float pitch)
}
// convert servo_out to radio_out and send to servo
channel_pitch.calc_pwm();
channel_pitch.output();
SRV_Channels::calc_pwm();
SRV_Channels::output_ch_all();
}
/**
@ -76,7 +77,7 @@ void Tracker::update_pitch_position_servo()
// calculate new servo position
g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch);
int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid();
int32_t new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) + g.pidPitch2Srv.get_pid();
// position limit pitch servo
if (new_servo_out <= pitch_min_cd) {
@ -88,7 +89,7 @@ void Tracker::update_pitch_position_servo()
g.pidPitch2Srv.reset_I();
}
// rate limit pitch servo
channel_pitch.set_servo_out(new_servo_out);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, new_servo_out);
if (pitch_servo_out_filt_init) {
pitch_servo_out_filt.apply(new_servo_out, G_Dt);
@ -110,15 +111,15 @@ void Tracker::update_pitch_onoff_servo(float pitch)
float acceptable_error = g.onoff_pitch_rate * g.onoff_pitch_mintime;
if (fabsf(nav_status.angle_error_pitch) < acceptable_error) {
channel_pitch.set_servo_out(0);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 0);
} else if ((nav_status.angle_error_pitch > 0) && (pitch*100>pitch_min_cd)) {
// positive error means we are pointing too low, so push the
// servo up
channel_pitch.set_servo_out(-9000);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, -9000);
} else if (pitch*100<pitch_max_cd) {
// negative error means we are pointing too high, so push the
// servo down
channel_pitch.set_servo_out(9000);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 9000);
}
}
@ -131,7 +132,7 @@ void Tracker::update_pitch_cr_servo(float pitch)
int32_t pitch_max_cd = g.pitch_max*100;
if ((pitch>pitch_min_cd) && (pitch<pitch_max_cd)) {
g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch);
channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, g.pidPitch2Srv.get_pid());
}
}
@ -156,8 +157,8 @@ void Tracker::update_yaw_servo(float yaw)
}
// convert servo_out to radio_out and send to servo
channel_yaw.calc_pwm();
channel_yaw.output();
SRV_Channels::calc_pwm();
SRV_Channels::output_ch_all();
}
/**
@ -200,7 +201,7 @@ void Tracker::update_yaw_position_servo()
g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw);
float servo_change = g.pidYaw2Srv.get_pid();
servo_change = constrain_float(servo_change, -18000, 18000);
float new_servo_out = constrain_float(channel_yaw.get_servo_out() + servo_change, -18000, 18000);
float new_servo_out = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_steering) + servo_change, -18000, 18000);
// position limit yaw servo
if (new_servo_out <= -yaw_limit_cd) {
@ -212,7 +213,7 @@ void Tracker::update_yaw_position_servo()
g.pidYaw2Srv.reset_I();
}
channel_yaw.set_servo_out(new_servo_out);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, new_servo_out);
if (yaw_servo_out_filt_init) {
yaw_servo_out_filt.apply(new_servo_out, G_Dt);
@ -232,15 +233,15 @@ void Tracker::update_yaw_onoff_servo(float yaw)
{
float acceptable_error = g.onoff_yaw_rate * g.onoff_yaw_mintime;
if (fabsf(nav_status.angle_error_yaw * 0.01f) < acceptable_error) {
channel_yaw.set_servo_out(0);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
} else if (nav_status.angle_error_yaw * 0.01f > 0) {
// positive error means we are counter-clockwise of the target, so
// move clockwise
channel_yaw.set_servo_out(18000);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 18000);
} else {
// negative error means we are clockwise of the target, so
// move counter-clockwise
channel_yaw.set_servo_out(-18000);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, -18000);
}
}
@ -250,5 +251,5 @@ void Tracker::update_yaw_onoff_servo(float yaw)
void Tracker::update_yaw_cr_servo(float yaw)
{
g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw);
channel_yaw.set_servo_out(-g.pidYaw2Srv.get_pid());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, -g.pidYaw2Srv.get_pid());
}

14
AntennaTracker/system.cpp
View File

@ -163,14 +163,12 @@ void Tracker::set_home(struct Location temp)
void Tracker::arm_servos()
{
channel_yaw.enable_out();
channel_pitch.enable_out();
hal.util->set_soft_armed(true);
}
void Tracker::disarm_servos()
{
channel_yaw.disable_out();
channel_pitch.disable_out();
hal.util->set_soft_armed(false);
}
/*
@ -179,10 +177,10 @@ void Tracker::disarm_servos()
void Tracker::prepare_servos()
{
start_time_ms = AP_HAL::millis();
channel_yaw.set_radio_out(channel_yaw.get_radio_trim());
channel_pitch.set_radio_out(channel_pitch.get_radio_trim());
channel_yaw.output();
channel_pitch.output();
SRV_Channels::set_output_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::calc_pwm();
SRV_Channels::output_ch_all();
}
void Tracker::set_mode(enum ControlMode mode)