Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tracker: Changing pitch_range to pitch_min and pitch_max

This commit is contained in:
stefanlynka 2016-07-01 12:40:01 +09:00 committed by Randy Mackay
parent 18e51da12d
commit ca22f6612d
4 changed files with 46 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
AntennaTracker/Parameters.cpp
View File

@ -196,15 +196,6 @@ const AP_Param::Info Tracker::var_info[] = {
// @User: Standard
GSCALAR(yaw_range, "YAW_RANGE", YAW_RANGE_DEFAULT),
// @Param: PITCH_RANGE
// @DisplayName: Pitch Range
// @Description: Pitch axis total range of motion in degrees
// @Units: degrees
// @Increment: 0.1
// @Range: 0 180
// @User: Standard
GSCALAR(pitch_range, "PITCH_RANGE", PITCH_RANGE_DEFAULT),
// @Param: DISTANCE_MIN
// @DisplayName: Distance minimum to target
// @Description: Tracker will track targets at least this distance away
@ -230,6 +221,24 @@ const AP_Param::Info Tracker::var_info[] = {
// @User: Standard
GSCALAR(mavlink_update_rate, "MAV_UPDATE_RATE", 1),
// @Param: PITCH_MIN
// @DisplayName: Minimum Pitch Angle
// @Description: The lowest angle the pitch can reach
// @Units: Degrees
// @Increment: 1
// @Range: 0 -90
// @User: Standard
GSCALAR(pitch_min, "PITCH_MIN", PITCH_MIN_DEFAULT),
// @Param: PITCH_MAX
// @DisplayName: Maximum Pitch Angle
// @Description: The highest angle the pitch can reach
// @Units: Degrees
// @Increment: 1
// @Range: 0 90
// @User: Standard
GSCALAR(pitch_max, "PITCH_MAX", PITCH_MAX_DEFAULT),
// barometer ground calibration. The GND_ prefix is chosen for
// compatibility with previous releases of ArduPlane
// @Group: GND_

7
AntennaTracker/Parameters.h
View File

@ -87,7 +87,7 @@ public:
k_param_yaw_trim,
k_param_pitch_trim,
k_param_yaw_range,
k_param_pitch_range,
k_param_pitch_range, //deprecated
k_param_distance_min,
k_param_sysid_target, // 138
k_param_gcs3, // stream rates for fourth MAVLink port
@ -101,6 +101,8 @@ public:
k_param_servo_yaw_type,
k_param_alt_source,
k_param_mavlink_update_rate,
k_param_pitch_min,
k_param_pitch_max,
//
// 200 : Radio settings
@ -149,8 +151,9 @@ public:
AP_Float yaw_trim;
AP_Float pitch_trim;
AP_Int16 yaw_range; // yaw axis total range of motion in degrees
AP_Int16 pitch_range; // pitch axis total range of motion in degrees
AP_Int16 distance_min; // target's must be at least this distance from tracker to be tracked
AP_Int16 pitch_min;
AP_Int16 pitch_max;
// Waypoints
//

7
AntennaTracker/config.h
View File

@ -42,8 +42,11 @@
#ifndef YAW_RANGE_DEFAULT
# define YAW_RANGE_DEFAULT 360
#endif
#ifndef PITCH_RANGE_DEFAULT
# define PITCH_RANGE_DEFAULT 180
#ifndef PITCH_MIN_DEFAULT
# define PITCH_MIN_DEFAULT -90
#endif
#ifndef PITCH_MAX_DEFAULT
# define PITCH_MAX_DEFAULT 90
#endif
//////////////////////////////////////////////////////////////////////////////

25
AntennaTracker/servos.cpp
View File

@ -11,7 +11,7 @@ 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_range * 100/2); // pitch range is +/- (PITCH_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
// move servos to mid position
channel_yaw.output_trim();
@ -59,7 +59,8 @@ void Tracker::update_pitch_position_servo(float pitch)
// servo_out is in 100ths of a degree
float ahrs_pitch = ahrs.pitch_sensor*0.01f;
int32_t angle_err = -(ahrs_pitch - pitch) * 100.0f;
int32_t pitch_limit_cd = g.pitch_range*100/2;
int32_t pitch_min_cd = g.pitch_min*100;
int32_t pitch_max_cd = g.pitch_max*100;
// Need to configure your servo so that increasing servo_out causes increase in pitch/elevation (ie pointing higher into the sky,
// above the horizon. On my antenna tracker this requires the pitch/elevation servo to be reversed
// param set RC2_REV -1
@ -83,7 +84,7 @@ void Tracker::update_pitch_position_servo(float pitch)
// rate limit pitch servo
if (g.pitch_slew_time > 0.02f) {
uint16_t max_change = 0.02f * (pitch_limit_cd) / g.pitch_slew_time;
uint16_t max_change = 0.02f * ((-pitch_min_cd+pitch_max_cd)/2) / g.pitch_slew_time;
if (max_change < 1) {
max_change = 1;
}
@ -97,12 +98,12 @@ void Tracker::update_pitch_position_servo(float pitch)
channel_pitch.set_servo_out(new_servo_out);
// position limit pitch servo
if (channel_pitch.get_servo_out() <= -pitch_limit_cd) {
channel_pitch.set_servo_out(-pitch_limit_cd);
if (channel_pitch.get_servo_out() <= pitch_min_cd) {
channel_pitch.set_servo_out(pitch_min_cd);
g.pidPitch2Srv.reset_I();
}
if (channel_pitch.get_servo_out() >= pitch_limit_cd) {
channel_pitch.set_servo_out(pitch_limit_cd);
if (channel_pitch.get_servo_out() >= pitch_max_cd) {
channel_pitch.set_servo_out(pitch_max_cd);
g.pidPitch2Srv.reset_I();
}
}
@ -119,15 +120,17 @@ void Tracker::update_pitch_onoff_servo(float pitch)
// get_servo_out() is in 100ths of a degree
float ahrs_pitch = degrees(ahrs.pitch);
float err = ahrs_pitch - pitch;
int32_t pitch_min_cd = g.pitch_min*100;
int32_t pitch_max_cd = g.pitch_max*100;
float acceptable_error = g.onoff_pitch_rate * g.onoff_pitch_mintime;
if (fabsf(err) < acceptable_error) {
channel_pitch.set_servo_out(0);
} else if (err > 0) {
} else if ((err > 0) && (pitch*100>pitch_min_cd)) {
// positive error means we are pointing too high, so push the
// servo down
channel_pitch.set_servo_out(-9000);
} else {
} else if (pitch*100<pitch_max_cd){
// negative error means we are pointing too low, so push the
// servo up
channel_pitch.set_servo_out(9000);
@ -141,9 +144,13 @@ void Tracker::update_pitch_cr_servo(float pitch)
{
float ahrs_pitch = degrees(ahrs.pitch);
float err_cd = (pitch - ahrs_pitch) * 100.0f;
int32_t pitch_min_cd = g.pitch_min*100;
int32_t pitch_max_cd = g.pitch_max*100;
if ((pitch>pitch_min_cd) && (pitch<pitch_max_cd)){
g.pidPitch2Srv.set_input_filter_all(err_cd);
channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid());
}
}
/**
update the yaw (azimuth) servo.