dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

drivers/actuators/modalai_esc: update to use mixer module and control allocator properly 

- update motor mapping to use new UART_ESC_FUNC* auto generated params
 - add support for actuator_test msg to support Actuator Testing in QGC
 - modalai_fc-vX targets start driver if configured
 - keep track of ESC spin direction in own param
 - set ramp up param in MixerOutput to false
This commit is contained in:
modaltb 2023-01-16 18:46:37 -08:00 committed by GitHub
parent 2a64145dcd
commit 6f718cd48d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 145 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
boards/modalai/fc-v1/init/rc.board_sensors
View File

@ -3,6 +3,11 @@
# ModalAI FC-v1 specific board sensors init
#------------------------------------------------------------------------------
if param greater UART_ESC_CONFIG 0
then
modalai_esc start
fi
board_adc start
# Start Digital power monitors

9
boards/modalai/fc-v2/init/rc.board_sensors
View File

@ -3,11 +3,10 @@
# ModalAI FC-v2 specific board sensors init
#------------------------------------------------------------------------------
# VOXL ESC - TODO
#if param greater UART_ESC_CONFIG 0
#then
# modalai_esc -d /dev/ttyS4 start
#fi
if param greater UART_ESC_CONFIG 0
then
modalai_esc start
fi
board_adc start

2
src/drivers/actuators/modalai_esc/CMakeLists.txt
View File

@ -48,4 +48,6 @@ px4_add_module(
DEPENDS
px4_work_queue
mixer_module
MODULE_CONFIG
module.yaml
)

71
src/drivers/actuators/modalai_esc/modalai_esc.cpp
View File

