Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add 60k RPM support to voxl-esc driver #49

Merged
merged 22 commits into from
Dec 8, 2023
Merged

Add 60k RPM support to voxl-esc driver #49

Show file tree
Hide file tree
Changes from 19 commits
Commits
Show all changes
22 commits
Select commit Hold shift + click to select a range
4495820
init commit, version check working, ESC arm fails
modaljc Dec 5, 2023
8d44d11
update packet size
modaljc Dec 5, 2023
92aa25f
fix extended rpm packet packing function
modaljc Dec 5, 2023
768a105
update data types for rpm packets
modaljc Dec 5, 2023
1e9665d
replace magic number, clean up code
modaljc Dec 5, 2023
d53dc5d
clean up code
modaljc Dec 5, 2023
917ea36
fix actuator tab, add voxl2_io default params
modaljc Dec 6, 2023
7a7674e
Merge branch 'voxl-dev' of https://github.com/modalai/px4-firmware in…
modaljc Dec 6, 2023
e24a6a8
init commit, version check working, ESC arm fails
modaljc Dec 5, 2023
1d829d9
update packet size
modaljc Dec 5, 2023
c162974
fix extended rpm packet packing function
modaljc Dec 5, 2023
535047d
update data types for rpm packets
modaljc Dec 5, 2023
c5450e3
replace magic number, clean up code
modaljc Dec 5, 2023
8304412
clean up code
modaljc Dec 5, 2023
8c03952
fix actuator tab, add voxl2_io default params
modaljc Dec 6, 2023
e4d1d36
Merge branch '60k-RPM' of https://github.com/modalai/px4-firmware int…
modaljc Dec 6, 2023
4d77eb7
Update packet packing functions
modaljc Dec 7, 2023
9667d79
Comment out debug messages
modaljc Dec 7, 2023
78b8130
Merge branch 'voxl-dev' of https://github.com/modalai/px4-firmware in…
modaljc Dec 7, 2023
dcb019e
Merge branch 'voxl-dev' of https://github.com/modalai/px4-firmware in…
modaljc Dec 7, 2023
4d4732e
remove magic numbers, add rpm max variables
modaljc Dec 7, 2023
3dd7d21
Update range checking, remove casting
modaljc Dec 7, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions boards/modalai/voxl2/default.px4board
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ CONFIG_DRIVERS_RC_INPUT=y
CONFIG_DRIVERS_QSHELL_POSIX=y
CONFIG_DRIVERS_OSD_MSP_OSD=y
CONFIG_DRIVERS_VOXL2_IO=y
CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
CONFIG_DRIVERS_IMU_SERVER=y
CONFIG_SYSTEMCMDS_PERF=y
CONFIG_SYSTEMCMDS_UORB=y
Expand Down
10 changes: 10 additions & 0 deletions boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,16 @@ param set VOXL_ESC_BAUD 2000000
param set VOXL_ESC_RPM_MAX 10500
param set VOXL_ESC_RPM_MIN 1000

# Set the Voxl2 IO outputs to function as motors
param set VOXL2_IO_FUNC1 101
param set VOXL2_IO_FUNC2 102
param set VOXL2_IO_FUNC3 103
param set VOXL2_IO_FUNC4 104

param set VOXL2_IO_BAUD 921600
param set VOXL2_IO_MIN 1000
param set VOXL2_IO_MAX 2000

# Some parameters for control allocation
param set CA_ROTOR_COUNT 4
param set CA_R_REV 0
Expand Down
46 changes: 30 additions & 16 deletions src/drivers/actuators/voxl_esc/qc_esc_packet.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,36 +119,50 @@ int32_t qc_esc_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, i
}


int32_t qc_esc_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3,
int32_t qc_esc_create_rpm_packet4(int32_t rpm0, int32_t rpm1, int32_t rpm2, int32_t rpm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
uint8_t *out, uint16_t out_size)
uint8_t *out, uint16_t out_size, uint8_t ext_rpm)
{
return qc_esc_create_rpm_packet4_fb(rpm0, rpm1, rpm2, rpm3, led0, led1, led2, led3, -1, out, out_size);
return qc_esc_create_rpm_packet4_fb(rpm0, rpm1, rpm2, rpm3, led0, led1, led2, led3, -1, out, out_size, ext_rpm);
}

