aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_DroneCAN: allow sending negative RawCommands to ESCs

Browse Source
This commit is contained in:
Fred Kummer 2023-07-19 15:08:39 -04:00 committed by Peter Barker
parent 9fb872cdd7
commit 7c6551af9b
2 changed files with 27 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_DroneCAN/AP_DroneCAN.cpp
View File

@ -141,6 +141,13 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = {
// @Range: 1024 16384
// @User: Advanced
AP_GROUPINFO("POOL", 8, AP_DroneCAN, _pool_size, DRONECAN_NODE_POOL_SIZE),
// @Param: ESC_RV
// @DisplayName: Bitmask for output channels for reversible ESCs over DroneCAN.
// @Description: Bitmask with one set for each output channel that uses a reversible ESC over DroneCAN. Reversible ESCs use both positive and negative values in RawCommands, with positive commanding the forward direction and negative commanding the reverse direction.
// @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16, 16: ESC 17, 17: ESC 18, 18: ESC 19, 19: ESC 20, 20: ESC 21, 21: ESC 22, 22: ESC 23, 23: ESC 24, 24: ESC 25, 25: ESC 26, 26: ESC 27, 27: ESC 28, 28: ESC 29, 29: ESC 30, 30: ESC 31, 31: ESC 32
// @User: Advanced
AP_GROUPINFO("ESC_RV", 9, AP_DroneCAN, _esc_rv, 0),
AP_GROUPEND
};
@ -540,6 +547,20 @@ void AP_DroneCAN::handle_node_info_request(const CanardRxTransfer& transfer, con
node_info_server.respond(transfer, node_info_rsp);
}
int16_t AP_DroneCAN::scale_esc_output(uint8_t idx){
static const int16_t cmd_max = ((1<<13)-1);
float scaled = 0;
//Check if this channel has a reversible ESC. If it does, we can send negative commands.
if ((((uint32_t) 1) << idx) & _esc_rv) {
scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[idx].pulse));
} else {
scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[idx].pulse) + 1.0) / 2.0;
scaled = constrain_float(scaled, 0, cmd_max);
}
return static_cast<int16_t>(scaled);
}
///// SRV output /////
@ -633,7 +654,6 @@ void AP_DroneCAN::SRV_send_himark(void)
void AP_DroneCAN::SRV_send_esc(void)
{
static const int cmd_max = ((1<<13)-1);
uavcan_equipment_esc_RawCommand esc_msg;
uint8_t active_esc_num = 0, max_esc_num = 0;
@ -658,12 +678,7 @@ void AP_DroneCAN::SRV_send_esc(void)
for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) {
if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
// TODO: ESC negative scaling for reverse thrust and reverse rotation
float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[i].pulse) + 1.0) / 2.0;
scaled = constrain_float(scaled, 0, cmd_max);
esc_msg.cmd.data[k] = static_cast<int>(scaled);
esc_msg.cmd.data[k] = scale_esc_output(i);
} else {
esc_msg.cmd.data[k] = static_cast<unsigned>(0);
}
@ -692,7 +707,6 @@ void AP_DroneCAN::SRV_send_esc(void)
*/
void AP_DroneCAN::SRV_send_esc_hobbywing(void)
{
static const int cmd_max = ((1<<13)-1);
com_hobbywing_esc_RawCommand esc_msg;
uint8_t active_esc_num = 0, max_esc_num = 0;
@ -717,12 +731,7 @@ void AP_DroneCAN::SRV_send_esc_hobbywing(void)
for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) {
if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
// TODO: ESC negative scaling for reverse thrust and reverse rotation
float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[i].pulse) + 1.0) / 2.0;
scaled = constrain_float(scaled, 0, cmd_max);
esc_msg.command.data[k] = static_cast<int>(scaled);
esc_msg.command.data[k] = scale_esc_output(i);
} else {
esc_msg.command.data[k] = static_cast<unsigned>(0);
}

4
libraries/AP_DroneCAN/AP_DroneCAN.h
View File

@ -147,6 +147,9 @@ private:
void SRV_send_esc();
void SRV_send_himark();
//scale servo output appropriately before sending
int16_t scale_esc_output(uint8_t idx);
// SafetyState
void safety_state_send();
@ -188,6 +191,7 @@ private:
AP_Int16 _options;
AP_Int16 _notify_state_hz;
AP_Int16 _pool_size;
AP_Int32 _esc_rv;
uint32_t *mem_pool;