fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 00:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

VOXL2 specific drivers, modules, and miscellaneous support files (#22588)

Browse Source
This commit is contained in:
Eric Katzfey
2024-01-18 09:14:17 -08:00
committed by GitHub
parent b60e73c76f
commit 2b69a3d290
96 changed files with 8266 additions and 678 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/actuators/modal_io/Kconfig
-5
View File
@@ -1,5 +0,0 @@
menuconfig DRIVERS_ACTUATORS_MODAL_IO
bool "modal_io"
default n
---help---
Enable support for modal_io
src/drivers/actuators/modal_io/module.yaml
-26
View File
@@ -1,26 +0,0 @@
module_name: MODAL IO Output
actuator_output:
show_subgroups_if: 'MODAL_IO_CONFIG>0'
config_parameters:
- param: 'MODAL_IO_CONFIG'
label: 'Configure'
function: 'enable'
- param: 'MODAL_IO_BAUD'
label: 'Bitrate'
- param: 'MODAL_IO_RPM_MIN'
label: 'RPM Min'
- param: 'MODAL_IO_RPM_MAX'
label: 'RPM Max'
- param: 'MODAL_IO_SDIR1'
label: 'ESC 1 Spin Direction'
- param: 'MODAL_IO_SDIR2'
label: 'ESC 2 Spin Direction'
- param: 'MODAL_IO_SDIR3'
label: 'ESC 3 Spin Direction'
- param: 'MODAL_IO_SDIR4'
label: 'ESC 4 Spin Direction'
output_groups:
- param_prefix: MODAL_IO
group_label: 'ESCs'
channel_label: 'ESC'
num_channels: 4
src/drivers/actuators/modal_io/CMakeLists.txt → src/drivers/actuators/voxl_esc/CMakeLists.txt
+7 -6
View File
@@ -32,20 +32,21 @@
############################################################################
px4_add_module(
MODULE drivers__actuators__modal_io
MAIN modal_io
MODULE drivers__actuators__voxl_esc
MAIN voxl_esc
SRCS
crc16.c
crc16.h
modal_io_serial.cpp
modal_io_serial.hpp
modal_io.cpp
modal_io.hpp
voxl_esc_serial.cpp
voxl_esc_serial.hpp
voxl_esc.cpp
voxl_esc.hpp
qc_esc_packet_types.h
qc_esc_packet.c
qc_esc_packet.h
DEPENDS
battery
px4_work_queue
mixer_module
MODULE_CONFIG
src/drivers/actuators/voxl_esc/Kconfig
+5
View File
@@ -0,0 +1,5 @@
menuconfig DRIVERS_ACTUATORS_VOXL_ESC
bool "voxl_esc"
default n
---help---
Enable support for voxl_esc
src/drivers/actuators/modal_io/crc16.c → src/drivers/actuators/voxl_esc/crc16.c
View File
src/drivers/actuators/modal_io/crc16.h → src/drivers/actuators/voxl_esc/crc16.h
View File
src/drivers/actuators/voxl_esc/module.yaml
+41
View File
@@ -0,0 +1,41 @@
module_name: VOXL ESC Output
actuator_output:
show_subgroups_if: 'VOXL_ESC_CONFIG>0'
config_parameters:
- param: 'VOXL_ESC_CONFIG'
label: 'Configure'
function: 'enable'
- param: 'VOXL_ESC_BAUD'
label: 'Bitrate'
- param: 'VOXL_ESC_RPM_MIN'
label: 'RPM Min'
- param: 'VOXL_ESC_RPM_MAX'
label: 'RPM Max'
- param: 'VOXL_ESC_SDIR1'
label: 'ESC 1 Spin Direction'
- param: 'VOXL_ESC_SDIR2'
label: 'ESC 2 Spin Direction'
- param: 'VOXL_ESC_SDIR3'
label: 'ESC 3 Spin Direction'
- param: 'VOXL_ESC_SDIR4'
label: 'ESC 4 Spin Direction'
output_groups:
- param_prefix: VOXL_ESC
group_label: 'ESCs'
channel_label: 'ESC'
num_channels: 4
parameters:
- group: ModalAI Custom Configuration
definitions:
MODALAI_CONFIG:
description:
short: Custom configuration for ModalAI drones
long: |
This can be set to indicate that drone behavior
needs to be changed to match a custom setting
type: int32
reboot_required: true
num_instances: 1
instance_start: 1
default: 0
src/drivers/actuators/modal_io/qc_esc_packet.c → src/drivers/actuators/voxl_esc/qc_esc_packet.c
+40 -16
View File
@@ -119,36 +119,60 @@ 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;
int32_t max = ext_rpm > 0 ? ESC_RPM_MAX_EXT : ESC_RPM_MAX;
int32_t min = ext_rpm > 0 ? ESC_RPM_MIN_EXT : ESC_RPM_MIN;
// Limit RPMs to prevent overflow when converting to int16_t
if (rpm0 > max) { rpm0 = max; } if (rpm0 < min) { rpm0 = min; }
if (rpm1 > max) { rpm1 = max; } if (rpm1 < min) { rpm1 = min; }
if (rpm2 > max) { rpm2 = max; } if (rpm2 < min) { rpm2 = min; }
if (rpm3 > max) { rpm3 = max; } if (rpm3 < min) { rpm3 = min; }
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] = ((rpm0 / 4) * 2);
data[1] = ((rpm1 / 4) * 2);
data[2] = ((rpm2 / 4) * 2);
data[3] = ((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)
src/drivers/actuators/modal_io/qc_esc_packet.h → src/drivers/actuators/voxl_esc/qc_esc_packet.h
+22 -5
View File
@@ -52,6 +52,11 @@ extern "C" {
#define QC_ESC_LED_GREEN_ON 2
#define QC_ESC_LED_BLUE_ON 4
// Define RPM command max and min values
#define ESC_RPM_MAX INT16_MAX-1 // 32k
#define ESC_RPM_MIN INT16_MIN+1 // -32k
#define ESC_RPM_MAX_EXT UINT16_MAX-5 // 65k
#define ESC_RPM_MIN_EXT -UINT16_MAX+5 // -65k
// Header of the packet. Each packet must start with this header
#define ESC_PACKET_HEADER 0xAF
@@ -142,6 +147,18 @@ typedef struct {
} __attribute__((__packed__)) QC_ESC_FB_RESPONSE_V2;
// Definition of the feedback response packet from ESC, which contains battery voltage and total current
typedef struct {
uint8_t header;
uint8_t length;
uint8_t type;
uint8_t id; //ESC Id (could be used as system ID elsewhere)
uint16_t voltage; //Input voltage (Millivolts)
int16_t current; //Total Current (8mA resolution)
uint16_t crc;
} __attribute__((__packed__)) QC_ESC_FB_POWER_STATUS;
//-------------------------------------------------------------------------
//Below are functions for generating packets that would be outgoing to ESCs
//-------------------------------------------------------------------------
@@ -197,15 +214,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);
//-------------------------------------------------------------------------
src/drivers/actuators/modal_io/qc_esc_packet_types.h → src/drivers/actuators/voxl_esc/qc_esc_packet_types.h
+2
View File
@@ -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
@@ -69,5 +70,6 @@
#define ESC_PACKET_TYPE_TUNE_CONFIG 114
#define ESC_PACKET_TYPE_FB_RESPONSE 128
#define ESC_PACKET_TYPE_VERSION_EXT_RESPONSE 131
#define ESC_PACKET_TYPE_FB_POWER_STATUS 132
#endif
src/drivers/actuators/modal_io/modal_io.cpp → src/drivers/actuators/voxl_esc/voxl_esc.cpp
+214 -251
View File
@@ -35,33 +35,32 @@
#include <px4_platform_common/getopt.h>
#include "modal_io.hpp"
#include "modal_io_serial.hpp"
// utility for running on VOXL and using driver as a bridge
#define MODAL_IO_VOXL_BRIDGE_PORT "/dev/ttyS4"
#include "voxl_esc.hpp"
#include "voxl_esc_serial.hpp"
// future use:
#define MODALAI_PUBLISH_ESC_STATUS 0
const char *_device;
ModalIo::ModalIo() :
OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(MODAL_IO_DEFAULT_PORT)),
VoxlEsc::VoxlEsc() :
OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(VOXL_ESC_DEFAULT_PORT)),
_mixing_output{"VOXL_ESC", VOXL_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false},
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
_output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval"))
_output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")),
_battery(1, nullptr, _battery_report_interval, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
{
_device = MODAL_IO_DEFAULT_PORT;
_device = VOXL_ESC_DEFAULT_PORT;
_mixing_output.setAllFailsafeValues(0);
_mixing_output.setAllDisarmedValues(0);
_esc_status.timestamp = hrt_absolute_time();
_esc_status.counter = 0;
_esc_status.esc_count = MODAL_IO_OUTPUT_CHANNELS;
_esc_status.esc_count = VOXL_ESC_OUTPUT_CHANNELS;
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL;
for (unsigned i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) {
for (unsigned i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) {
_esc_status.esc[i].timestamp = 0;
_esc_status.esc[i].esc_address = 0;
_esc_status.esc[i].esc_rpm = 0;
@@ -75,15 +74,12 @@ ModalIo::ModalIo() :
_esc_status.esc[i].esc_power = 0;
}
_esc_status_pub.advertise();
qc_esc_packet_init(&_fb_packet);
qc_esc_packet_init(&_uart_bridge_packet);
_fb_idx = 0;
}
ModalIo::~ModalIo()
VoxlEsc::~VoxlEsc()
{
_outputs_on = false;
@@ -92,16 +88,11 @@ ModalIo::~ModalIo()
_uart_port = nullptr;
}
if (_uart_port_bridge) {
_uart_port_bridge->uart_close();
_uart_port_bridge = nullptr;
}
perf_free(_cycle_perf);
perf_free(_output_update_perf);
}
int ModalIo::init()
int VoxlEsc::init()
{
/* Getting initial parameter values */
@@ -111,10 +102,15 @@ int ModalIo::init()
return ret;
}
_uart_port = new ModalIoSerial();
_uart_port_bridge = new ModalIoSerial();
_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
ScheduleNow();
@@ -122,81 +118,82 @@ int ModalIo::init()
return 0;
}
int ModalIo::load_params(modal_io_params_t *params, ch_assign_t *map)
int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map)
{
int ret = PX4_OK;
// initialize out
for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) {
for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) {
params->function_map[i] = (int)OutputFunction::Disabled;
params->direction_map[i] = 0;
params->motor_map[i] = 0;
}
param_get(param_find("MODAL_IO_CONFIG"), &params->config);
param_get(param_find("MODAL_IO_MODE"), &params->mode);
param_get(param_find("MODAL_IO_BAUD"), &params->baud_rate);
param_get(param_find("VOXL_ESC_CONFIG"), &params->config);
param_get(param_find("VOXL_ESC_MODE"), &params->mode);
param_get(param_find("VOXL_ESC_BAUD"), &params->baud_rate);
param_get(param_find("MODAL_IO_T_PERC"), &params->turtle_motor_percent);
param_get(param_find("MODAL_IO_T_DEAD"), &params->turtle_motor_deadband);
param_get(param_find("MODAL_IO_T_EXPO"), &params->turtle_motor_expo);
param_get(param_find("MODAL_IO_T_MINF"), &params->turtle_stick_minf);
param_get(param_find("MODAL_IO_T_COSP"), &params->turtle_cosphi);
param_get(param_find("VOXL_ESC_T_PERC"), &params->turtle_motor_percent);
param_get(param_find("VOXL_ESC_T_DEAD"), &params->turtle_motor_deadband);
param_get(param_find("VOXL_ESC_T_EXPO"), &params->turtle_motor_expo);
param_get(param_find("VOXL_ESC_T_MINF"), &params->turtle_stick_minf);
param_get(param_find("VOXL_ESC_T_COSP"), &params->turtle_cosphi);
param_get(param_find("MODAL_IO_FUNC1"), &params->function_map[0]);
param_get(param_find("MODAL_IO_FUNC2"), &params->function_map[1]);
param_get(param_find("MODAL_IO_FUNC3"), &params->function_map[2]);
param_get(param_find("MODAL_IO_FUNC4"), &params->function_map[3]);
param_get(param_find("VOXL_ESC_FUNC1"), &params->function_map[0]);
param_get(param_find("VOXL_ESC_FUNC2"), &params->function_map[1]);
param_get(param_find("VOXL_ESC_FUNC3"), &params->function_map[2]);
param_get(param_find("VOXL_ESC_FUNC4"), &params->function_map[3]);
param_get(param_find("MODAL_IO_SDIR1"), &params->direction_map[0]);
param_get(param_find("MODAL_IO_SDIR2"), &params->direction_map[1]);
param_get(param_find("MODAL_IO_SDIR3"), &params->direction_map[2]);
param_get(param_find("MODAL_IO_SDIR4"), &params->direction_map[3]);
param_get(param_find("VOXL_ESC_SDIR1"), &params->direction_map[0]);
param_get(param_find("VOXL_ESC_SDIR2"), &params->direction_map[1]);
param_get(param_find("VOXL_ESC_SDIR3"), &params->direction_map[2]);
param_get(param_find("VOXL_ESC_SDIR4"), &params->direction_map[3]);
param_get(param_find("MODAL_IO_RPM_MIN"), &params->rpm_min);
param_get(param_find("MODAL_IO_RPM_MAX"), &params->rpm_max);
param_get(param_find("VOXL_ESC_RPM_MIN"), &params->rpm_min);
param_get(param_find("VOXL_ESC_RPM_MAX"), &params->rpm_max);
param_get(param_find("MODAL_IO_VLOG"), &params->verbose_logging);
param_get(param_find("VOXL_ESC_VLOG"), &params->verbose_logging);
param_get(param_find("VOXL_ESC_PUB_BST"), &params->publish_battery_status);
if (params->rpm_min >= params->rpm_max) {
PX4_ERR("Invalid parameter MODAL_IO_RPM_MIN. Please verify parameters.");
PX4_ERR("Invalid parameter VOXL_ESC_RPM_MIN. Please verify parameters.");
params->rpm_min = 0;
ret = PX4_ERROR;
}
if (params->turtle_motor_percent < 0 || params->turtle_motor_percent > 100) {
PX4_ERR("Invalid parameter MODAL_IO_T_PERC. Please verify parameters.");
PX4_ERR("Invalid parameter VOXL_ESC_T_PERC. Please verify parameters.");
params->turtle_motor_percent = 0;
ret = PX4_ERROR;
}
if (params->turtle_motor_deadband < 0 || params->turtle_motor_deadband > 100) {
PX4_ERR("Invalid parameter MODAL_IO_T_DEAD. Please verify parameters.");
PX4_ERR("Invalid parameter VOXL_ESC_T_DEAD. Please verify parameters.");
params->turtle_motor_deadband = 0;
ret = PX4_ERROR;
}
if (params->turtle_motor_expo < 0 || params->turtle_motor_expo > 100) {
PX4_ERR("Invalid parameter MODAL_IO_T_EXPO. Please verify parameters.");
PX4_ERR("Invalid parameter VOXL_ESC_T_EXPO. Please verify parameters.");
params->turtle_motor_expo = 0;
ret = PX4_ERROR;
}
if (params->turtle_stick_minf < 0.0f || params->turtle_stick_minf > 100.0f) {
PX4_ERR("Invalid parameter MODAL_IO_T_MINF. Please verify parameters.");
PX4_ERR("Invalid parameter VOXL_ESC_T_MINF. Please verify parameters.");
params->turtle_stick_minf = 0.0f;
ret = PX4_ERROR;
}
if (params->turtle_cosphi < 0.0f || params->turtle_cosphi > 100.0f) {
PX4_ERR("Invalid parameter MODAL_IO_T_COSP. Please verify parameters.");
PX4_ERR("Invalid parameter VOXL_ESC_T_COSP. Please verify parameters.");
params->turtle_cosphi = 0.0f;
ret = PX4_ERROR;
}
for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) {
for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) {
if (params->function_map[i] < (int)OutputFunction::Motor1 || params->function_map[i] > (int)OutputFunction::Motor4) {
PX4_ERR("Invalid parameter MODAL_IO_FUNCX. Only supports motors 1-4. Please verify parameters.");
PX4_ERR("Invalid parameter VOXL_ESC_FUNCX. Only supports motors 1-4. Please verify parameters.");
params->function_map[i] = 0;
ret = PX4_ERROR;
@@ -210,11 +207,11 @@ int ModalIo::load_params(modal_io_params_t *params, ch_assign_t *map)
}
}
for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) {
if (params->motor_map[i] == MODAL_IO_OUTPUT_DISABLED ||
params->motor_map[i] < -(MODAL_IO_OUTPUT_CHANNELS) ||
params->motor_map[i] > MODAL_IO_OUTPUT_CHANNELS) {
PX4_ERR("Invalid parameter MODAL_IO_MOTORX. Please verify parameters.");
for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) {
if (params->motor_map[i] == VOXL_ESC_OUTPUT_DISABLED ||
params->motor_map[i] < -(VOXL_ESC_OUTPUT_CHANNELS) ||
params->motor_map[i] > VOXL_ESC_OUTPUT_CHANNELS) {
PX4_ERR("Invalid parameter VOXL_ESC_MOTORX. Please verify parameters.");
params->motor_map[i] = 0;
ret = PX4_ERROR;
}
@@ -227,7 +224,7 @@ int ModalIo::load_params(modal_io_params_t *params, ch_assign_t *map)
return ret;
}
int ModalIo::task_spawn(int argc, char *argv[])
int VoxlEsc::task_spawn(int argc, char *argv[])
{
int myoptind = 0;
int ch;
@@ -244,7 +241,7 @@ int ModalIo::task_spawn(int argc, char *argv[])
}
}
ModalIo *instance = new ModalIo();
VoxlEsc *instance = new VoxlEsc();
if (instance) {
_object.store(instance);
@@ -267,14 +264,31 @@ int ModalIo::task_spawn(int argc, char *argv[])
return PX4_ERROR;
}
int ModalIo::flush_uart_rx()
int VoxlEsc::flush_uart_rx()
{
while (_uart_port->uart_read(_read_buf, sizeof(_read_buf)) > 0) {}
return 0;
}
int ModalIo::read_response(Command *out_cmd)
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);
@@ -297,7 +311,7 @@ int ModalIo::read_response(Command *out_cmd)
return 0;
}
int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
{
hrt_abstime tnow = hrt_absolute_time();
@@ -311,13 +325,13 @@ int ModalIo::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);
uint32_t id = (fb.id_state & 0xF0) >> 4; //ID of the ESC based on hardware address
if (id < MODAL_IO_OUTPUT_CHANNELS) {
if (id < VOXL_ESC_OUTPUT_CHANNELS) {
int motor_idx = _output_map[id].number - 1; // mapped motor id.. user defined mapping is 1-4, array is 0-3
@@ -335,9 +349,9 @@ int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
_esc_chans[id].power_applied = fb.power;
_esc_chans[id].state = fb.id_state & 0x0F;
_esc_chans[id].cmd_counter = fb.cmd_counter;
_esc_chans[id].voltage = fb.voltage * 0.001;
_esc_chans[id].current = fb.current * 0.008;
_esc_chans[id].temperature = fb.temperature * 0.01;
_esc_chans[id].voltage = fb.voltage * 0.001f;
_esc_chans[id].current = fb.current * 0.008f;
_esc_chans[id].temperature = fb.temperature * 0.01f;
_esc_chans[id].feedback_time = tnow;
// also update our internal report for logging
@@ -382,6 +396,13 @@ int ModalIo::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);
@@ -400,83 +421,60 @@ int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
PX4_INFO("\tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version);
PX4_INFO("\tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version);
} else if (packet_type == ESC_PACKET_TYPE_FB_POWER_STATUS && packet_size == sizeof(QC_ESC_FB_POWER_STATUS)) {
QC_ESC_FB_POWER_STATUS packet;
memcpy(&packet, _fb_packet.buffer, packet_size);
float voltage = packet.voltage * 0.001f; // Voltage is reported at 1 mV resolution
float current = packet.current * 0.008f; // Total current is reported at 8mA resolution
// Limit the frequency of battery status reports
if (_parameters.publish_battery_status) {
_battery.setConnected(true);
_battery.updateVoltage(voltage);
_battery.updateCurrent(current);
hrt_abstime current_time = hrt_absolute_time();
if ((current_time - _last_battery_report_time) >= _battery_report_interval) {
_last_battery_report_time = current_time;
_battery.updateAndPublishBatteryStatus(current_time);
}
}
}
} else { //parser error
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;
}
}
}
/*
if (len < 4 || buf[0] != ESC_PACKET_HEADER) {
return -1;
}
switch (buf[2]) {
case ESC_PACKET_TYPE_VERSION_RESPONSE:
if (len != sizeof(QC_ESC_VERSION_INFO)) {
return -1;
} else {
QC_ESC_VERSION_INFO ver;
memcpy(&ver, buf, len);
PX4_INFO("ESC ID: %i", ver.id);
PX4_INFO("HW Version: %i", ver.hw_version);
PX4_INFO("SW Version: %i", ver.sw_version);
PX4_INFO("Unique ID: %i", ver.unique_id);
}
break;
case ESC_PACKET_TYPE_FB_RESPONSE:
if (len != sizeof(QC_ESC_FB_RESPONSE)) {
return -1;
} else {
QC_ESC_FB_RESPONSE fb;
memcpy(&fb, buf, len);
uint8_t id = (fb.state & 0xF0) >> 4;
if (id < MODAL_IO_OUTPUT_CHANNELS) {
_esc_chans[id].rate_meas = fb.rpm;
_esc_chans[id].state = fb.state & 0x0F;
_esc_chans[id].cmd_counter = fb.cmd_counter;
_esc_chans[id].voltage = 9.0 + fb.voltage / 34.0;
}
}
break;
default:
return -1;
}
*/
return 0;
}
int ModalIo::check_for_esc_timeout()
int VoxlEsc::check_for_esc_timeout()
{
hrt_abstime tnow = hrt_absolute_time();
for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) {
for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) {
// PX4 motor indexed user defined mapping is 1-4, we want to use in bitmask (0-3)
uint8_t motor_idx = _output_map[i].number - 1;
if (motor_idx < MODAL_IO_OUTPUT_CHANNELS) {
if (motor_idx < VOXL_ESC_OUTPUT_CHANNELS) {
// we are using PX4 motor index in the bitmask
if (_esc_status.esc_online_flags & (1 << motor_idx)) {
// using index i here for esc_chans enumeration stored in ESC ID order
if ((tnow - _esc_chans[i].feedback_time) > MODAL_IO_DISCONNECT_TIMEOUT_US) {
if ((tnow - _esc_chans[i].feedback_time) > VOXL_ESC_DISCONNECT_TIMEOUT_US) {
// stale data, assume offline and clear armed
_esc_status.esc_online_flags &= ~(1 << motor_idx);
_esc_status.esc_armed_flags &= ~(1 << motor_idx);
@@ -489,7 +487,7 @@ int ModalIo::check_for_esc_timeout()
}
int ModalIo::send_cmd_thread_safe(Command *cmd)
int VoxlEsc::send_cmd_thread_safe(Command *cmd)
{
cmd->id = _cmd_id++;
_pending_cmd.store(cmd);
@@ -504,7 +502,7 @@ int ModalIo::send_cmd_thread_safe(Command *cmd)
int ModalIo::custom_command(int argc, char *argv[])
int VoxlEsc::custom_command(int argc, char *argv[])
{
int myoptind = 0;
int ch;
@@ -530,7 +528,7 @@ int ModalIo::custom_command(int argc, char *argv[])
/* start the FMU if not running */
if (!strcmp(verb, "start")) {
if (!is_running()) {
return ModalIo::task_spawn(argc, argv);
return VoxlEsc::task_spawn(argc, argv);
}
}
@@ -593,7 +591,7 @@ int ModalIo::custom_command(int argc, char *argv[])
}
if (!strcmp(verb, "reset")) {
if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
PX4_INFO("Reset ESC: %i", esc_id);
cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf));
cmd.response = false;
@@ -605,7 +603,7 @@ int ModalIo::custom_command(int argc, char *argv[])
}
} else if (!strcmp(verb, "version")) {
if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
PX4_INFO("Request version for ESC: %i", esc_id);
cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
cmd.response = true;
@@ -618,7 +616,7 @@ int ModalIo::custom_command(int argc, char *argv[])
}
} else if (!strcmp(verb, "version-ext")) {
if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
PX4_INFO("Request extended version for ESC: %i", esc_id);
cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
cmd.response = true;
@@ -631,7 +629,7 @@ int ModalIo::custom_command(int argc, char *argv[])
}
} else if (!strcmp(verb, "tone")) {
if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
PX4_INFO("Request tone for ESC mask: %i", esc_id);
cmd.len = qc_esc_create_sound_packet(period, duration, power, esc_id, cmd.buf, sizeof(cmd.buf));
cmd.response = false;
@@ -660,12 +658,12 @@ int ModalIo::custom_command(int argc, char *argv[])
}
} else if (!strcmp(verb, "rpm")) {
if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
PX4_INFO("Request RPM for ESC ID: %i - RPM: %i", esc_id, rate);
int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0};
uint8_t id_fb = 0;
if (esc_id == 0xFF) {
if (esc_id == 0xFF) { //WARNING: this condition is not possible due to check 'if (esc_id < VOXL_ESC_OUTPUT_CHANNELS)'.
rate_req[0] = rate;
rate_req[1] = rate;
rate_req[2] = rate;
@@ -686,7 +684,8 @@ int ModalIo::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;
@@ -705,12 +704,12 @@ int ModalIo::custom_command(int argc, char *argv[])
}
} else if (!strcmp(verb, "pwm")) {
if (esc_id < MODAL_IO_OUTPUT_CHANNELS) {
if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
PX4_INFO("Request PWM for ESC ID: %i - PWM: %i", esc_id, rate);
int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0};
uint8_t id_fb = 0;
if (esc_id == 0xFF) {
if (esc_id == 0xFF) { //WARNING: this condition is not possible due to check 'if (esc_id < VOXL_ESC_OUTPUT_CHANNELS)'.
rate_req[0] = rate;
rate_req[1] = rate;
rate_req[2] = rate;
@@ -745,7 +744,7 @@ int ModalIo::custom_command(int argc, char *argv[])
return get_instance()->send_cmd_thread_safe(&cmd);
} else {
print_usage("Invalid ESC mask, use 1-15");
print_usage("Invalid ESC ID, use 0-3");
return 0;
}
}
@@ -753,7 +752,7 @@ int ModalIo::custom_command(int argc, char *argv[])
return print_usage("unknown command");
}
int ModalIo::update_params()
int VoxlEsc::update_params()
{
int ret = PX4_ERROR;
@@ -772,7 +771,7 @@ int ModalIo::update_params()
return ret;
}
void ModalIo::update_leds(vehicle_control_mode_s mode, led_control_s control)
void VoxlEsc::update_leds(vehicle_control_mode_s mode, led_control_s control)
{
int i = 0;
uint8_t led_mask = _led_rsc.led_mask;
@@ -872,12 +871,12 @@ void ModalIo::update_leds(vehicle_control_mode_s mode, led_control_s control)
}
}
for (i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) {
for (i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) {
_esc_chans[i].led = led_mask;
}
}
void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
void VoxlEsc::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
{
bool use_pitch = true;
bool use_roll = true;
@@ -1052,10 +1051,13 @@ void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
}
/* OutputModuleInterface */
bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated)
{
if (num_outputs != MODAL_IO_OUTPUT_CHANNELS) {
//in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function)
//So, if Run() is blocked by a custom command, this function will not be called until Run is running again
if (num_outputs != VOXL_ESC_OUTPUT_CHANNELS) {
return false;
}
@@ -1064,11 +1066,18 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
mix_turtle_mode(outputs);
}
for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) {
for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) {
if (!_outputs_on || stop_motors) {
_esc_chans[i].rate_req = 0;
} else {
if (_extended_rpm) {
if (outputs[i] > VOXL_ESC_RPM_MAX_EXT) { outputs[i] = VOXL_ESC_RPM_MAX_EXT; }
} else {
if (outputs[i] > VOXL_ESC_RPM_MAX) { outputs[i] = VOXL_ESC_RPM_MAX; }
}
if (!_turtle_mode_en) {
_esc_chans[i].rate_req = outputs[i] * _output_map[i].direction;
@@ -1079,6 +1088,7 @@ bool ModalIo::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,
@@ -1090,24 +1100,24 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
_esc_chans[3].led,
_fb_idx,
cmd.buf,
sizeof(cmd.buf));
sizeof(cmd.buf),
_extended_rpm);
if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
PX4_ERR("Failed to send packet");
return false;
}
// round robin
_fb_idx = (_fb_idx + 1) % MODAL_IO_OUTPUT_CHANNELS;
// increment ESC id from which to request feedback in round robin order
_fb_idx = (_fb_idx + 1) % VOXL_ESC_OUTPUT_CHANNELS;
/*
* Here we parse the feedback response. Rarely the packet is mangled
* but this means we simply miss a feedback response and will come back
* around in roughly 8ms for another... so don't freak out and keep on
* trucking I say
* Here we read and parse response from ESCs. Since the latest command has just been sent out,
* the response packet we may read here is probabaly from previous iteration, but it is totally ok.
* uart_read is non-blocking and we will just parse whatever bytes came in up until this point
*/
int res = _uart_port->uart_read(_read_buf, sizeof(_read_buf));
if (res > 0) {
@@ -1135,13 +1145,29 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
_esc_status_pub.publish(_esc_status);
// If any extra external modal io data has been received then
// send it over as well
while (_voxl2_io_data_sub.updated()) {
buffer128_s io_data{};
_voxl2_io_data_sub.copy(&io_data);
// PX4_INFO("Got Modal IO data: %u bytes", io_data.len);
// PX4_INFO(" 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x",
// io_data.data[0], io_data.data[1], io_data.data[2], io_data.data[3],
// io_data.data[4], io_data.data[5], io_data.data[6], io_data.data[7]);
if (_uart_port->uart_write(io_data.data, io_data.len) != io_data.len) {
PX4_ERR("Failed to send modal io data to esc");
return false;
}
}
perf_count(_output_update_perf);
return true;
}
void ModalIo::Run()
void VoxlEsc::Run()
{
if (should_exit()) {
ScheduleClear();
@@ -1164,23 +1190,22 @@ void ModalIo::Run()
}
}
/*
for (int ii=0; ii<9; ii++)
{
const char * test_str = "Hello World!";
_uart_port_bridge->uart_write((char*)test_str,12);
px4_usleep(10000);
/* 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!");
}
}
*/
/*
uint8_t echo_buf[16];
int bytes_read = _uart_port_bridge->uart_read(echo_buf,sizeof(echo_buf));
if (bytes_read > 0)
_uart_port_bridge->uart_write(echo_buf,bytes_read);
*/
}
_mixing_output.update();
_mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module
/* update output status if armed */
_outputs_on = _mixing_output.armed().armed;
@@ -1222,16 +1247,16 @@ void ModalIo::Run()
if (!_outputs_on) {
float setpoint = MODAL_IO_MODE_DISABLED_SETPOINT;
float setpoint = VOXL_ESC_MODE_DISABLED_SETPOINT;
if (_parameters.mode == MODAL_IO_MODE_TURTLE_AUX1) {
if (_parameters.mode == VOXL_ESC_MODE_TURTLE_AUX1) {
setpoint = _manual_control_setpoint.aux1;
} else if (_parameters.mode == MODAL_IO_MODE_TURTLE_AUX2) {
} else if (_parameters.mode == VOXL_ESC_MODE_TURTLE_AUX2) {
setpoint = _manual_control_setpoint.aux2;
}
if (setpoint > MODAL_IO_MODE_THRESHOLD) {
if (setpoint > VOXL_ESC_MODE_THRESHOLD) {
_turtle_mode_en = true;
} else {
@@ -1240,68 +1265,6 @@ void ModalIo::Run()
}
}
if (_parameters.mode == MODAL_IO_MODE_UART_BRIDGE) {
if (!_uart_port_bridge->is_open()) {
if (_uart_port_bridge->uart_open(MODAL_IO_VOXL_BRIDGE_PORT, 230400) == PX4_OK) {
PX4_INFO("Opened UART ESC Bridge device");
} else {
PX4_ERR("Failed openening UART ESC Bridge device");
return;
}
}
//uart passthrough test code
//run 9 times because i just don't know how to change update rate of the module from 10hz to 100hz..
for (int ii = 0; ii < 9; ii++) {
uint8_t uart_buf[128];
int bytes_read = _uart_port_bridge->uart_read(uart_buf, sizeof(uart_buf));
if (bytes_read > 0) {
_uart_port->uart_write(uart_buf, bytes_read);
for (int i = 0; i < bytes_read; i++) {
int16_t ret = qc_esc_packet_process_char(uart_buf[i], &_uart_bridge_packet);
if (ret > 0) {
//PX4_INFO("got packet of length %i",ret);
uint8_t packet_type = qc_esc_packet_get_type(&_uart_bridge_packet);
//uint8_t packet_size = qc_esc_packet_get_size(&_uart_bridge_packet);
//if we received a command for ESC to reset, most likely firmware update is coming, switch to bootloader baud rate
if (packet_type == ESC_PACKET_TYPE_RESET_CMD) {
int bootloader_baud_rate = 230400;
if (_uart_port->uart_get_baud() != bootloader_baud_rate) {
px4_usleep(5000);
_uart_port->uart_set_baud(bootloader_baud_rate);
}
} else {
if (_uart_port->uart_get_baud() != _parameters.baud_rate) {
px4_usleep(5000);
_uart_port->uart_set_baud(_parameters.baud_rate); //restore normal baud rate
}
}
}
}
}
bytes_read = _uart_port->uart_read(uart_buf, sizeof(uart_buf));
if (bytes_read > 0) {
_uart_port_bridge->uart_write(uart_buf, bytes_read);
}
px4_usleep(10000);
}
}
} else {
if (_uart_port_bridge->is_open()) {
PX4_INFO("Closed UART ESC Bridge device");
_uart_port_bridge->uart_close();
}
}
if (!_outputs_on) {
@@ -1364,7 +1327,7 @@ void ModalIo::Run()
}
int ModalIo::print_usage(const char *reason)
int VoxlEsc::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
@@ -1384,7 +1347,7 @@ $ todo
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("modal_io", "driver");
PRINT_MODULE_USAGE_NAME("voxl_esc", "driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task");
PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Send reset request to ESC");
@@ -1422,7 +1385,7 @@ $ todo
return 0;
}
int ModalIo::print_status()
int VoxlEsc::print_status()
{
PX4_INFO("Max update rate: %i Hz", _current_update_rate);
PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no");
@@ -1431,31 +1394,31 @@ int ModalIo::print_status()
PX4_INFO("");
PX4_INFO("Params: MODAL_IO_CONFIG: %" PRId32, _parameters.config);
PX4_INFO("Params: MODAL_IO_BAUD: %" PRId32, _parameters.baud_rate);
PX4_INFO("Params: VOXL_ESC_CONFIG: %" PRId32, _parameters.config);
PX4_INFO("Params: VOXL_ESC_BAUD: %" PRId32, _parameters.baud_rate);
PX4_INFO("Params: MODAL_IO_FUNC1: %" PRId32, _parameters.function_map[0]);
PX4_INFO("Params: MODAL_IO_FUNC2: %" PRId32, _parameters.function_map[1]);
PX4_INFO("Params: MODAL_IO_FUNC3: %" PRId32, _parameters.function_map[2]);
PX4_INFO("Params: MODAL_IO_FUNC4: %" PRId32, _parameters.function_map[3]);
PX4_INFO("Params: VOXL_ESC_FUNC1: %" PRId32, _parameters.function_map[0]);
PX4_INFO("Params: VOXL_ESC_FUNC2: %" PRId32, _parameters.function_map[1]);
PX4_INFO("Params: VOXL_ESC_FUNC3: %" PRId32, _parameters.function_map[2]);
PX4_INFO("Params: VOXL_ESC_FUNC4: %" PRId32, _parameters.function_map[3]);
PX4_INFO("Params: MODAL_IO_SDIR1: %" PRId32, _parameters.direction_map[0]);
PX4_INFO("Params: MODAL_IO_SDIR2: %" PRId32, _parameters.direction_map[1]);
PX4_INFO("Params: MODAL_IO_SDIR3: %" PRId32, _parameters.direction_map[2]);
PX4_INFO("Params: MODAL_IO_SDIR4: %" PRId32, _parameters.direction_map[3]);
PX4_INFO("Params: VOXL_ESC_SDIR1: %" PRId32, _parameters.direction_map[0]);
PX4_INFO("Params: VOXL_ESC_SDIR2: %" PRId32, _parameters.direction_map[1]);
PX4_INFO("Params: VOXL_ESC_SDIR3: %" PRId32, _parameters.direction_map[2]);
PX4_INFO("Params: VOXL_ESC_SDIR4: %" PRId32, _parameters.direction_map[3]);
PX4_INFO("Params: MODAL_IO_RPM_MIN: %" PRId32, _parameters.rpm_min);
PX4_INFO("Params: MODAL_IO_RPM_MAX: %" PRId32, _parameters.rpm_max);
PX4_INFO("Params: VOXL_ESC_RPM_MIN: %" PRId32, _parameters.rpm_min);
PX4_INFO("Params: VOXL_ESC_RPM_MAX: %" PRId32, _parameters.rpm_max);
PX4_INFO("");
for( int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++){
for( int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++){
PX4_INFO("-- ID: %i", i);
PX4_INFO(" Motor: %i", _output_map[i].number);
PX4_INFO(" Direction: %i", _output_map[i].direction);
PX4_INFO(" State: %i", _esc_chans[i].state);
PX4_INFO(" Requested: %i RPM", _esc_chans[i].rate_req);
PX4_INFO(" Measured: %i RPM", _esc_chans[i].rate_meas);
PX4_INFO(" Requested: %" PRIi32 " RPM", _esc_chans[i].rate_req);
PX4_INFO(" Measured: %" PRIi32 " RPM", _esc_chans[i].rate_meas);
PX4_INFO(" Command Counter: %i", _esc_chans[i].cmd_counter);
PX4_INFO(" Voltage: %f VDC", (double)_esc_chans[i].voltage);
PX4_INFO("");
@@ -1469,9 +1432,9 @@ int ModalIo::print_status()
return 0;
}
extern "C" __EXPORT int modal_io_main(int argc, char *argv[]);
extern "C" __EXPORT int voxl_esc_main(int argc, char *argv[]);
int modal_io_main(int argc, char *argv[])
int voxl_esc_main(int argc, char *argv[])
{
return ModalIo::main(argc, argv);
return VoxlEsc::main(argc, argv);
}
src/drivers/actuators/modal_io/modal_io.hpp → src/drivers/actuators/voxl_esc/voxl_esc.hpp
+59 -42
View File
@@ -41,23 +41,26 @@
#include <px4_log.h>
#include <px4_platform_common/module.h>
#include <battery/battery.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/led_control.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/actuator_test.h>
#include <uORB/topics/buffer128.h>
#include "modal_io_serial.hpp"
#include "voxl_esc_serial.hpp"
#include "qc_esc_packet.h"
#include "qc_esc_packet_types.h"
class ModalIo : public ModuleBase<ModalIo>, public OutputModuleInterface
class VoxlEsc : public ModuleBase<VoxlEsc>, public OutputModuleInterface
{
public:
ModalIo();
virtual ~ModalIo();
VoxlEsc();
virtual ~VoxlEsc();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
@@ -100,56 +103,61 @@ public:
int send_cmd_thread_safe(Command *cmd);
private:
static constexpr uint32_t MODAL_IO_UART_CONFIG = 1;
static constexpr uint32_t MODAL_IO_DEFAULT_BAUD = 250000;
static constexpr uint16_t MODAL_IO_OUTPUT_CHANNELS = 4;
static constexpr uint16_t MODAL_IO_OUTPUT_DISABLED = 0;
static constexpr uint32_t VOXL_ESC_UART_CONFIG = 1;
static constexpr uint32_t VOXL_ESC_DEFAULT_BAUD = 250000;
static constexpr uint16_t VOXL_ESC_OUTPUT_CHANNELS = 4;
static constexpr uint16_t VOXL_ESC_OUTPUT_DISABLED = 0;
static constexpr uint32_t MODAL_IO_WRITE_WAIT_US = 200;
static constexpr uint32_t MODAL_IO_DISCONNECT_TIMEOUT_US = 500000;
static constexpr uint32_t VOXL_ESC_WRITE_WAIT_US = 200;
static constexpr uint32_t VOXL_ESC_DISCONNECT_TIMEOUT_US = 500000;
static constexpr uint16_t DISARMED_VALUE = 0;
static constexpr uint16_t MODAL_IO_PWM_MIN = 0;
static constexpr uint16_t MODAL_IO_PWM_MAX = 800;
static constexpr uint16_t MODAL_IO_DEFAULT_RPM_MIN = 5000;
static constexpr uint16_t MODAL_IO_DEFAULT_RPM_MAX = 17000;
static constexpr uint16_t VOXL_ESC_PWM_MIN = 0;
static constexpr uint16_t VOXL_ESC_PWM_MAX = 800;
static constexpr uint16_t VOXL_ESC_DEFAULT_RPM_MIN = 5000;
static constexpr uint16_t VOXL_ESC_DEFAULT_RPM_MAX = 17000;
static constexpr float MODAL_IO_MODE_DISABLED_SETPOINT = -0.1f;
static constexpr float MODAL_IO_MODE_THRESHOLD = 0.0f;
static constexpr float VOXL_ESC_MODE_DISABLED_SETPOINT = -0.1f;
static constexpr float VOXL_ESC_MODE_THRESHOLD = 0.0f;
static constexpr uint32_t MODAL_IO_MODE = 0;
static constexpr uint32_t MODAL_IO_MODE_TURTLE_AUX1 = 1;
static constexpr uint32_t MODAL_IO_MODE_TURTLE_AUX2 = 2;
static constexpr uint32_t MODAL_IO_MODE_UART_BRIDGE = 3;
static constexpr uint32_t VOXL_ESC_MODE = 0;
static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX1 = 1;
static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX2 = 2;
//static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, MODAL_IO_PWM_MAX); }
//static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, MODAL_IO_RPM_MAX); }
static constexpr uint16_t VOXL_ESC_EXT_RPM = 39;
static constexpr uint16_t VOXL_ESC_RPM_MAX = INT16_MAX -
1; // 32K, Limit max standard range RPM to prevent overflow (rpm packet packing function accepts int32_t)
static constexpr uint16_t VOXL_ESC_RPM_MAX_EXT = UINT16_MAX -
5; // 65K, Limit max extended range RPM to prevent overflow (rpm packet packing function accepts int32_t)
ModalIoSerial *_uart_port;
ModalIoSerial *_uart_port_bridge;
//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); }
VoxlEscSerial *_uart_port;
typedef struct {
int32_t config{MODAL_IO_UART_CONFIG};
int32_t mode{MODAL_IO_MODE};
int32_t config{VOXL_ESC_UART_CONFIG};
int32_t mode{VOXL_ESC_MODE};
int32_t turtle_motor_expo{35};
int32_t turtle_motor_deadband{20};
int32_t turtle_motor_percent{90};
float turtle_stick_minf{0.15f};
float turtle_cosphi{0.99f};
int32_t baud_rate{MODAL_IO_DEFAULT_BAUD};
int32_t rpm_min{MODAL_IO_DEFAULT_RPM_MIN};
int32_t rpm_max{MODAL_IO_DEFAULT_RPM_MAX};
int32_t function_map[MODAL_IO_OUTPUT_CHANNELS] {0, 0, 0, 0};
int32_t motor_map[MODAL_IO_OUTPUT_CHANNELS] {1, 2, 3, 4};
int32_t direction_map[MODAL_IO_OUTPUT_CHANNELS] {1, 1, 1, 1};
int32_t baud_rate{VOXL_ESC_DEFAULT_BAUD};
int32_t rpm_min{VOXL_ESC_DEFAULT_RPM_MIN};
int32_t rpm_max{VOXL_ESC_DEFAULT_RPM_MAX};
int32_t function_map[VOXL_ESC_OUTPUT_CHANNELS] {0, 0, 0, 0};
int32_t motor_map[VOXL_ESC_OUTPUT_CHANNELS] {1, 2, 3, 4};
int32_t direction_map[VOXL_ESC_OUTPUT_CHANNELS] {1, 1, 1, 1};
int32_t verbose_logging{0};
} modal_io_params_t;
int32_t publish_battery_status{0};
} voxl_esc_params_t;
struct EscChan {
int16_t rate_req;
int32_t rate_req;
uint8_t state;
uint16_t rate_meas;
uint32_t rate_meas;
uint8_t power_applied;
uint8_t led;
uint8_t cmd_counter;
@@ -167,14 +175,14 @@ private:
typedef struct {
led_control_s control{};
vehicle_control_mode_s mode{};
uint8_t led_mask;// TODO led_mask[MODAL_IO_OUTPUT_CHANNELS];
uint8_t led_mask;// TODO led_mask[VOXL_ESC_OUTPUT_CHANNELS];
bool breath_en;
uint8_t breath_counter;
bool test;
} led_rsc_t;
ch_assign_t _output_map[MODAL_IO_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}};
MixingOutput _mixing_output{"MODAL_IO", MODAL_IO_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
ch_assign_t _output_map[VOXL_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}};
MixingOutput _mixing_output;
perf_counter_t _cycle_perf;
perf_counter_t _output_update_perf;
@@ -188,13 +196,19 @@ private:
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
uORB::Subscription _led_update_sub{ORB_ID(led_control)};
uORB::Subscription _voxl2_io_data_sub{ORB_ID(voxl2_io_data)};
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)};
modal_io_params_t _parameters;
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(modal_io_params_t *params, ch_assign_t *map);
int load_params(voxl_esc_params_t *params, ch_assign_t *map);
bool _turtle_mode_en{false};
int32_t _rpm_turtle_min{0};
@@ -205,11 +219,10 @@ private:
Command _current_cmd;
px4::atomic<Command *> _pending_cmd{nullptr};
EscChan _esc_chans[MODAL_IO_OUTPUT_CHANNELS];
EscChan _esc_chans[VOXL_ESC_OUTPUT_CHANNELS];
Command _esc_cmd;
esc_status_s _esc_status;
EscPacket _fb_packet;
EscPacket _uart_bridge_packet;
led_rsc_t _led_rsc;
int _fb_idx;
@@ -219,6 +232,10 @@ private:
static const uint8_t READ_BUF_SIZE = 128;
uint8_t _read_buf[READ_BUF_SIZE];
Battery _battery;
static constexpr unsigned _battery_report_interval{100_ms};
hrt_abstime _last_battery_report_time;
void update_leds(vehicle_control_mode_s mode, led_control_s control);
int read_response(Command *out_cmd);
src/drivers/actuators/modal_io/modal_io_params.c → src/drivers/actuators/voxl_esc/voxl_esc_params.c
+50 -34
View File
@@ -38,23 +38,23 @@
*
* @reboot_required true
*
* @group MODAL IO
* @group VOXL ESC
* @value 0 - Disabled
* @value 1 - VOXL ESC
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(MODAL_IO_CONFIG, 0);
PARAM_DEFINE_INT32(VOXL_ESC_CONFIG, 0);
/**
* UART ESC baud rate
*
* Default rate is 250Kbps, which is used in off-the-shelf MoadalAI ESC products.
*
* @group MODAL IO
* @group VOXL ESC
* @unit bit/s
*/
PARAM_DEFINE_INT32(MODAL_IO_BAUD, 250000);
PARAM_DEFINE_INT32(VOXL_ESC_BAUD, 250000);
/**
* Motor mappings for ModalAI ESC
@@ -68,33 +68,33 @@ PARAM_DEFINE_INT32(MODAL_IO_BAUD, 250000);
// The following are auto generated params from control allocator pattern, put here for reference
// Default ESC1 to motor2
//PARAM_DEFINE_INT32(MODAL_IO_FUNC1, 102);
//PARAM_DEFINE_INT32(VOXL_ESC_FUNC1, 102);
//PARAM_DEFINE_INT32(MODAL_IO_FUNC2, 103);
//PARAM_DEFINE_INT32(VOXL_ESC_FUNC2, 103);
//PARAM_DEFINE_INT32(MODAL_IO_FUNC3, 101);
//PARAM_DEFINE_INT32(VOXL_ESC_FUNC3, 101);
//PARAM_DEFINE_INT32(MODAL_IO_FUNC4, 104);
//PARAM_DEFINE_INT32(VOXL_ESC_FUNC4, 104);
/**
* UART ESC RPM Min
*
* Minimum RPM for ESC
*
* @group MODAL IO
* @group VOXL ESC
* @unit rpm
*/
PARAM_DEFINE_INT32(MODAL_IO_RPM_MIN, 5500);
PARAM_DEFINE_INT32(VOXL_ESC_RPM_MIN, 5500);
/**
* UART ESC RPM Max
*
* Maximum RPM for ESC
*
* @group MODAL IO
* @group VOXL ESC
* @unit rpm
*/
PARAM_DEFINE_INT32(MODAL_IO_RPM_MAX, 15000);
PARAM_DEFINE_INT32(VOXL_ESC_RPM_MAX, 15000);
/**
* UART ESC Mode
@@ -103,7 +103,7 @@ PARAM_DEFINE_INT32(MODAL_IO_RPM_MAX, 15000);
*
* @reboot_required true
*
* @group MODAL IO
* @group VOXL ESC
* @value 0 - None
* @value 1 - Turtle Mode enabled via AUX1
* @value 2 - Turtle Mode enabled via AUX2
@@ -111,108 +111,124 @@ PARAM_DEFINE_INT32(MODAL_IO_RPM_MAX, 15000);
* @min 0
* @max 2
*/
PARAM_DEFINE_INT32(MODAL_IO_MODE, 0);
PARAM_DEFINE_INT32(VOXL_ESC_MODE, 0);
/**
* UART ESC ID 1 Spin Direction Flag
*
* @group MODAL IO
* @group VOXL ESC
* @value 0 - Default
* @value 1 - Reverse
*/
PARAM_DEFINE_INT32(MODAL_IO_SDIR1, 0);
PARAM_DEFINE_INT32(VOXL_ESC_SDIR1, 0);
/**
* UART ESC ID 2 Spin Direction Flag
*
* @group MODAL IO
* @group VOXL ESC
* @value 0 - Default
* @value 1 - Reverse
*/
PARAM_DEFINE_INT32(MODAL_IO_SDIR2, 0);
PARAM_DEFINE_INT32(VOXL_ESC_SDIR2, 0);
/**
* UART ESC ID 3 Spin Direction Flag
*
* @group MODAL IO
* @group VOXL ESC
* @value 0 - Default
* @value 1 - Reverse
*/
PARAM_DEFINE_INT32(MODAL_IO_SDIR3, 0);
PARAM_DEFINE_INT32(VOXL_ESC_SDIR3, 0);
/**
* UART ESC ID 4 Spin Direction Flag
*
* @group MODAL IO
* @group VOXL ESC
* @value 0 - Default
* @value 1 - Reverse
*/
PARAM_DEFINE_INT32(MODAL_IO_SDIR4, 0);
PARAM_DEFINE_INT32(VOXL_ESC_SDIR4, 0);
/**
* UART ESC Turtle Mode Crash Flip Motor Percent
*
* @group MODAL IO
* @group VOXL ESC
* @min 1
* @max 100
* @decimal 10
* @increment 1
*/
PARAM_DEFINE_INT32(MODAL_IO_T_PERC, 90);
PARAM_DEFINE_INT32(VOXL_ESC_T_PERC, 90);
/**
* UART ESC Turtle Mode Crash Flip Motor Deadband
*
* @group MODAL IO
* @group VOXL ESC
* @min 0
* @max 100
* @decimal 10
* @increment 1
*/
PARAM_DEFINE_INT32(MODAL_IO_T_DEAD, 20);
PARAM_DEFINE_INT32(VOXL_ESC_T_DEAD, 20);
/**
* UART ESC Turtle Mode Crash Flip Motor STICK_MINF
*
* @group MODAL IO
* @group VOXL ESC
* @min 0.0
* @max 100.0
* @decimal 10
* @increment 1.0
*/
PARAM_DEFINE_FLOAT(MODAL_IO_T_MINF, 0.15);
PARAM_DEFINE_FLOAT(VOXL_ESC_T_MINF, 0.15);
/**
* UART ESC Turtle Mode Crash Flip Motor expo
*
* @group MODAL IO
* @group VOXL ESC
* @min 0
* @max 100
* @decimal 10
* @increment 1
*/
PARAM_DEFINE_INT32(MODAL_IO_T_EXPO, 35);
PARAM_DEFINE_INT32(VOXL_ESC_T_EXPO, 35);
/**
* UART ESC Turtle Mode Cosphi
*
* @group MODAL IO
* @group VOXL ESC
* @min 0.000
* @max 1.000
* @decimal 10
* @increment 0.001
*/
PARAM_DEFINE_FLOAT(MODAL_IO_T_COSP, 0.990);
PARAM_DEFINE_FLOAT(VOXL_ESC_T_COSP, 0.990);
/**
* UART ESC verbose logging
*
* @reboot_required true
*
* @group MODAL IO
* @group VOXL ESC
* @value 0 - Disabled
* @value 1 - Enabled
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(MODAL_IO_VLOG, 0);
PARAM_DEFINE_INT32(VOXL_ESC_VLOG, 0);
/**
* UART ESC Enable publishing of battery status
*
* Only applicable to ESCs that report total battery voltage and current
*
* @reboot_required true
*
* @group VOXL ESC
* @value 0 - Disabled
* @value 1 - Enabled
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(VOXL_ESC_PUB_BST, 1);
src/drivers/actuators/modal_io/modal_io_serial.cpp → src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp
+8 -8
View File
@@ -32,20 +32,20 @@
****************************************************************************/
#include "string.h"
#include "modal_io_serial.hpp"
#include "voxl_esc_serial.hpp"
ModalIoSerial::ModalIoSerial()
VoxlEscSerial::VoxlEscSerial()
{
}
ModalIoSerial::~ModalIoSerial()
VoxlEscSerial::~VoxlEscSerial()
{
if (_uart_fd >= 0) {
uart_close();
}
}
int ModalIoSerial::uart_open(const char *dev, speed_t speed)
int VoxlEscSerial::uart_open(const char *dev, speed_t speed)
{
if (_uart_fd >= 0) {
PX4_ERR("Port in use: %s (%i)", dev, errno);
@@ -110,7 +110,7 @@ int ModalIoSerial::uart_open(const char *dev, speed_t speed)
return 0;
}
int ModalIoSerial::uart_set_baud(speed_t speed)
int VoxlEscSerial::uart_set_baud(speed_t speed)
{
#ifndef __PX4_QURT
@@ -134,7 +134,7 @@ int ModalIoSerial::uart_set_baud(speed_t speed)
return -1;
}
int ModalIoSerial::uart_close()
int VoxlEscSerial::uart_close()
{
#ifndef __PX4_QURT
@@ -158,7 +158,7 @@ int ModalIoSerial::uart_close()
return 0;
}
int ModalIoSerial::uart_write(FAR void *buf, size_t len)
int VoxlEscSerial::uart_write(FAR void *buf, size_t len)
{
if (_uart_fd < 0 || buf == NULL) {
PX4_ERR("invalid state for writing or buffer");
@@ -172,7 +172,7 @@ int ModalIoSerial::uart_write(FAR void *buf, size_t len)
#endif
}
int ModalIoSerial::uart_read(FAR void *buf, size_t len)
int VoxlEscSerial::uart_read(FAR void *buf, size_t len)
{
if (_uart_fd < 0 || buf == NULL) {
PX4_ERR("invalid state for reading or buffer");
src/drivers/actuators/modal_io/modal_io_serial.hpp → src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp
+3 -3
View File
@@ -43,11 +43,11 @@
#define FAR
#endif
class ModalIoSerial
class VoxlEscSerial
{
public:
ModalIoSerial();
virtual ~ModalIoSerial();
VoxlEscSerial();
virtual ~VoxlEscSerial();
int uart_open(const char *dev, speed_t speed);
int uart_set_baud(speed_t speed);
src/drivers/power_monitor/voxlpm/voxlpm.cpp
+6 -2
View File
@@ -74,7 +74,6 @@ VOXLPM::init()
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
}
/* do I2C init, it will probe the bus for two possible configurations, LTC2946 or INA231 */
@@ -82,6 +81,11 @@ VOXLPM::init()
return ret;
}
// Don't actually publish anything unless we have had a successful probe
if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
}
/* If we've probed and succeeded we'll have an accurate address here for the VBat addr */
uint8_t addr = get_device_address();
@@ -420,7 +424,7 @@ VOXLPM::measure_ina231()
int16_t vshunt = -1;
uint16_t vbus = -1;
uint16_t amps = 0;
int16_t amps = 0;
int vshunt_ret = read_reg_buf(INA231_REG_SHUNTVOLTAGE, raw_vshunt, sizeof(raw_vshunt));
int vbus_ret = read_reg_buf(INA231_REG_BUSVOLTAGE, raw_vbus, sizeof(raw_vbus));
src/drivers/voxl2_io/CMakeLists.txt
+52
View File
@@ -0,0 +1,52 @@
############################################################################
#
# Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE drivers__voxl2_io
MAIN voxl2_io
SRCS
voxl2_io_crc16.c
voxl2_io_crc16.h
voxl2_io_serial.cpp
voxl2_io_serial.hpp
voxl2_io_packet.c
voxl2_io_packet.h
voxl2_io_packet_types.h
voxl2_io.cpp
voxl2_io.hpp
DEPENDS
px4_work_queue
mixer_module
MODULE_CONFIG
module.yaml
)
src/drivers/voxl2_io/Kconfig
+5
View File
@@ -0,0 +1,5 @@
menuconfig DRIVERS_VOXL2_IO
bool "voxl2_io"
default n
---help---
Enable support for voxl2_io
src/drivers/voxl2_io/module.yaml
+13
View File
@@ -0,0 +1,13 @@
module_name: VOXL2 IO Output
actuator_output:
config_parameters:
- param: 'VOXL2_IO_MIN'
label: 'PWM min value'
- param: 'VOXL2_IO_MAX'
label: 'PWM max value'
output_groups:
- param_prefix: VOXL2_IO
group_label: 'PWMs'
channel_label: 'PWM Channel'
num_channels: 4
src/drivers/voxl2_io/voxl2_io.cpp
+1122
View File
File diff suppressed because it is too large Load Diff
src/drivers/voxl2_io/voxl2_io.hpp
+228
View File
@@ -0,0 +1,228 @@
/****************************************************************************
*
* Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <float.h>
#include <math.h>
#include <board_config.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <lib/rc/sbus.h>
#include <lib/mathlib/mathlib.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
#include <uORB/Subscription.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/input_rc.h>
#include "voxl2_io_serial.hpp"
#include "voxl2_io_packet.h"
#include "voxl2_io_packet_types.h"
using namespace time_literals;
class Voxl2IO final : public ModuleBase<Voxl2IO>, public OutputModuleInterface
{
public:
Voxl2IO();
~Voxl2IO() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::print_status() */
int print_status() override;
/** @see OutputModuleInterface */
bool updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
virtual int init();
void update_pwm_config();
int get_version_info();
struct Command {
uint16_t id = 0;
uint8_t len = 0;
uint16_t repeats = 0;
uint16_t repeat_delay_us = 2000;
uint8_t retries = 0;
bool response = false;
uint16_t resp_delay_us = 1000;
bool print_feedback = false;
static const uint8_t BUF_SIZE = 128;
uint8_t buf[BUF_SIZE];
bool valid() const { return len > 0; }
void clear() { len = 0; }
};
int send_cmd_thread_safe(Command *cmd);
int receive_sbus();
void fill_rc_in(uint16_t raw_rc_count_local,
uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
hrt_abstime now, bool frame_drop, bool failsafe,
unsigned frame_drops, int rssi, input_rc_s &input_rc);
private:
void Run() override;
bool stop_all_pwms();
/* PWM Parameters */
static constexpr uint32_t VOXL2_IO_CONFIG = 0; // Default to off
static constexpr uint32_t VOXL2_IO_BOARD_CONFIG_SIZE = 4; // PWM_MIN, PWM_MAX, 4 bytes
static constexpr uint32_t VOXL2_IO_ESC_CAL_SIZE = 1;
static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600;
static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 4;
static constexpr uint16_t VOXL2_IO_OUTPUT_DISABLED = 0;
static constexpr uint32_t VOXL2_IO_WRITE_WAIT_US = 200;
static constexpr uint32_t VOXL2_IO_DISCONNECT_TIMEOUT_US = 500000;
static constexpr uint16_t DISARMED_VALUE = 0;
static constexpr uint16_t VOXL2_IO_MIXER_MIN = 0;
static constexpr uint16_t VOXL2_IO_MIXER_MAX = 800;
static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE = 0;
static constexpr uint16_t VOXL2_IO_MIXER_DISARMED = 0;
static constexpr int32_t VOXL2_IO_DEFAULT_MIN = 1000;
static constexpr int32_t VOXL2_IO_DEFAULT_MAX = 2000;
static constexpr int32_t VOXL2_IO_DEFAULT_FAILSAFE = 900;
static constexpr int32_t VOXL2_IO_TICS = 24; // 24 tics per us on M0065 timer clks
/* SBUS */
static constexpr uint16_t VOXL2_IO_SBUS_FRAME_SIZE = 30;
static constexpr uint16_t SBUS_PAYLOAD = 3;
/* M0065 version info */
static constexpr uint16_t VOXL2_IO_VERSION_INFO_SIZE = 6;
static constexpr uint16_t VOXL2_IO_SW_PROTOCOL_VERSION = 1;
static constexpr uint16_t VOXL2_IO_HW_PROTOCOL_VERSION = 35;
VOXL2_IO_VERSION_INFO _version_info;
/* Module update interval */
static constexpr unsigned _current_update_interval{4000}; // 250 Hz
typedef struct {
int32_t config{VOXL2_IO_CONFIG};
int32_t baud_rate{VOXL2_IO_DEFAULT_BAUD};
int32_t pwm_min{VOXL2_IO_DEFAULT_MIN};
int32_t pwm_max{VOXL2_IO_DEFAULT_MAX};
int32_t pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE};
int32_t param_rc_input_proto{0};
int32_t param_rc_rssi_pwm_chan{0};
int32_t function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0};
int32_t verbose_logging{0};
} voxl2_io_params_t;
voxl2_io_params_t _parameters;
typedef enum {
PWM_MODE_START = 0,
PWM_MODE_400,
PWM_MODE_END
} PWM_MODE;
enum RC_MODE {
DISABLED = 0,
SBUS,
SPEKTRUM,
EXTERNAL,
SCAN
} _rc_mode{RC_MODE::SCAN};
/* QUP7, VOXL2 J19, /dev/slpi-uart-7*/
char _device[10] {VOXL2_IO_DEFAULT_PORT};
Voxl2IoSerial *_uart_port;
/* Mixer output */
MixingOutput _mixing_output;
/* RC input */
VOXL2_IOPacket _sbus_packet;
uint64_t _rc_last_valid;
uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {UINT16_MAX};
unsigned _sbus_frame_drops{0};
uint16_t _sbus_total_frames{0};
bool _new_packet{false};
/* Publications */
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
/* Subscriptions */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
bool _pwm_on{false};
int32_t _pwm_fullscale{0};
int16_t _pwm_values[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
bool _outputs_disabled{false};
perf_counter_t _cycle_perf;
perf_counter_t _output_update_perf;
bool _debug{false};
uint16_t _cmd_id{0};
Command _current_cmd;
px4::atomic<Command *> _pending_cmd{nullptr};
static const uint8_t READ_BUF_SIZE = 128;
uint8_t _read_buf[READ_BUF_SIZE];
uint32_t _bytes_sent{0};
uint32_t _bytes_received{0};
uint32_t _packets_sent{0};
uint32_t _packets_received{0};
int parse_response(uint8_t *buf, uint8_t len);
int load_params(voxl2_io_params_t *params);
int update_params();
int flush_uart_rx();
int calibrate_escs();
};
src/drivers/voxl2_io/voxl2_io_crc16.c
+95
View File
@@ -0,0 +1,95 @@
/****************************************************************************
* Copyright (c) 2017 The Linux Foundation. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name The Linux Foundation nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY
* THIS LICENSE.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
* In addition Supplemental Terms apply. See the SUPPLEMENTAL file.
*
****************************************************************************/
#include <stdint.h>
#include "voxl2_io_crc16.h"
// Look-up table for fast CRC16 computations
const uint16_t voxl2_io_crc16_table[256] = {
0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241,
0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440,
0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40,
0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841,
0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40,
0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41,
0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641,
0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040,
0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240,
0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441,
0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41,
0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840,
0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41,
0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40,
0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640,
0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041,
0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240,
0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441,
0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41,
0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840,
0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41,
0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40,
0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640,
0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041,
0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241,
0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440,
0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40,
0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841,
0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40,
0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41,
0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641,
0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040,
};
uint16_t voxl2_io_crc16_init()
{
return 0xFFFF;
}
uint16_t voxl2_io_crc16_byte(uint16_t prev_crc, const uint8_t new_byte)
{
uint8_t lut = (prev_crc ^ new_byte) & 0xFF;
return (prev_crc >> 8) ^ voxl2_io_crc16_table[lut];
}
uint16_t voxl2_io_crc16(uint16_t prev_crc, uint8_t const *input_buffer, uint16_t input_length)
{
uint16_t crc = prev_crc;
while (input_length--) {
crc = voxl2_io_crc16_byte(crc, *input_buffer++);
}
return crc;
}
src/drivers/voxl2_io/voxl2_io_crc16.h
+65
View File
@@ -0,0 +1,65 @@
/****************************************************************************
* Copyright (c) 2017 The Linux Foundation. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name The Linux Foundation nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY
* THIS LICENSE.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
* In addition Supplemental Terms apply. See the SUPPLEMENTAL file.
*
****************************************************************************/
/*
* This file contains function prototypes for crc16 computations using polynomial 0x8005
*/
#ifndef CRC16_H_
#define CRC16_H_
#ifdef __cplusplus
extern "C"
{
#endif
#include <stdint.h>
// Returns the seed of crc output, which should be used when computing crc16 of a byte sequence
uint16_t voxl2_io_crc16_init(void);
// Process one byte by providing crc16 from previous step and new byte to consume.
// Output is the new crc16 value
uint16_t voxl2_io_crc16_byte(uint16_t prev_crc, const uint8_t new_byte);
// Process an array of bytes by providing crc16 from previous step (or seed), array of bytes and its length
// Output is the new crc16 value
uint16_t voxl2_io_crc16(uint16_t prev_crc, uint8_t const *input_buffer, uint16_t input_length);
#ifdef __cplusplus
}
#endif
#endif //CRC16_H_
src/drivers/voxl2_io/voxl2_io_packet.c
+253
View File
@@ -0,0 +1,253 @@
/****************************************************************************
* Copyright (c) 2017 The Linux Foundation. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name The Linux Foundation nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY
* THIS LICENSE.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
* In addition Supplemental Terms apply. See the SUPPLEMENTAL file.
*
****************************************************************************/
#include "voxl2_io_packet.h"
#include "voxl2_io_packet_types.h"
#include <string.h>
int32_t voxl2_io_create_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size)
{
return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_VERSION_REQUEST, &id, 1, out, out_size);
}
int32_t voxl2_io_create_extended_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size)
{
return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_VERSION_EXT_REQUEST, &id, 1, out, out_size);
}
int32_t voxl2_io_create_reset_packet(uint8_t id, uint8_t *out, uint16_t out_size)
{
char payload[] = "RESET0";
payload[5] += id;
return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_RESET_CMD, (uint8_t *)payload, 6 /*sizeof(payload)*/, out, out_size);
}
int32_t voxl2_io_create_sound_packet(uint8_t frequency, uint8_t duration, uint8_t power, uint8_t mask, uint8_t *out,
uint16_t out_size)
{
uint8_t data[4] = {frequency, duration, power, mask};
return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_SOUND_CMD, (uint8_t *) & (data[0]), 4, out, out_size);
}
int32_t voxl2_io_create_led_control_packet(uint8_t led_byte_1, uint8_t led_byte_2, uint8_t *out, uint16_t out_size)
{
uint8_t data[2] = {led_byte_1, led_byte_2};
return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_LED_CMD, (uint8_t *) & (data[0]), 2, out, out_size);
}
int32_t voxl2_io_create_set_id_packet(uint8_t id, uint8_t *out, uint16_t out_size)
{
return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_SET_ID_CMD, (uint8_t *)&id, 1, out, out_size);
}
int32_t voxl2_io_create_pwm_packet4(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
uint8_t *out, uint16_t out_size)
{
return voxl2_io_create_pwm_packet4_fb(pwm0, pwm1, pwm2, pwm3, led0, led1, led2, led3, -1, out, out_size);
}
int32_t voxl2_io_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
int32_t fb_id, uint8_t *out, uint16_t out_size)
{
uint16_t data[5];
uint16_t leds = 0;
if (fb_id != -1) { fb_id = fb_id % 4; }
//limit the pwm commands
if (pwm0 > 800) { pwm0 = 800; } if (pwm0 < -800) { pwm0 = -800; }
if (pwm1 > 800) { pwm1 = 800; } if (pwm1 < -800) { pwm1 = -800; }
if (pwm2 > 800) { pwm2 = 800; } if (pwm2 < -800) { pwm2 = -800; }
if (pwm3 > 800) { pwm3 = 800; } if (pwm3 < -800) { pwm3 = -800; }
//least significant bit is used for feedback request
pwm0 &= ~(0x0001); pwm1 &= ~(0x0001); pwm2 &= ~(0x0001); pwm3 &= ~(0x0001);
if (fb_id == 0) { pwm0 |= 0x0001; } if (fb_id == 1) { pwm1 |= 0x0001; }
if (fb_id == 2) { pwm2 |= 0x0001; } if (fb_id == 3) { pwm3 |= 0x0001; }
leds |= led0 & 0b00000111;
leds |= (led1 & 0b00000111) << 3;
leds |= ((uint16_t)(led2 & 0b00000111)) << 6;
leds |= ((uint16_t)(led3 & 0b00000111)) << 9;
data[0] = pwm0; data[1] = pwm1; data[2] = pwm2; data[3] = pwm3; data[4] = leds;
return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_PWM_CMD, (uint8_t *) & (data[0]), 10, out, out_size);
}
int32_t voxl2_io_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
uint8_t *out, uint16_t out_size)
{
return voxl2_io_create_rpm_packet4_fb(rpm0, rpm1, rpm2, rpm3, led0, led1, led2, led3, -1, out, out_size);
}
int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_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)
{
uint16_t data[5];
uint16_t leds = 0;
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 |= (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 voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size);
}
int32_t voxl2_io_create_packet(uint8_t type, uint8_t *data, uint16_t size, uint8_t *out, uint16_t out_size)
{
uint16_t packet_size = size + 5;
if (packet_size > 255) { return -1; }
if (out_size < packet_size) { return -2; }
out[0] = 0xAF;
out[1] = packet_size;
out[2] = type;
memcpy(&(out[3]), data, size);
uint16_t crc = voxl2_io_crc16_init();
crc = voxl2_io_crc16(crc, &(out[1]), packet_size - 3);
memcpy(&(out[packet_size - 2]), &crc, sizeof(uint16_t));
return packet_size;
}
//feed in a character and see if we got a complete packet
int16_t voxl2_io_packet_process_char(uint8_t c, VOXL2_IOPacket *packet)
{
int16_t ret = VOXL2_IO_NO_PACKET;
uint16_t chk_comp;
uint16_t chk_rcvd;
if (packet->len_received >= (sizeof(packet->buffer) - 1)) {
packet->len_received = 0;
}
//reset the packet and start parsing from beginning if length byte == header
//this can only happen if the packet is de-synced and last char of checksum
//ends up being equal to the header, in that case we can end up in endless loop
//unable to re-sync with the packet
if (packet->len_received == 1 && c == VOXL2_IO_PACKET_HEADER) {
packet->len_received = 0;
}
switch (packet->len_received) {
case 0: //header
packet->bp = packet->buffer; //reset the pointer for storing data
voxl2_io_packet_checksum_reset(packet); //reset the checksum to starting value
if (c != VOXL2_IO_PACKET_HEADER) { //check the packet header
packet->len_received = 0;
ret = VOXL2_IO_ERROR_BAD_HEADER;
break;
}
packet->len_received++;
*(packet->bp)++ = c;
break;
case 1: //length
packet->len_received++;
*(packet->bp)++ = c;
packet->len_expected = c;
if (packet->len_expected >= (sizeof(packet->buffer) - 1)) {
packet->len_received = 0;
ret = VOXL2_IO_ERROR_BAD_LENGTH;
break;
}
voxl2_io_packet_checksum_process_char(packet, c);
break;
default: //rest of the packet
packet->len_received++;
*(packet->bp)++ = c;
if (packet->len_received < (packet->len_expected - 1)) { //do not compute checksum of checksum (last 2 bytes)
voxl2_io_packet_checksum_process_char(packet, c);
}
if (packet->len_received < packet->len_expected) { //waiting for more bytes
break;
}
//grab the computed checksum and compare against the received value
chk_comp = voxl2_io_packet_checksum_get(packet);
memcpy(&chk_rcvd, packet->bp - 2, sizeof(uint16_t));
if (chk_comp == chk_rcvd) { ret = packet->len_received; }
else { ret = VOXL2_IO_ERROR_BAD_CHECKSUM; }
packet->len_received = 0;
break;
}
return ret;
}
src/drivers/voxl2_io/voxl2_io_packet.h
+287
View File
@@ -0,0 +1,287 @@
/****************************************************************************
* Copyright (c) 2017 The Linux Foundation. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name The Linux Foundation nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY
* THIS LICENSE.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
* In addition Supplemental Terms apply. See the SUPPLEMENTAL file.
*
****************************************************************************/
/*
* This file contains function prototypes for Voxl2 IO UART interface
*/
#ifndef VOXL2_IO_PACKET
#define VOXL2_IO_PACKET
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include "voxl2_io_crc16.h"
// Define byte values that correspond to setting red, greed, and blue LEDs
#define VOXL2_IO_LED_RED_ON 1
#define VOXL2_IO_LED_GREEN_ON 2
#define VOXL2_IO_LED_BLUE_ON 4
// Header of the packet. Each packet must start with this header
#define VOXL2_IO_PACKET_HEADER 0xAF
enum { VOXL2_IO_ERROR_BAD_LENGTH = -3,
VOXL2_IO_ERROR_BAD_HEADER = -2,
VOXL2_IO_ERROR_BAD_CHECKSUM = -1,
VOXL2_IO_NO_PACKET = 0
};
// Defines for the constatnt offsets of different parts of the packet
enum { VOXL2_IO_PACKET_POS_HEADER1 = 0,
VOXL2_IO_PACKET_POS_LENGTH,
VOXL2_IO_PACKET_POS_TYPE,
VOXL2_IO_PACKET_POS_DATA
};
// Definition of the structure that holds the state of the incoming data that is being recived (i.e. incomplete packets)
typedef struct {
uint8_t len_received; // Number of chars received so far
uint8_t len_expected; // Expected number of chars based on header
uint8_t *bp; // Pointer to the next write position in the buffer
uint16_t crc; // Accumulated CRC value so far
uint8_t buffer[64]; // Buffer to hold incoming data that is being parsed
} VOXL2_IOPacket;
// Definition of the response packet from ESC, containing the ESC version information
typedef struct {
uint8_t header;
uint8_t length; // Total length of the packet
uint8_t type; // This will be equal to ESC_PACKET_TYPE_VERSION_RESPONSE
uint8_t id; // ID of the ESC that responded
uint16_t sw_version; // Software version of the ESC firmware
uint16_t hw_version; // HW version of the board
uint32_t unique_id; // Unique ID of the ESC, if available
uint16_t crc;
} __attribute__((__packed__)) VOXL2_IO_VERSION_INFO;
typedef struct {
uint8_t header;
uint8_t length;
uint8_t type;
uint8_t id;
uint16_t sw_version;
uint16_t hw_version;
uint8_t unique_id[12];
char firmware_git_version[12];
char bootloader_git_version[12];
uint16_t bootloader_version;
uint16_t crc;
} __attribute__((__packed__)) VOXL2_IO_EXTENDED_VERSION_INFO;
// Definition of the feedback response packet from ESC
typedef struct {
uint8_t header;
uint8_t length; // Total length of the packet
uint8_t type; // This will be equal to ESC_PACKET_TYPE_FB_RESPONSE
uint8_t state; // bits 0:3 = state, bits 4:7 = ID
uint16_t rpm; // Current RPM of the motor
uint8_t cmd_counter; // Number of commands received by the ESC
uint8_t reserved0;
int8_t voltage; // Voltage = (-28)/34.0 + 9.0 = 8.176V. 0xE4 --> 228 (-28)
uint8_t reserved1;
uint16_t crc;
} __attribute__((__packed__)) VOXL2_IO_FB_RESPONSE;
// Definition of the feedback response packet from ESC
typedef struct {
uint8_t header;
uint8_t length; // Total length of the packet
uint8_t type; // This will be equal to ESC_PACKET_TYPE_FB_RESPONSE
uint8_t id_state; // bits 0:3 = state, bits 4:7 = ID
uint16_t rpm; // Current RPM of the motor
uint8_t cmd_counter; // Number of commands received by the ESC
uint8_t power; // Applied power [0..100]
uint16_t voltage; // Voltage measured by the ESC in mV
int16_t current; // Current measured by the ESC in 8mA resolution
int16_t temperature; // Temperature measured by the ESC in 0.01 degC resolution
uint16_t crc;
} __attribute__((__packed__)) VOXL2_IO_FB_RESPONSE_V2;
// Definition of the feedback response packet from ESC, which contains battery voltage and total current
typedef struct {
uint8_t header;
uint8_t length;
uint8_t type;
uint8_t id; //ESC Id (could be used as system ID elsewhere)
uint16_t voltage; //Input voltage (Millivolts)
int16_t current; //Total Current (8mA resolution)
uint16_t crc;
} __attribute__((__packed__)) VOXL2_IO_FB_POWER_STATUS;
//-------------------------------------------------------------------------
//Below are functions for generating packets that would be outgoing to ESCs
//-------------------------------------------------------------------------
// Create a generic packet by providing all required components
// Inputs are packet type, input data array and its size, output array and maximum size of output array
// Resulting packet will be placed in the output data array together with appropriate header and checksum
// Output value represents total length of the created packet (if positive), otherwise error code
int32_t voxl2_io_create_packet(uint8_t type, uint8_t *input_data, uint16_t input_size, uint8_t *out_data,
uint16_t out_data_size);
// Create a packet for requesting version information from ESC with desired id
// If an ESC with this id is connected and receives this command, it will reply with it's version information
int32_t voxl2_io_create_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size);
int32_t voxl2_io_create_extended_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size);
// Create a packet for requesting an ESC with desired id to reset
// When ESC with the particular id receives this command, and it's not spinning, ESC will reset
// This is useful for entering bootloader without removing power from the system
int32_t voxl2_io_create_reset_packet(uint8_t id, uint8_t *out, uint16_t out_size);
// Create a packet for generating a tone packet (signals are applied to motor to generate sounds)
// Inputs are relative frequency (0-255), relative duration (0-255), power (0-255) and bit mask for which ESCs should play a tone
// Bit mask definition: if bit i is set to 1, then ESC with ID=i will generate the tone
// Note that tones can only be generated when motor is not spinning
int32_t voxl2_io_create_sound_packet(uint8_t frequency, uint8_t duration, uint8_t power, uint8_t mask, uint8_t *out,
uint16_t out_size);
// Create a packet for standalone LED control
// Bit mask definition:
// led_byte_1 - bit0 = ESC0 Red, bit1 = ESC0, Green, bit2 = ESC0 Blue, bit3 = ESC1 Red, bit4 = ESC1 Green,
// bit5 = ESC1 Blue, bit6 = ESC2 Red, bit7 = ESC2 Green
// led_byte_2 - bit0 = ESC2 Blue, bit1 = ESC3 Red, bit2 = ESC3 Green, bit3 = ESC3 Blue, bits 4:7 = unused
// Note that control can only be sent when motor is not spinning
int32_t voxl2_io_create_led_control_packet(uint8_t led_byte_1, uint8_t led_byte_2, uint8_t *out, uint16_t out_size);
// Create a packet for setting the ID of an ESC
// Return value is the length of generated packet (if positive), otherwise error code
// Note that all ESCs that will receive this command will be set to this ID
int32_t voxl2_io_create_set_id_packet(uint8_t id, uint8_t *out, uint16_t out_size);
// Create a packet for sending open-loop 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 voxl2_io_create_pwm_packet4(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
uint8_t *out, uint16_t out_size);
// Create a packet for sending open-loop command 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 voxl2_io_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
int32_t fb_id, uint8_t *out, uint16_t out_size);
// 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 voxl2_io_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3,
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
uint8_t *out, uint16_t out_size);
// 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)
// Return value is the length of generated packet (if positive), otherwise error code
int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_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);
//-------------------------------------------------------------------------
// Below are functions for processing incoming packets
//-------------------------------------------------------------------------
// Feed one char and see if we have accumulated a complete packet
int16_t voxl2_io_packet_process_char(uint8_t c, VOXL2_IOPacket *packet);
// Get a pointer to the packet type from a pointer to VOXL2_IOPacket
static inline uint8_t voxl2_io_packet_get_type(VOXL2_IOPacket *packet) { return packet->buffer[VOXL2_IO_PACKET_POS_TYPE]; }
// Get a pointer to the packet type from a uint8_t pointer that points to the raw packet data as it comes from UART port
static inline uint8_t voxl2_io_packet_raw_get_type(uint8_t *packet) { return packet[VOXL2_IO_PACKET_POS_TYPE]; }
//get a pointer to the packet payload from a pointer to VOXL2_IOPacket
static inline uint8_t *voxl2_io_packet_get_data_ptr(VOXL2_IOPacket *packet) { return &(packet->buffer[VOXL2_IO_PACKET_POS_DATA]); }
// Get a pointer to the packet payload from a uint8_t pointer that points to the raw packet data as it comes from UART port
static inline uint8_t *voxl2_io_packet_raw_get_data_ptr(uint8_t *packet) { return &(packet[VOXL2_IO_PACKET_POS_DATA]); }
// Get the total size (length) in bytes of the packet
static inline uint8_t voxl2_io_packet_get_size(VOXL2_IOPacket *packet) { return packet->buffer[VOXL2_IO_PACKET_POS_LENGTH]; }
// Get checksum of the packet from a pointer to VOXL2_IOPacket
static inline uint16_t voxl2_io_packet_checksum_get(VOXL2_IOPacket *packet) { return packet->crc; }
// Calculate the checksum of a data array. Used for packet generation / processing
static inline uint16_t voxl2_io_packet_checksum(uint8_t *buf, uint16_t size)
{
uint16_t crc = voxl2_io_crc16_init();
return voxl2_io_crc16(crc, buf, size);
}
// Reset the checksum of the incoming packet. Used internally for packet reception
static inline void voxl2_io_packet_checksum_reset(VOXL2_IOPacket *packet) { packet->crc = voxl2_io_crc16_init(); }
// Process one character for checksum calculation while receiving a packet (used internally for packet reception)
static inline void voxl2_io_packet_checksum_process_char(VOXL2_IOPacket *packet, uint8_t c)
{
packet->crc = voxl2_io_crc16_byte(packet->crc, c);
}
// Initialize an instance of an VOXL2_IOPacket. This should be called once before using an instance of VOXL2_IOPacket
static inline void voxl2_io_packet_init(VOXL2_IOPacket *packet)
{
packet->len_received = 0;
packet->len_expected = 0;
packet->bp = 0;
voxl2_io_packet_checksum_reset(packet);
}
// Reset status of the packet that is being parsed. Effectively, this achieves the same thing as _packet_init
// so that _packet_init may be redundant
static inline void voxl2_io_packet_reset(VOXL2_IOPacket *packet)
{
packet->len_received = 0;
}
#endif //VOXL2_IO_PACKET
#ifdef __cplusplus
}
#endif
src/drivers/voxl2_io/voxl2_io_packet_types.h
+75
View File
@@ -0,0 +1,75 @@
/****************************************************************************
* Copyright (c) 2017 The Linux Foundation. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name The Linux Foundation nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY
* THIS LICENSE.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
* In addition Supplemental Terms apply. See the SUPPLEMENTAL file.
*
****************************************************************************/
/*
* This file contains the type values of all supported VOXL2_IO UART packets
*/
#ifndef VOXL2_IO_PACKET_TYPES
#define VOXL2_IO_PACKET_TYPES
#define VOXL2_IO_PACKET_TYPE_VERSION_REQUEST 0
#define VOXL2_IO_PACKET_TYPE_PWM_CMD 1
#define VOXL2_IO_PACKET_TYPE_RPM_CMD 2
#define VOXL2_IO_PACKET_TYPE_SOUND_CMD 3
#define VOXL2_IO_PACKET_TYPE_STEP_CMD 4
#define VOXL2_IO_PACKET_TYPE_LED_CMD 5
#define VOXL2_IO_PACKET_TYPE_RESET_CMD 10
#define VOXL2_IO_PACKET_TYPE_SET_ID_CMD 11
#define VOXL2_IO_PACKET_TYPE_SET_DIR_CMD 12
#define VOXL2_IO_PACKET_TYPE_CONFIG_BOARD_REQUEST 20
#define VOXL2_IO_PACKET_TYPE_CONFIG_USER_REQUEST 21
#define VOXL2_IO_PACKET_TYPE_CONFIG_UART_REQUEST 22
#define VOXL2_IO_PACKET_TYPE_CONFIG_TUNE_REQUEST 23
#define VOXL2_IO_PACKET_TYPE_VERSION_EXT_REQUEST 24
#define VOXL2_IO_PACKET_TYPE_SET_FEEDBACK_MODE 50 //reserved for future use
#define VOXL2_IO_PACKET_TYPE_EEPROM_WRITE_UNLOCK 70
#define VOXL2_IO_PACKET_TYPE_EEPROM_READ_UNLOCK 71
#define VOXL2_IO_PACKET_TYPE_EEPROM_WRITE 72
#define VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE 109
#define VOXL2_IO_PACKET_TYPE_PARAMS 110
#define VOXL2_IO_PACKET_TYPE_BOARD_CONFIG 111
#define VOXL2_IO_PACKET_TYPE_USER_CONFIG 112
#define VOXL2_IO_PACKET_TYPE_UART_CONFIG 113
#define VOXL2_IO_PACKET_TYPE_TUNE_CONFIG 114
#define VOXL2_IO_PACKET_TYPE_FB_RESPONSE 128
#define VOXL2_IO_PACKET_TYPE_VERSION_EXT_RESPONSE 131
#define VOXL2_IO_PACKET_TYPE_FB_POWER_STATUS 132
#define VOXL2_IO_PACKET_TYPE_RC_DATA_RAW 133
#endif
src/drivers/voxl2_io/voxl2_io_params.c
+66
View File
@@ -0,0 +1,66 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* UART M0065 baud rate
*
* Default rate is 921600, which is used for communicating with M0065.
*
* @group VOXL2 IO
* @unit bit/s
*/
PARAM_DEFINE_INT32(VOXL2_IO_BAUD, 921600);
/**
* M0065 PWM Min
*
* Minimum duration (microseconds) for M0065 PWM
*
* @min 0
* @max 2000
* @group VOXL2 IO
* @unit us
*/
PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1000);
/**
* M0065 PWM Max
*
* Maximum duration (microseconds) for M0065 PWM
* @min 0
* @max 2000
* @group VOXL2 IO
* @unit us
*/
PARAM_DEFINE_INT32(VOXL2_IO_MAX, 2000);
src/drivers/voxl2_io/voxl2_io_serial.cpp
+191
View File
@@ -0,0 +1,191 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "string.h"
#include "voxl2_io_serial.hpp"
Voxl2IoSerial::Voxl2IoSerial()
{
}
Voxl2IoSerial::~Voxl2IoSerial()
{
if (_uart_fd >= 0) {
uart_close();
}
}
int Voxl2IoSerial::uart_open(const char *dev, speed_t speed)
{
if (_uart_fd >= 0) {
PX4_ERR("Port in use: %s (%i)", dev, errno);
return -1;
}
/* Open UART */
#ifdef __PX4_QURT
_uart_fd = qurt_uart_open(dev, speed);
#else
_uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
#endif
if (_uart_fd < 0) {
PX4_ERR("Error opening port: %s (%i)", dev, errno);
return -1;
}
#ifndef __PX4_QURT
/* Back up the original UART configuration to restore it after exit */
int termios_state;
if ((termios_state = tcgetattr(_uart_fd, &_orig_cfg)) < 0) {
PX4_ERR("Error configuring port: tcgetattr %s: %d", dev, termios_state);
uart_close();
return -1;
}
/* Fill the struct for the new configuration */
tcgetattr(_uart_fd, &_cfg);
/* Disable output post-processing */
_cfg.c_oflag &= ~OPOST;
_cfg.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */
_cfg.c_cflag &= ~CSIZE;
_cfg.c_cflag |= CS8; /* 8-bit characters */
_cfg.c_cflag &= ~PARENB; /* no parity bit */
_cfg.c_cflag &= ~CSTOPB; /* only need 1 stop bit */
_cfg.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */
/* setup for non-canonical mode */
_cfg.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
_cfg.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
if (cfsetispeed(&_cfg, speed) < 0 || cfsetospeed(&_cfg, speed) < 0) {
PX4_ERR("Error configuring port: %s: %d (cfsetispeed, cfsetospeed)", dev, termios_state);
uart_close();
return -1;
}
if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &_cfg)) < 0) {
PX4_ERR("Error configuring port: %s (tcsetattr)", dev);
uart_close();
return -1;
}
#endif
_speed = speed;
return 0;
}
int Voxl2IoSerial::uart_set_baud(speed_t speed)
{
#ifndef __PX4_QURT
if (_uart_fd < 0) {
return -1;
}
if (cfsetispeed(&_cfg, speed) < 0) {
return -1;
}
if (tcsetattr(_uart_fd, TCSANOW, &_cfg) < 0) {
return -1;
}
_speed = speed;
return 0;
#endif
return -1;
}
int Voxl2IoSerial::uart_close()
{
#ifndef __PX4_QURT
if (_uart_fd < 0) {
PX4_ERR("invalid state for closing");
return -1;
}
if (tcsetattr(_uart_fd, TCSANOW, &_orig_cfg)) {
PX4_ERR("failed restoring uart to original state");
}
if (close(_uart_fd)) {
PX4_ERR("error closing uart");
}
#endif
_uart_fd = -1;
return 0;
}
int Voxl2IoSerial::uart_write(FAR void *buf, size_t len)
{
if (_uart_fd < 0 || buf == NULL) {
PX4_ERR("invalid state for writing or buffer");
return -1;
}
#ifdef __PX4_QURT
return qurt_uart_write(_uart_fd, (const char *) buf, len);
#else
return write(_uart_fd, buf, len);
#endif
}
int Voxl2IoSerial::uart_read(FAR void *buf, size_t len)
{
if (_uart_fd < 0 || buf == NULL) {
PX4_ERR("invalid state for reading or buffer");
return -1;
}
#ifdef __PX4_QURT
#define ASYNC_UART_READ_WAIT_US 2000
// The UART read on SLPI is via an asynchronous service so specify a timeout
// for the return. The driver will poll periodically until the read comes in
// so this may block for a while. However, it will timeout if no read comes in.
return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US);
#else
return read(_uart_fd, buf, len);
#endif
}
src/modules/muorb/apps/fc_sensor.h → src/drivers/voxl2_io/voxl2_io_serial.hpp
+29 -29
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2022 ModalAI, Inc. All rights reserved.
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -31,39 +31,39 @@
*
****************************************************************************/
#ifndef FC_SENSOR_H
#define FC_SENSOR_H
#pragma once
#ifdef __cplusplus
extern "C" {
#include <px4_log.h>
#include <errno.h>
#include <fcntl.h>
#include <termios.h>
#ifdef __PX4_QURT
#include <drivers/device/qurt/uart.h>
#define FAR
#endif
#include <stdint.h>
#include <stdbool.h>
class Voxl2IoSerial
{
public:
Voxl2IoSerial();
virtual ~Voxl2IoSerial();
typedef void (*fc_receive_cb)(const char *topic,
const uint8_t *data,
uint32_t length_in_bytes);
typedef void (*fc_advertise_cb)(const char *topic);
typedef void (*fc_add_subscription_cb)(const char *topic);
typedef void (*fc_remove_subscription_cb)(const char *topic);
int uart_open(const char *dev, speed_t speed);
int uart_set_baud(speed_t speed);
int uart_close();
int uart_write(FAR void *buf, size_t len);
int uart_read(FAR void *buf, size_t len);
bool is_open() { return _uart_fd >= 0; };
int uart_get_baud() {return _speed; }
typedef struct {
fc_receive_cb rx_callback;
fc_advertise_cb ad_callback;
fc_add_subscription_cb sub_callback;
fc_remove_subscription_cb remove_sub_callback;
} fc_callbacks;
private:
int _uart_fd = -1;
int fc_sensor_initialize(bool enable_debug_messages, fc_callbacks *callbacks);
int fc_sensor_advertise(const char *topic);
int fc_sensor_subscribe(const char *topic);
int fc_sensor_unsubscribe(const char *topic);
int fc_sensor_send_data(const char *topic,
const uint8_t *data,
uint32_t length_in_bytes);
#ifdef __cplusplus
}
#if ! defined(__PX4_QURT)
struct termios _orig_cfg;
struct termios _cfg;
#endif
#endif // FC_SENSOR_H
int _speed = -1;
};
src/modules/muorb/apps/CMakeLists.txt
+1
View File
@@ -39,6 +39,7 @@ px4_add_module(
INCLUDES
../test
../aggregator
libfc-sensor-api/inc
SRCS
uORBAppsProtobufChannel.cpp
muorb_main.cpp
src/modules/muorb/apps/libfc-sensor-api
Submodule
+1
Submodule src/modules/muorb/apps/libfc-sensor-api added at ca16e99074
src/modules/muorb/apps/uORBAppsProtobufChannel.cpp
+5
View File
@@ -113,6 +113,11 @@ void uORB::AppsProtobufChannel::SubscribeCallback(const char *topic)
test_flag = true;
return;
} else if (strcmp(topic, "CPULOAD") == 0) {
// PX4_ERR("Got CPULOAD subscription");
// This will happen when a newer PX4 version is talking to a
// SLPI image that doesn't support the CPULOAD request. If the
// SLPI image does support it then we wouldn't get this.
} else if (_RxHandler) {
pthread_mutex_lock(&_rx_mutex);
_SlpiSubscriberCache[topic]++;
src/modules/muorb/slpi/uORBProtobufChannel.cpp
+36 -2
View File
@@ -385,8 +385,13 @@ int px4muorb_add_subscriber(const char *topic_name)
uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler();
if (rxHandler) {
channel->AddRemoteSubscriber(topic_name);
// Pick a high message rate of 1000 Hz
if (channel->AddRemoteSubscriber(topic_name)) {
// Only process this subscription if it is the only one for the topic.
// Otherwise it will send some data from the queue and, most likely,
// mess up the queue on the remote side.
return 0;
}
return rxHandler->process_add_subscription(topic_name);
} else {
@@ -476,3 +481,32 @@ int px4muorb_send_topic_data(const char *topic_name, const uint8_t *data,
return -1;
}
float px4muorb_get_cpu_load(void)
{
// Default value to return if the SLPI code doesn't support
// queries for the CPU load
float cpu_load = 0.1f;
uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance();
if (channel) {
// The method to get the CPU load from the SLPI image is to send
// in the special code string to the add_subscription call. If it
// isn't supported the only return values can be 0 or -1. If it is
// supported then it will be some positive integer.
int16_t int_cpu_load = channel->add_subscription("CPULOAD", 0);
if (int_cpu_load > 1) {
// Yay! CPU Load query is supported!
cpu_load = (float) int_cpu_load;
}
} else {
PX4_ERR("Null channel pointer in %s", __FUNCTION__);
}
return cpu_load;
}
src/modules/muorb/slpi/uORBProtobufChannel.hpp
+6 -2
View File
@@ -132,11 +132,13 @@ public:
_Aggregator.RegisterSendHandler(func);
}
void AddRemoteSubscriber(const std::string &messageName)
int AddRemoteSubscriber(const std::string &messageName)
{
int currentRemoteSubscribers;
pthread_mutex_lock(&_rx_mutex);
_AppsSubscriberCache[messageName]++;
currentRemoteSubscribers = _AppsSubscriberCache[messageName]++;
pthread_mutex_unlock(&_rx_mutex);
return currentRemoteSubscribers;
}
void RemoveRemoteSubscriber(const std::string &messageName)
@@ -214,6 +216,8 @@ extern "C" {
int px4muorb_remove_subscriber(const char *name) __EXPORT;
int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT;
float px4muorb_get_cpu_load(void) __EXPORT;
}
#endif // _uORBProtobufChannel_hpp_