int32_t qc_esc_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3,
int32_t qc_esc_create_rpm_packet4_fb(int32_t rpm0, int32_t rpm1, int32_t rpm2, int32_t rpm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
int32_t fb_id, uint8_t *out, uint16_t out_size)
int32_t fb_id, uint8_t *out, uint16_t out_size, uint8_t ext_rpm)
{
uint16_t data[5];
int16_t data[5];
uint16_t leds = 0;
uint8_t cmd = ESC_PACKET_TYPE_RPM_CMD;

// Limit RPMs to prevent overflow when converting to int16_t
if (rpm0 > UINT16_MAX) { rpm0 = UINT16_MAX-5; } if (rpm1 > UINT16_MAX) { rpm1 = UINT16_MAX-5; }
if (rpm2 > UINT16_MAX) { rpm2 = UINT16_MAX-5; } if (rpm3 > UINT16_MAX) { rpm3 = UINT16_MAX-5; }
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is confusing to me. The comment says it is trying to prevent overflow of int16_t. So why would UINT16_MAX-5 accomplish that?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

When using extended range rpm command we divide the value passed in by 2 before sending to the ESC. Requested rpms passed in can theoretically be larger than UINT16_MAX since they are int32_t (RPMs can be negative), which would mean that it's possible when we divide the value for extended range packet that it is still larger than UINT16_MAX, which would cause overflow when assigning the value to the data array (int16_t). I.e. if a value of 2,000,000 is passed in, the divided value is 1,000,000 which is still above UINT16_MAX but when assigned to the int16_t data variable it would be truncated to 16960.

After thinking about it more, I think it might be a good idea to add some range limiting to the negative end of the range as well as maybe limit the standard range too since the input is int32_t... or we could leave it up to the user to input the correct values... thoughts?

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Let's let Alex submit his comments to see what his ideas are.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

From Alex:
in the limit values for regular (not extended rpm) the limits should be +-/32k (2^15), not UINT16_MAX . For the extended rpm range, the limit will be +/- 65K (but lets use slightly smaller limit as you already started doing)

I updated this in my latest commit (3dd7d21). I added max and min for both the standard and extended range... -32k to 32k and -65k to 65k, respectively


if (fb_id != -1) { fb_id = fb_id % 4; }

//least significant bit is used for feedback request
rpm0 &= ~(0x0001); rpm1 &= ~(0x0001); rpm2 &= ~(0x0001); rpm3 &= ~(0x0001);

if (fb_id == 0) { rpm0 |= 0x0001; } if (fb_id == 1) { rpm1 |= 0x0001; }

if (fb_id == 2) { rpm2 |= 0x0001; } if (fb_id == 3) { rpm3 |= 0x0001; }

leds |= led0 & 0b00000111;
leds |= led0 & 0b00000111;
leds |= (led1 & 0b00000111) << 3;
leds |= ((uint16_t)(led2 & 0b00000111)) << 6;
leds |= ((uint16_t)(led3 & 0b00000111)) << 9;

data[0] = rpm0; data[1] = rpm1; data[2] = rpm2; data[3] = rpm3; data[4] = leds;
return qc_esc_create_packet(ESC_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size);
if (ext_rpm > 0){
cmd = ESC_PACKET_TYPE_RPM_DIV2_CMD;
data[0] = (int16_t)(((double)rpm0 / 4) * 2);
data[1] = (int16_t)(((double)rpm1 / 4) * 2);
data[2] = (int16_t)(((double)rpm2 / 4) * 2);
data[3] = (int16_t)(((double)rpm3 / 4) * 2);
data[4]= leds;
} else {
data[0] = rpm0; data[1] = rpm1; data[2] = rpm2; data[3] = rpm3; data[4] = leds;
}

//least significant bit is used for feedback request
data[0] &= ~(0x0001); data[1] &= ~(0x0001); data[2] &= ~(0x0001); data[3] &= ~(0x0001);

if (fb_id == 0) { data[0] |= 0x0001; } if (fb_id == 1) { data[1] |= 0x0001; }
if (fb_id == 2) { data[2] |= 0x0001; } if (fb_id == 3) { data[3] |= 0x0001; }

return qc_esc_create_packet(cmd, (uint8_t *) & (data[0]), 10, out, out_size);
}