@ -36,8 +36,6 @@
#include "modalai_esc.hpp"
#include "modalai_esc_serial.hpp"
#define MODALAI_ESC_DEVICE_PATH "/dev/uart_esc"
// utility for running on VOXL and using driver as a bridge
#define MODALAI_ESC_VOXL_BRIDGE_PORT "/dev/ttyS4"
@ -47,7 +45,8 @@
const char *_device;
ModalaiEsc::ModalaiEsc() :
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(MODALAI_ESC_DEFAULT_PORT)),
_mixing_output{"UART_ESC", MODALAI_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"))
{
@ -123,18 +122,33 @@ int ModalaiEsc::load_params(uart_esc_params_t *params, ch_assign_t *map)
{
int ret = PX4_OK;
// initialize out
for (int i = 0; i < MODALAI_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("UART_ESC_CONFIG"), &params->config);
param_get(param_find("UART_ESC_MODE"), &params->mode);
param_get(param_find("UART_ESC_BAUD"), &params->baud_rate);
param_get(param_find("UART_ESC_T_PERC"), &params->turtle_motor_percent);
param_get(param_find("UART_ESC_T_DEAD"), &params->turtle_motor_deadband);
param_get(param_find("UART_ESC_T_EXPO"), &params->turtle_motor_expo);
param_get(param_find("UART_ESC_T_MINF"), &params->turtle_stick_minf);
param_get(param_find("UART_ESC_T_COSP"), &params->turtle_cosphi);
param_get(param_find("UART_ESC_BAUD"), &params->baud_rate);
param_get(param_find("UART_ESC_MOTOR1"), &params->motor_map[0]);
param_get(param_find("UART_ESC_MOTOR2"), &params->motor_map[1]);
param_get(param_find("UART_ESC_MOTOR3"), &params->motor_map[2]);
param_get(param_find("UART_ESC_MOTOR4"), &params->motor_map[3]);
param_get(param_find("UART_ESC_FUNC1"), &params->function_map[0]);
param_get(param_find("UART_ESC_FUNC2"), &params->function_map[1]);
param_get(param_find("UART_ESC_FUNC3"), &params->function_map[2]);
param_get(param_find("UART_ESC_FUNC4"), &params->function_map[3]);
param_get(param_find("UART_ESC_SDIR1"), &params->direction_map[0]);
param_get(param_find("UART_ESC_SDIR2"), &params->direction_map[1]);
param_get(param_find("UART_ESC_SDIR3"), &params->direction_map[2]);
param_get(param_find("UART_ESC_SDIR4"), &params->direction_map[3]);
param_get(param_find("UART_ESC_RPM_MIN"), &params->rpm_min);
param_get(param_find("UART_ESC_RPM_MAX"), &params->rpm_max);
@ -174,6 +188,18 @@ int ModalaiEsc::load_params(uart_esc_params_t *params, ch_assign_t *map)
ret = PX4_ERROR;
}
for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) {
if (params->function_map[i] < (int)OutputFunction::Motor1 || params->function_map[i] > (int)OutputFunction::Motor4) {
PX4_ERR("Invalid parameter UART_ESC_FUNCX. Only supports motors 1-4. Please verify parameters.");
params->function_map[i] = 0;
ret = PX4_ERROR;
} else {
// Motor function IDs start at 100, Motor1 = 101, Motor2 = 102...
params->motor_map[i] = params->function_map[i] - 100;
}
}
for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) {
if (params->motor_map[i] == MODALAI_ESC_OUTPUT_DISABLED ||
params->motor_map[i] < -(MODALAI_ESC_OUTPUT_CHANNELS) ||
@ -185,7 +211,7 @@ int ModalaiEsc::load_params(uart_esc_params_t *params, ch_assign_t *map)
/* Can map -4 to 4, 0 being disabled. Negative represents reverse direction */
map[i].number = abs(params->motor_map[i]);
map[i].direction = (params->motor_map[i] > 0) ? 1 : -1;
map[i].direction = (params->direction_map[i] > 0) ? -1 : 1;
}
return ret;
@ -769,8 +795,11 @@ int ModalaiEsc::update_params()
ret = load_params(&_parameters, (ch_assign_t *)&_output_map);
if (ret == PX4_OK) {
_mixing_output.setAllDisarmedValues(0);
_mixing_output.setAllFailsafeValues(0);
_mixing_output.setAllMinValues(_parameters.rpm_min);
_mixing_output.setAllMaxValues(_parameters.rpm_max);
_rpm_fullscale = _parameters.rpm_max - _parameters.rpm_min;
}
@ -1318,6 +1347,13 @@ void ModalaiEsc::Run()
}
}
if (!_outputs_on) {
if (_actuator_test_sub.updated()) {
// values are set in ActuatorTest::update, we just need to enable outputs to let them through
_outputs_on = true;
}
}
/* Don't process commands if outputs on */
if (!_outputs_on) {
if (_current_cmd.valid()) {
@ -1438,10 +1474,17 @@ int ModalaiEsc::print_status()
PX4_INFO("Params: UART_ESC_CONFIG: %li", _parameters.config);
PX4_INFO("Params: UART_ESC_BAUD: %li", _parameters.baud_rate);
PX4_INFO("Params: UART_ESC_MOTOR1: %li", _parameters.motor_map[0]);
PX4_INFO("Params: UART_ESC_MOTOR2: %li", _parameters.motor_map[1]);
PX4_INFO("Params: UART_ESC_MOTOR3: %li", _parameters.motor_map[2]);
PX4_INFO("Params: UART_ESC_MOTOR4: %li", _parameters.motor_map[3]);
PX4_INFO("Params: UART_ESC_FUNC1: %li", _parameters.function_map[0]);
PX4_INFO("Params: UART_ESC_FUNC2: %li", _parameters.function_map[1]);
PX4_INFO("Params: UART_ESC_FUNC3: %li", _parameters.function_map[2]);
PX4_INFO("Params: UART_ESC_FUNC4: %li", _parameters.function_map[3]);
PX4_INFO("Params: UART_ESC_SDIR1: %li", _parameters.direction_map[0]);
PX4_INFO("Params: UART_ESC_SDIR2: %li", _parameters.direction_map[1]);
PX4_INFO("Params: UART_ESC_SDIR3: %li", _parameters.direction_map[2]);
PX4_INFO("Params: UART_ESC_SDIR4: %li", _parameters.direction_map[3]);
PX4_INFO("Params: UART_ESC_RPM_MIN: %li", _parameters.rpm_min);
PX4_INFO("Params: UART_ESC_RPM_MAX: %li", _parameters.rpm_max);
@ -1449,6 +1492,8 @@ int ModalaiEsc::print_status()
for( int i = 0; i < MODALAI_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);

7
src/drivers/actuators/modalai_esc/modalai_esc.hpp
View File

@ -46,6 +46,7 @@
#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 "modalai_esc_serial.hpp"
@ -146,7 +147,9 @@ private:
int32_t baud_rate{MODALAI_ESC_DEFAULT_BAUD};
int32_t rpm_min{MODALAI_ESC_DEFAULT_RPM_MIN};
int32_t rpm_max{MODALAI_ESC_DEFAULT_RPM_MAX};
int32_t function_map[MODALAI_ESC_OUTPUT_CHANNELS] {0, 0, 0, 0};
int32_t motor_map[MODALAI_ESC_OUTPUT_CHANNELS] {1, 2, 3, 4};
int32_t direction_map[MODALAI_ESC_OUTPUT_CHANNELS] {1, 1, 1, 1};
} uart_esc_params_t;
struct EscChan {
@ -177,7 +180,7 @@ private:
} led_rsc_t;
ch_assign_t _output_map[MODALAI_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}};
MixingOutput _mixing_output{"MODALAI_ESC", MODALAI_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
MixingOutput _mixing_output;
perf_counter_t _cycle_perf;
perf_counter_t _output_update_perf;
@ -189,6 +192,7 @@ private:
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
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::Publication<actuator_outputs_s> _outputs_debug_pub{ORB_ID(actuator_outputs_debug)};
@ -228,4 +232,5 @@ private:
int flush_uart_rx();
int check_for_esc_timeout();
void mix_turtle_mode(uint16_t outputs[]);
void handle_actuator_test();
};

