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

SRV_Channel: use floats for get/set output scaled

This commit is contained in:
Iampete1 2021-09-18 18:54:52 +01:00 committed by Andrew Tridgell
parent f178717212
commit b84633630a
3 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/SRV_Channel/SRV_Channel.cpp
View File

@ -84,33 +84,33 @@ SRV_Channel::SRV_Channel(void)
}
// convert a 0..range_max to a pwm
uint16_t SRV_Channel::pwm_from_range(int16_t scaled_value) const
uint16_t SRV_Channel::pwm_from_range(float scaled_value) const
{
if (servo_max <= servo_min || high_out == 0) {
return servo_min;
}
scaled_value = constrain_int16(scaled_value, 0, high_out);
scaled_value = constrain_float(scaled_value, 0, high_out);
if (reversed) {
scaled_value = high_out - scaled_value;
}
return servo_min + ((int32_t)scaled_value * (int32_t)(servo_max - servo_min)) / (int32_t)high_out;
return servo_min + uint16_t( (scaled_value * (float)(servo_max - servo_min)) / (float)high_out );
}
// convert a -angle_max..angle_max to a pwm
uint16_t SRV_Channel::pwm_from_angle(int16_t scaled_value) const
uint16_t SRV_Channel::pwm_from_angle(float scaled_value) const
{
if (reversed) {
scaled_value = -scaled_value;
}
scaled_value = constrain_int16(scaled_value, -high_out, high_out);
scaled_value = constrain_float(scaled_value, -high_out, high_out);
if (scaled_value > 0) {
return servo_trim + ((int32_t)scaled_value * (int32_t)(servo_max - servo_trim)) / (int32_t)high_out;
return servo_trim + uint16_t( (scaled_value * (float)(servo_max - servo_trim)) / (float)high_out);
} else {
return servo_trim - (-(int32_t)scaled_value * (int32_t)(servo_trim - servo_min)) / (int32_t)high_out;
return servo_trim - uint16_t( (-scaled_value * (float)(servo_trim - servo_min)) / (float)high_out);
}
}
void SRV_Channel::calc_pwm(int16_t output_scaled)
void SRV_Channel::calc_pwm(float output_scaled)
{
if (have_pwm_mask & (1U<<ch_num)) {
// Note that this allows a set_output_pwm call to override E-Stop!!
@ -121,7 +121,7 @@ void SRV_Channel::calc_pwm(int16_t output_scaled)
// check for E - stop
bool force = false;
if (SRV_Channel::should_e_stop(get_function()) && SRV_Channels::emergency_stop) {
output_scaled = 0;
output_scaled = 0.0;
force = true;
}

12
libraries/SRV_Channel/SRV_Channel.h
View File

@ -285,13 +285,13 @@ private:
uint16_t high_out;
// convert a 0..range_max to a pwm
uint16_t pwm_from_range(int16_t scaled_value) const;
uint16_t pwm_from_range(float scaled_value) const;
// convert a -angle_max..angle_max to a pwm
uint16_t pwm_from_angle(int16_t scaled_value) const;
uint16_t pwm_from_angle(float scaled_value) const;
// convert a scaled output to a pwm value
void calc_pwm(int16_t output_scaled);
void calc_pwm(float output_scaled);
// output value based on function
void output_ch(void);
@ -351,10 +351,10 @@ public:
// set output value for a function channel as a scaled value. This
// calls calc_pwm() to also set the pwm value
static void set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value);
static void set_output_scaled(SRV_Channel::Aux_servo_function_t function, float value);
// get scaled output for the given function type.
static int16_t get_output_scaled(SRV_Channel::Aux_servo_function_t function);
static float get_output_scaled(SRV_Channel::Aux_servo_function_t function);
// get pwm output for the first channel of the given function type.
static bool get_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t &value);
@ -593,7 +593,7 @@ private:
SRV_Channel::servo_mask_t channel_mask;
// scaled output for this function
int16_t output_scaled;
float output_scaled;
} functions[SRV_Channel::k_nr_aux_servo_functions];
AP_Int8 auto_trim;

4
libraries/SRV_Channel/SRV_Channel_aux.cpp
View File

@ -522,7 +522,7 @@ SRV_Channel *SRV_Channels::get_channel_for(SRV_Channel::Aux_servo_function_t fun
return &channels[chan];
}
void SRV_Channels::set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value)
void SRV_Channels::set_output_scaled(SRV_Channel::Aux_servo_function_t function, float value)
{
if (function < SRV_Channel::k_nr_aux_servo_functions) {
functions[function].output_scaled = value;
@ -530,7 +530,7 @@ void SRV_Channels::set_output_scaled(SRV_Channel::Aux_servo_function_t function,
}
}
int16_t SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t function)
float SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t function)
{
if (function < SRV_Channel::k_nr_aux_servo_functions) {
return functions[function].output_scaled;