int32_t qc_esc_create_packet(uint8_t type, uint8_t *data, uint16_t size, uint8_t *out, uint16_t out_size)
Expand Down
10 changes: 5 additions & 5 deletions src/drivers/actuators/voxl_esc/qc_esc_packet.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -210,15 +210,15 @@ int32_t qc_esc_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, i

// Create a packet for sending closed-loop RPM command and LED command to 4 ESCs without requesting any feedback
// Return value is the length of generated packet (if positive), otherwise error code
int32_t qc_esc_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3,
int32_t qc_esc_create_rpm_packet4(int32_t rpm0, int32_t rpm1, int32_t rpm2, int32_t rpm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
uint8_t *out, uint16_t out_size);
uint8_t *out, uint16_t out_size, uint8_t ext_rpm);

// Create a packet for sending closed-loop RPM command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id)
// Create a packet for sending closed-loop RPM command (32766 or 65530 max RPM) and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id)
// Return value is the length of generated packet (if positive), otherwise error code
int32_t qc_esc_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3,
int32_t qc_esc_create_rpm_packet4_fb(int32_t rpm0, int32_t rpm1, int32_t rpm2, int32_t rpm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
int32_t fb_id, uint8_t *out, uint16_t out_size);
int32_t fb_id, uint8_t *out, uint16_t out_size, uint8_t ext_rpm);


//-------------------------------------------------------------------------
Expand Down
1 change: 1 addition & 0 deletions src/drivers/actuators/voxl_esc/qc_esc_packet_types.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@
#define ESC_PACKET_TYPE_SOUND_CMD 3
#define ESC_PACKET_TYPE_STEP_CMD 4
#define ESC_PACKET_TYPE_LED_CMD 5
#define ESC_PACKET_TYPE_RPM_DIV2_CMD 7
#define ESC_PACKET_TYPE_RESET_CMD 10
#define ESC_PACKET_TYPE_SET_ID_CMD 11
#define ESC_PACKET_TYPE_SET_DIR_CMD 12
Expand Down
75 changes: 60 additions & 15 deletions src/drivers/actuators/voxl_esc/voxl_esc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,6 +104,11 @@ int VoxlEsc::init()

_uart_port = new VoxlEscSerial();
memset(&_esc_chans, 0x00, sizeof(_esc_chans));
for (int esc_id=0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id){
_version_info[esc_id].sw_version = UINT16_MAX;
_version_info[esc_id].hw_version = UINT16_MAX;
_version_info[esc_id].id = esc_id;
}

//get_instance()->ScheduleOnInterval(10000); //100hz

Expand Down Expand Up @@ -265,6 +270,20 @@ int VoxlEsc::flush_uart_rx()
return 0;
}

bool VoxlEsc::check_versions_updated(){
for (int esc_id=0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id){
if (_version_info[esc_id].sw_version == UINT16_MAX) return false;
}

// PX4_INFO("Got all ESC Version info!");
_extended_rpm = true;
_need_version_info = false;
for (int esc_id=0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id){
if (_version_info[esc_id].sw_version < VOXL_ESC_EXT_RPM) _extended_rpm = false;
}
return true;
}

int VoxlEsc::read_response(Command *out_cmd)
{
px4_usleep(_current_cmd.resp_delay_us);
Expand Down Expand Up @@ -302,7 +321,7 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
uint8_t packet_size = qc_esc_packet_get_size(&_fb_packet);

if (packet_type == ESC_PACKET_TYPE_FB_RESPONSE && packet_size == sizeof(QC_ESC_FB_RESPONSE_V2)) {
//PX4_INFO("Got feedback V2 packet!");
// PX4_INFO("Got feedback V2 packet!");
QC_ESC_FB_RESPONSE_V2 fb;
memcpy(&fb, _fb_packet.buffer, packet_size);

Expand Down Expand Up @@ -373,6 +392,11 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
else if (packet_type == ESC_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(QC_ESC_VERSION_INFO)) {
QC_ESC_VERSION_INFO ver;
memcpy(&ver, _fb_packet.buffer, packet_size);
if (_need_version_info){
memcpy(&_version_info[ver.id], &ver, sizeof(QC_ESC_VERSION_INFO));
check_versions_updated();
break;
}
PX4_INFO("ESC ID: %i", ver.id);
PX4_INFO("HW Version: %i", ver.hw_version);
PX4_INFO("SW Version: %i", ver.sw_version);
Expand Down Expand Up @@ -417,11 +441,11 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
switch (ret) {
case ESC_ERROR_BAD_CHECKSUM:
_rx_crc_error_count++;
//PX4_INFO("BAD ESC packet checksum");
// PX4_INFO("BAD ESC packet checksum");
break;

case ESC_ERROR_BAD_LENGTH:
//PX4_INFO("BAD ESC packet length");
// PX4_INFO("BAD ESC packet length");
break;
}
}
Expand Down Expand Up @@ -652,7 +676,8 @@ int VoxlEsc::custom_command(int argc, char *argv[])
0,
id_fb,
cmd.buf,
sizeof(cmd.buf));
sizeof(cmd.buf),
get_instance()->_extended_rpm);

