mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_HAL: move ESC scaling up to AP_HAL level
This commit is contained in:
parent
19ce28a49d
commit
64040bfab4
@ -114,3 +114,11 @@ uint32_t AP_HAL::RCOutput::calculate_bitrate_prescaler(uint32_t timer_clock, uin
|
||||
return prescaler;
|
||||
}
|
||||
|
||||
/*
|
||||
returns the pwm value scaled to [-1;1] regrading to set_esc_scaling ranges range without constraints.
|
||||
*/
|
||||
float AP_HAL::RCOutput::scale_esc_to_unity(uint16_t pwm) const
|
||||
{
|
||||
return 2.0 * ((float) pwm - _esc_pwm_min) / (_esc_pwm_max - _esc_pwm_min) - 1.0;
|
||||
}
|
||||
|
||||
|
@ -139,17 +139,15 @@ public:
|
||||
microseconds, and represent minimum and maximum PWM values which
|
||||
will be used to convert channel writes into a percentage
|
||||
*/
|
||||
virtual void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) {}
|
||||
|
||||
/*
|
||||
return ESC scaling value from set_esc_scaling()
|
||||
*/
|
||||
virtual bool get_esc_scaling(uint16_t &min_pwm, uint16_t &max_pwm) { return false; }
|
||||
void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) {
|
||||
_esc_pwm_min = min_pwm;
|
||||
_esc_pwm_max = max_pwm;
|
||||
}
|
||||
|
||||
/*
|
||||
returns the pwm value scaled to [-1;1] regrading to set_esc_scaling ranges range without constraints.
|
||||
*/
|
||||
virtual float scale_esc_to_unity(uint16_t pwm) { return 0; }
|
||||
float scale_esc_to_unity(uint16_t pwm) const;
|
||||
|
||||
/*
|
||||
return the erpm and error rate for a channel if available
|
||||
@ -415,4 +413,7 @@ protected:
|
||||
// helper functions for implementation of get_output_mode_banner
|
||||
void append_to_banner(char banner_msg[], uint8_t banner_msg_len, output_mode out_mode, uint8_t low_ch, uint8_t high_ch) const;
|
||||
const char* get_output_mode_string(enum output_mode out_mode) const;
|
||||
|
||||
uint16_t _esc_pwm_min;
|
||||
uint16_t _esc_pwm_max;
|
||||
};
|
||||
|
@ -66,11 +66,6 @@ public:
|
||||
uint16_t read(uint8_t ch) override;
|
||||
void read(uint16_t *period_us, uint8_t len) override;
|
||||
|
||||
void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override {
|
||||
_esc_pwm_min = min_pwm;
|
||||
_esc_pwm_max = max_pwm;
|
||||
}
|
||||
|
||||
void cork() override;
|
||||
void push() override;
|
||||
|
||||
@ -92,8 +87,6 @@ private:
|
||||
uint8_t _channels_count = MAX_MOTORS;
|
||||
|
||||
uint16_t _period[MAX_MOTORS];
|
||||
uint16_t _esc_pwm_min;
|
||||
uint16_t _esc_pwm_max;
|
||||
uint32_t _last_led_update_msec;
|
||||
int _uart_fd = -1;
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user