Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_ServoRelayEvents: allow mavlink command of rcin scaled functions #25241

Merged
merged 2 commits into from
Oct 16, 2023
Merged

AP_ServoRelayEvents: allow mavlink command of rcin scaled functions #25241

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1ea62e9
SRV_Channel: expose public function to convert scaled value to pwm
tpwrules Oct 11, 2023
6c5181c
AP_ServoRelayEvents: allow mavlink command of rcin scaled functions
tpwrules Oct 11, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 16 additions & 0 deletions libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,14 @@ bool AP_ServoRelayEvents::do_set_servo(uint8_t _channel, uint16_t pwm)
case SRV_Channel::k_gripper:
case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: // rc pass-thru
break;
case SRV_Channel::k_rcin1_mapped ... SRV_Channel::k_rcin16_mapped: {
// mapped channels are set up with a -/+ 4500 angle range by
// SRV_Channel::aux_servo_function_setup
int16_t angle_scaled = constrain_uint16(pwm, 1000, 2000);
angle_scaled = (angle_scaled - 1500) * 9; // 1000 ... 2000 -> -500 ... 500 -> -4500 ... 4500
Copy link
Member

@IamPete1 IamPete1 Oct 16, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
angle_scaled = (angle_scaled - 1500) * 9; // 1000 ... 2000 -> -500 ... 500 -> -4500 ... 4500
angle_scaled = (angle_scaled - 1500) * (4500/500); // 1000 ... 2000 -> -500 ... 500 -> -4500 ... 4500

The compiler will evaluate it to 9 in any case and I think this makes it a little easier to understand.

pwm = c->pwm_from_scaled_value(angle_scaled);
break;
}
default:
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel);
return false;
Expand Down Expand Up @@ -101,6 +109,14 @@ bool AP_ServoRelayEvents::do_repeat_servo(uint8_t _channel, uint16_t _servo_valu
case SRV_Channel::k_gripper:
case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: // rc pass-thru
break;
case SRV_Channel::k_rcin1_mapped ... SRV_Channel::k_rcin16_mapped: {
// mapped channels are set up with a -/+ 4500 angle range by
// SRV_Channel::aux_servo_function_setup
int16_t angle_scaled = constrain_uint16(_servo_value, 1000, 2000);
angle_scaled = (angle_scaled - 1500) * 9; // 1000 ... 2000 -> -500 ... 500 -> -4500 ... 4500
_servo_value = c->pwm_from_scaled_value(angle_scaled);
break;
}
default:
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel);
return false;
Expand Down
21 changes: 11 additions & 10 deletions libraries/SRV_Channel/SRV_Channel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -201,6 +201,15 @@ uint16_t SRV_Channel::pwm_from_angle(float scaled_value) const
}
}

uint16_t SRV_Channel::pwm_from_scaled_value(float scaled_value) const
{
if (type_angle) {
return pwm_from_angle(scaled_value);
} else {
return pwm_from_range(scaled_value);
}
}

void SRV_Channel::calc_pwm(float output_scaled)
{
if (have_pwm_mask & (1U<<ch_num)) {
Expand All @@ -221,11 +230,7 @@ void SRV_Channel::calc_pwm(float output_scaled)
return;
}

if (type_angle) {
output_pwm = pwm_from_angle(output_scaled);
} else {
output_pwm = pwm_from_range(output_scaled);
}
output_pwm = pwm_from_scaled_value(output_scaled);
}

void SRV_Channel::set_output_pwm(uint16_t pwm, bool force)
Expand All @@ -240,11 +245,7 @@ void SRV_Channel::set_output_pwm(uint16_t pwm, bool force)
void SRV_Channel::set_output_norm(float value)
{
// convert normalised value to pwm
if (type_angle) {
set_output_pwm(pwm_from_angle(value * high_out));
} else {
set_output_pwm(pwm_from_range(value * high_out));
}
set_output_pwm(pwm_from_scaled_value(value * high_out));
}

// set angular range of scaled output
Expand Down
3 changes: 3 additions & 0 deletions libraries/SRV_Channel/SRV_Channel.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -280,6 +280,9 @@ class SRV_Channel {
return function.configured();
}

// convert a scaled value (either range or angle depending on setup) to a pwm
uint16_t pwm_from_scaled_value(float scaled_value) const;

// specify that small rc input changes should be ignored during passthrough
// used by DO_SET_SERVO commands
void ignore_small_rcin_changes() { ign_small_rcin_changes = true; }
Expand Down