cmd.response = true;
cmd.repeats = repeat_count;
Expand Down Expand Up @@ -1038,6 +1063,12 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
_esc_chans[i].rate_req = 0;

} else {
if (_extended_rpm) {
if (outputs[i] > 65530) outputs[i] = 65530;
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Perhaps a constant definition of 65530 and 32766 with some comments about why those values were chosen

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I will make this happen

} else {
if (outputs[i] > 32766) outputs[i] = 32766;
}

if (!_turtle_mode_en) {
_esc_chans[i].rate_req = outputs[i] * _output_map[i].direction;

Expand All @@ -1047,20 +1078,21 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
}
}
}


Command cmd;
cmd.len = qc_esc_create_rpm_packet4_fb(_esc_chans[0].rate_req,
_esc_chans[1].rate_req,
_esc_chans[2].rate_req,
_esc_chans[3].rate_req,
_esc_chans[0].led,
_esc_chans[1].led,
_esc_chans[2].led,
_esc_chans[3].led,
_fb_idx,
cmd.buf,
sizeof(cmd.buf));

_esc_chans[1].rate_req,
_esc_chans[2].rate_req,
_esc_chans[3].rate_req,
_esc_chans[0].led,
_esc_chans[1].led,
_esc_chans[2].led,
_esc_chans[3].led,
_fb_idx,
cmd.buf,
sizeof(cmd.buf),
_extended_rpm);

if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
PX4_ERR("Failed to send packet");
Expand Down Expand Up @@ -1148,6 +1180,19 @@ void VoxlEsc::Run()
}
}

/* Get ESC FW version info */
if (_need_version_info){
for (uint8_t esc_id=0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id){
Command cmd;
cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
if (_uart_port->uart_write(cmd.buf, cmd.len) == cmd.len) {
if (read_response(&_current_cmd) != 0) PX4_ERR("Failed to parse version request response packet!");
} else {
PX4_ERR("Failed to send version request packet!");
}
}
}

_mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module

/* update output status if armed */
Expand Down
9 changes: 8 additions & 1 deletion src/drivers/actuators/voxl_esc/voxl_esc.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,8 @@ class VoxlEsc : public ModuleBase<VoxlEsc>, public OutputModuleInterface
static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX1 = 1;
static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX2 = 2;

static constexpr uint16_t VOXL_ESC_EXT_RPM = 39;

//static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, VOXL_ESC_PWM_MAX); }
//static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, VOXL_ESC_RPM_MAX); }

Expand All @@ -149,7 +151,7 @@ class VoxlEsc : public ModuleBase<VoxlEsc>, public OutputModuleInterface
} voxl_esc_params_t;

struct EscChan {
int16_t rate_req;
int32_t rate_req;
uint8_t state;
uint16_t rate_meas;
uint8_t power_applied;
Expand Down Expand Up @@ -195,6 +197,11 @@ class VoxlEsc : public ModuleBase<VoxlEsc>, public OutputModuleInterface
uORB::Publication<actuator_outputs_s> _outputs_debug_pub{ORB_ID(actuator_outputs_debug)};
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};

bool _extended_rpm{false};
bool _need_version_info{true};
QC_ESC_VERSION_INFO _version_info[4];
bool check_versions_updated();

voxl_esc_params_t _parameters;
int update_params();
int load_params(voxl_esc_params_t *params, ch_assign_t *map);
Expand Down
Loading