diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp index c63abde62c..9398e861be 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp @@ -11,6 +11,7 @@ #include #include #include "RCOutput_Bebop.h" +#include "Util.h" /* BEBOP BLDC motor controller address and registers description */ #define BEBOP_BLDC_I2C_ADDR 0x08 @@ -51,7 +52,9 @@ struct bldc_obs_data { #define BEBOP_BLDC_MIN_PERIOD_US 1100 #define BEBOP_BLDC_MAX_PERIOD_US 1900 #define BEBOP_BLDC_MIN_RPM 3000 -#define BEBOP_BLDC_MAX_RPM 11000 +/* the max rpm speed is different on Bebop 2 */ +#define BEBOP_BLDC_MAX_RPM_1 11000 +#define BEBOP_BLDC_MAX_RPM_2 12200 /* Priority of the thread controlling the BLDC via i2c * set to 14, which is the same as the UART @@ -226,7 +229,7 @@ uint16_t RCOutput_Bebop::_period_us_to_rpm(uint16_t period_us) float period_us_fl = period_us; float rpm_fl = (period_us_fl - _min_pwm)/(_max_pwm - _min_pwm) * - (BEBOP_BLDC_MAX_RPM - BEBOP_BLDC_MIN_RPM) + BEBOP_BLDC_MIN_RPM; + (_max_rpm - BEBOP_BLDC_MIN_RPM) + BEBOP_BLDC_MIN_RPM; return (uint16_t)rpm_fl; } @@ -370,6 +373,7 @@ void RCOutput_Bebop::_run_rcout() uint8_t bebop_bldc_channels[BEBOP_BLDC_MOTORS_NUM]; uint8_t bebop_bldc_right_front, bebop_bldc_left_front, bebop_bldc_left_back, bebop_bldc_right_back; + int hw_version; memset(current_period_us, 0, sizeof(current_period_us)); @@ -398,6 +402,17 @@ void RCOutput_Bebop::_run_rcout() bebop_bldc_channels[2] = bebop_bldc_left_front; bebop_bldc_channels[3] = bebop_bldc_right_back; + hw_version = Util::from(hal.util)->get_hw_arm32(); + if (hw_version == UTIL_HARDWARE_BEBOP) { + _max_rpm = BEBOP_BLDC_MAX_RPM_1; + } else if (hw_version == UTIL_HARDWARE_BEBOP2) { + _max_rpm = BEBOP_BLDC_MAX_RPM_2; + } else if (hw_version < 0) { + AP_HAL::panic("failed to get hw version %s", strerror(hw_version)); + } else { + AP_HAL::panic("unsupported hw version %d", hw_version); + } + while (true) { pthread_mutex_lock(&_mutex); ret = clock_gettime(CLOCK_MONOTONIC, &ts); diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.h b/libraries/AP_HAL_Linux/RCOutput_Bebop.h index 672e2f61eb..f5baceeee1 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Bebop.h +++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.h @@ -89,6 +89,7 @@ private: uint16_t _max_pwm; uint8_t _state; bool _corking = false; + uint16_t _max_rpm; uint8_t _checksum(uint8_t *data, unsigned int len); void _start_prop();