mirror of https://github.com/ArduPilot/ardupilot
115 lines
3.7 KiB
C++
115 lines
3.7 KiB
C++
#include "AP_HAL.h"
|
|
|
|
extern const AP_HAL::HAL &hal;
|
|
|
|
uint32_t AP_HAL::RCOutput::DSHOT_BIT_WIDTH_TICKS = DSHOT_BIT_WIDTH_TICKS_DEFAULT;
|
|
uint32_t AP_HAL::RCOutput::DSHOT_BIT_0_TICKS = DSHOT_BIT_0_TICKS_DEFAULT;
|
|
uint32_t AP_HAL::RCOutput::DSHOT_BIT_1_TICKS = DSHOT_BIT_1_TICKS_DEFAULT;
|
|
|
|
// helper function for implementation of get_output_mode_banner
|
|
const char* AP_HAL::RCOutput::get_output_mode_string(enum output_mode out_mode) const
|
|
{
|
|
// convert mode to string
|
|
switch (out_mode) {
|
|
case MODE_PWM_NONE:
|
|
return "None";
|
|
case MODE_PWM_NORMAL:
|
|
return "PWM";
|
|
case MODE_PWM_ONESHOT:
|
|
return "OneS";
|
|
case MODE_PWM_ONESHOT125:
|
|
return "OS125";
|
|
case MODE_PWM_BRUSHED:
|
|
return "Brush";
|
|
case MODE_PWM_DSHOT150:
|
|
return "DS150";
|
|
case MODE_PWM_DSHOT300:
|
|
return "DS300";
|
|
case MODE_PWM_DSHOT600:
|
|
return "DS600";
|
|
case MODE_PWM_DSHOT1200:
|
|
return "DS1200";
|
|
case MODE_NEOPIXEL:
|
|
return "NeoP";
|
|
case MODE_PROFILED:
|
|
return "ProfiLED";
|
|
}
|
|
|
|
// we should never reach here but just in case
|
|
return "Unknown";
|
|
}
|
|
|
|
// convert output mode to string. helper function for implementation of get_output_mode_banner
|
|
void AP_HAL::RCOutput::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* mode_str = get_output_mode_string(out_mode);
|
|
|
|
// make copy of banner_msg
|
|
char banner_msg_temp[banner_msg_len];
|
|
memcpy(banner_msg_temp, banner_msg, banner_msg_len);
|
|
|
|
if (low_ch == high_ch) {
|
|
// handle single channel case
|
|
hal.util->snprintf(banner_msg, banner_msg_len, "%s %s:%u", banner_msg_temp, mode_str, (unsigned)low_ch);
|
|
} else {
|
|
// the general case
|
|
hal.util->snprintf(banner_msg, banner_msg_len, "%s %s:%u-%u", banner_msg_temp, mode_str, (unsigned)low_ch, (unsigned)high_ch);
|
|
}
|
|
}
|
|
|
|
/*
|
|
true when the output mode is of type dshot
|
|
*/
|
|
bool AP_HAL::RCOutput::is_dshot_protocol(const enum output_mode mode)
|
|
{
|
|
switch (mode) {
|
|
case MODE_PWM_DSHOT150:
|
|
case MODE_PWM_DSHOT300:
|
|
case MODE_PWM_DSHOT600:
|
|
case MODE_PWM_DSHOT1200:
|
|
return true;
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* calculate the prescaler required to achieve the desire bitrate
|
|
*/
|
|
uint32_t AP_HAL::RCOutput::calculate_bitrate_prescaler(uint32_t timer_clock, uint32_t target_frequency, bool is_dshot)
|
|
{
|
|
uint32_t prescaler;
|
|
if (is_dshot) {
|
|
// original prescaler calculation from betaflight. bi-dir dshot is incredibly sensitive to the bitrate
|
|
prescaler = uint32_t(lrintf((float) timer_clock / target_frequency + 0.01f) - 1);
|
|
} else {
|
|
// adjust frequency to give an allowed value given the clock, erring on the high side
|
|
prescaler = timer_clock / target_frequency;
|
|
while ((timer_clock / prescaler) < target_frequency && prescaler > 1) {
|
|
prescaler--;
|
|
}
|
|
}
|
|
|
|
const uint32_t freq = timer_clock / prescaler;
|
|
// if using dshot then always pick the high value. choosing low seems to not agree with some
|
|
// ESCs despite the fact that BLHeli32 is supposed not to care what the bitrate is
|
|
if (is_dshot) {
|
|
if (freq < target_frequency) {
|
|
prescaler--;
|
|
}
|
|
} else {
|
|
// find the closest value
|
|
const float delta = fabsf(float(freq) - target_frequency);
|
|
if (freq > target_frequency
|
|
&& delta > fabsf(float(timer_clock / (prescaler+1)) - target_frequency)) {
|
|
prescaler++;
|
|
} else if (freq < target_frequency
|
|
&& delta > fabsf(float(timer_clock / (prescaler-1)) - target_frequency)) {
|
|
prescaler--;
|
|
}
|
|
}
|
|
|
|
return prescaler;
|
|
}
|
|
|