AP_BLHeli: added SERVO_BLH_REMASK for reversible motors

this allows the user to specify which motors are configured as
reversible (3D)
This commit is contained in:
Andrew Tridgell 2018-11-09 21:25:00 +11:00
parent adf7fefc2e
commit 1910f266a6
2 changed files with 22 additions and 13 deletions

View File

@ -100,14 +100,21 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = {
// @Values: 0:Console,1:Telem1,2:Telem2,3:Telem3,4:Telem4,5:Telem5 // @Values: 0:Console,1:Telem1,2:Telem2,3:Telem3,4:Telem4,5:Telem5
// @User: Advanced // @User: Advanced
AP_GROUPINFO("PORT", 8, AP_BLHeli, control_port, 0), AP_GROUPINFO("PORT", 8, AP_BLHeli, control_port, 0),
// @Param: POLES // @Param: POLES
// @DisplayName: Motor Poles // @DisplayName: Motor Poles
// @Description: This allows calculation of true RPM from ESC's eRPM. The default is 14. // @Description: This allows calculation of true RPM from ESC's eRPM. The default is 14.
// @Range: 1 127 // @Range: 1 127
// @User: Advanced // @User: Advanced
AP_GROUPINFO("POLES", 9, AP_BLHeli, motor_poles, 14), AP_GROUPINFO("POLES", 9, AP_BLHeli, motor_poles, 14),
// @Param: REMASK
// @DisplayName: Channel Reversible Bitmask
// @Description: Mask of channels which are reversible. This is used for ESCs which have been configured in '3D' mode, allowing for the motor to spin in either direction
// @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16
// @User: Advanced
AP_GROUPINFO("REMASK", 10, AP_BLHeli, channel_reversible_mask, 0),
AP_GROUPEND AP_GROUPEND
}; };
@ -387,13 +394,10 @@ void AP_BLHeli::msp_process_command(void)
case MSP_MOTOR_CONFIG: { case MSP_MOTOR_CONFIG: {
debug("MSP_MOTOR_CONFIG"); debug("MSP_MOTOR_CONFIG");
uint16_t min_pwm = 1000;
uint16_t max_pwm = 2000;
hal.rcout->get_esc_scaling(min_pwm, max_pwm);
uint8_t buf[6]; uint8_t buf[6];
putU16(&buf[0], min_pwm+30); // min throttle putU16(&buf[0], 1030); // min throttle
putU16(&buf[2], max_pwm); // max throttle putU16(&buf[2], 2000); // max throttle
putU16(&buf[4], min_pwm); // min command putU16(&buf[4], 1000); // min command
msp_send_reply(msp.cmdMSP, buf, sizeof(buf)); msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
break; break;
} }
@ -402,9 +406,6 @@ void AP_BLHeli::msp_process_command(void)
debug("MSP_MOTOR"); debug("MSP_MOTOR");
// get the output going to each motor // get the output going to each motor
uint8_t buf[16] {}; uint8_t buf[16] {};
uint16_t min_pwm = 1000;
uint16_t max_pwm = 2000;
hal.rcout->get_esc_scaling(min_pwm, max_pwm);
for (uint8_t i = 0; i < num_motors; i++) { for (uint8_t i = 0; i < num_motors; i++) {
uint16_t v = hal.rcout->read(motor_map[i]); uint16_t v = hal.rcout->read(motor_map[i]);
putU16(&buf[2*i], v); putU16(&buf[2*i], v);
@ -427,7 +428,9 @@ void AP_BLHeli::msp_process_command(void)
} }
uint16_t v = getU16(&msp.buf[i*2]); uint16_t v = getU16(&msp.buf[i*2]);
debug("MSP_SET_MOTOR %u %u", i, v); debug("MSP_SET_MOTOR %u %u", i, v);
hal.rcout->write(motor_map[i], v); // map from a MSP value to a value in the range 1000 to 2000
uint16_t pwm = (v < 1000)?0:v;
hal.rcout->write(motor_map[i], pwm);
} }
hal.rcout->push(); hal.rcout->push();
break; break;
@ -1255,7 +1258,12 @@ void AP_BLHeli::update(void)
} }
} }
#endif #endif
// tell SRV_Channels about ESC capabilities
SRV_Channels::set_digital_mask(mask);
SRV_Channels::set_reversible_mask(uint16_t(channel_reversible_mask.get()) & mask);
hal.rcout->set_reversible_mask(channel_reversible_mask.get() & mask);
// add motors from channel mask // add motors from channel mask
for (uint8_t i=0; i<16 && num_motors < max_motors; i++) { for (uint8_t i=0; i<16 && num_motors < max_motors; i++) {
if (mask & (1U<<i)) { if (mask & (1U<<i)) {

View File

@ -70,6 +70,7 @@ private:
// mask of channels to use for BLHeli protocol // mask of channels to use for BLHeli protocol
AP_Int32 channel_mask; AP_Int32 channel_mask;
AP_Int32 channel_reversible_mask;
AP_Int8 channel_auto; AP_Int8 channel_auto;
AP_Int8 run_test; AP_Int8 run_test;
AP_Int16 timeout_sec; AP_Int16 timeout_sec;