77
src/drivers/actuators/modalai_esc/modalai_esc_params.c
View File

@ -57,7 +57,7 @@ PARAM_DEFINE_INT32(UART_ESC_CONFIG, 0);
PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000);
/**
* Motor mappings for ModalAI ESC M004
* Motor mappings for ModalAI ESC
*
* HW Channel Idexes (PX4 Indexes) (note: silkscreen shows 0 indexed)
* 4(1) 3(4)
@ -65,41 +65,16 @@ PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000);
* 1(3) 2(2)
*/
/**
* UART ESC Motor 1 Mapping. 1-4 (negative for reversal).
*
* @group UART ESC
* @min -4
* @max 4
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 3);
// The following are auto generated params from control allocator pattern, put here for reference
/**
*UART ESC Motor 2 Mapping. 1-4 (negative for reversal).
*
* @group UART ESC
* @min -4
* @max 4
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2);
// Default ESC1 to motor2
//PARAM_DEFINE_INT32(UART_ESC_FUNC1, 102);
/**
* UART ESC Motor 3 Mapping. 1-4 (negative for reversal).
*
* @group UART ESC
* @min -4
* @max 4
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 4);
//PARAM_DEFINE_INT32(UART_ESC_FUNC2, 103);
/**
* UART ESC Motor 4 Mapping. 1-4 (negative for reversal).
*
* @group UART ESC
* @min -4
* @max 4
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 1);
//PARAM_DEFINE_INT32(UART_ESC_FUNC3, 101);
//PARAM_DEFINE_INT32(UART_ESC_FUNC4, 104);
/**
* UART ESC RPM Min
@ -136,6 +111,42 @@ PARAM_DEFINE_INT32(UART_ESC_RPM_MAX, 15000);
*/
PARAM_DEFINE_INT32(UART_ESC_MODE, 0);
/**
* UART ESC ID 1 Spin Direction Flag
*
* @group UART ESC
* @value 0 - Default
* @value 1 - Reverse
*/
PARAM_DEFINE_INT32(UART_ESC_SDIR1, 0);
/**
* UART ESC ID 2 Spin Direction Flag
*
* @group UART ESC
* @value 0 - Default
* @value 1 - Reverse
*/
PARAM_DEFINE_INT32(UART_ESC_SDIR2, 0);
/**
* UART ESC ID 3 Spin Direction Flag
*
* @group UART ESC
* @value 0 - Default
* @value 1 - Reverse
*/
PARAM_DEFINE_INT32(UART_ESC_SDIR3, 0);
/**
* UART ESC ID 4 Spin Direction Flag
*
* @group UART ESC
* @value 0 - Default
* @value 1 - Reverse
*/
PARAM_DEFINE_INT32(UART_ESC_SDIR4, 0);
/**
* UART ESC Turtle Mode Crash Flip Motor Percent
*

26
src/drivers/actuators/modalai_esc/module.yaml Normal file
View File

@ -0,0 +1,26 @@
module_name: MODALAI ESC Output
actuator_output:
show_subgroups_if: 'UART_ESC_CONFIG>0'
config_parameters:
- param: 'UART_ESC_CONFIG'
label: 'Configure'
function: 'enable'
- param: 'UART_ESC_BAUD'
label: 'Bitrate'
- param: 'UART_ESC_RPM_MIN'
label: 'RPM Min'
- param: 'UART_ESC_RPM_MAX'
label: 'RPM Max'
- param: 'UART_ESC_SDIR1'
label: 'ESC 1 Spin Direction'
- param: 'UART_ESC_SDIR2'
label: 'ESC 2 Spin Direction'
- param: 'UART_ESC_SDIR3'
label: 'ESC 3 Spin Direction'
- param: 'UART_ESC_SDIR4'
label: 'ESC 4 Spin Direction'
output_groups:
- param_prefix: UART_ESC
group_label: 'ESCs'
channel_label: 'ESC'
num_channels: 4