5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-02 22:18:28 -04:00

Tracker: Making servo type customizable

This commit is contained in:
stefanlynka 2016-05-23 16:54:47 +09:00 committed by Randy Mackay
parent 8d1f50e78f
commit d2ec395cdf
3 changed files with 17 additions and 8 deletions

View File

@ -119,12 +119,19 @@ const AP_Param::Info Tracker::var_info[] = {
// @User: Standard
GSCALAR(startup_delay, "STARTUP_DELAY", 0),
// @Param: SERVO_TYPE
// @DisplayName: Type of servo system being used
// @Description: This allows selection of position servos or on/off servos
// @Param: SERVO_PITCH_TYPE
// @DisplayName: Type of servo system being used for pitch
// @Description: This allows selection of position servos or on/off servos for pitch
// @Values: 0:Position,1:OnOff,2:ContinuousRotation
// @User: Standard
GSCALAR(servo_type, "SERVO_TYPE", SERVO_TYPE_POSITION),
GSCALAR(servo_pitch_type, "SERVO_PITCH_TYPE", SERVO_TYPE_POSITION),
// @Param: SERVO_YAW_TYPE
// @DisplayName: Type of servo system being used for yaw
// @Description: This allows selection of position servos or on/off servos for yaw
// @Values: 0:Position,1:OnOff,2:ContinuousRotation
// @User: Standard
GSCALAR(servo_yaw_type, "SERVO_YAW_TYPE", SERVO_TYPE_POSITION),
// @Param: ONOFF_YAW_RATE
// @DisplayName: Yaw rate for on/off servos

View File

@ -79,7 +79,7 @@ public:
k_param_gps,
k_param_scan_speed,
k_param_proxy_mode_unused, // deprecated
k_param_servo_type,
k_param_servo_pitch_type,
k_param_onoff_yaw_rate,
k_param_onoff_pitch_rate,
k_param_onoff_yaw_mintime,
@ -98,6 +98,7 @@ public:
// 150: Telemetry control
//
k_param_serial_manager, // serial manager library
k_param_servo_yaw_type,
//
// 200 : Radio settings
@ -133,7 +134,8 @@ public:
AP_Float start_longitude;
AP_Float startup_delay;
AP_Int8 servo_type;
AP_Int8 servo_pitch_type;
AP_Int8 servo_yaw_type;
AP_Float onoff_yaw_rate;
AP_Float onoff_pitch_rate;
AP_Float onoff_yaw_mintime;

View File

@ -28,7 +28,7 @@ void Tracker::init_servos()
*/
void Tracker::update_pitch_servo(float pitch)
{
switch ((enum ServoType)g.servo_type.get()) {
switch ((enum ServoType)g.servo_pitch_type.get()) {
case SERVO_TYPE_ONOFF:
update_pitch_onoff_servo(pitch);
break;
@ -154,7 +154,7 @@ void Tracker::update_pitch_cr_servo(float pitch)
*/
void Tracker::update_yaw_servo(float yaw)
{
switch ((enum ServoType)g.servo_type.get()) {
switch ((enum ServoType)g.servo_yaw_type.get()) {
case SERVO_TYPE_ONOFF:
update_yaw_onoff_servo(yaw);
break;