From 912baafd5b459a6e4c3f8d04f40a0fcd926e71b8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 30 Apr 2021 09:11:25 +0200 Subject: [PATCH] TEMP --- ROMFS/px4fmu_common/init.d/rc.interface | 5 ++ ROMFS/px4fmu_common/init.d/rcS | 5 ++ src/drivers/dshot/DShot.cpp | 1 - src/drivers/vesc/VescSerialDevice.cpp | 90 ++++++++++++++++++++++--- src/drivers/vesc/VescSerialDevice.hpp | 16 ++++- src/drivers/vesc/vesc_main.cpp | 2 +- src/lib/mixer_module/mixer_module.cpp | 64 ++++++++++-------- src/lib/mixer_module/mixer_module.hpp | 4 +- src/lib/mixer_module/params.c | 1 + 9 files changed, 147 insertions(+), 41 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 3bbcda9746..92c563eeb1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -144,6 +144,11 @@ then set OUTPUT_DEV /dev/uavcan/esc fi + if [ $OUTPUT_MODE = vesc ] + then + set OUTPUT_DEV /dev/vesc + fi + if mixer load ${OUTPUT_DEV} ${MIXER_FILE} then echo "INFO [init] Mixer: ${MIXER_FILE} on ${OUTPUT_DEV}" diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 389a4ec97d..7ec2efd1d9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -282,6 +282,11 @@ else fi fi + if param greater -s VESC_PORT 0 + then + set OUTPUT_MODE vesc + fi + # # Check if PX4IO present and update firmware if needed. # Assumption IOFW set to firmware file and IO_PRESENT = no diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 42449567ac..50783312fc 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -43,7 +43,6 @@ DShot::DShot() : _mixing_output.setAllDisarmedValues(DSHOT_DISARM_VALUE); _mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE); _mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE); - } DShot::~DShot() diff --git a/src/drivers/vesc/VescSerialDevice.cpp b/src/drivers/vesc/VescSerialDevice.cpp index 8d351035b9..d96757c2e0 100644 --- a/src/drivers/vesc/VescSerialDevice.cpp +++ b/src/drivers/vesc/VescSerialDevice.cpp @@ -44,7 +44,8 @@ using namespace time_literals; VescDevice::VescDevice(const char *port) : - ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), + CDev("/dev/vesc"), + OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), _vesc_driver(*this) { // Store port name @@ -52,6 +53,10 @@ VescDevice::VescDevice(const char *port) : // Enforce null termination _port[sizeof(_port) - 1] = '\0'; + + _mixing_output.setAllDisarmedValues(DISARM_VALUE); + _mixing_output.setAllMinValues(MIN_THROTTLE); + _mixing_output.setAllMaxValues(MAX_THROTTLE); } VescDevice::~VescDevice() @@ -61,22 +66,57 @@ VescDevice::~VescDevice() int VescDevice::init() { - int ret = setBaudrate(115200); + // Do regular cdev init + int ret = CDev::init(); - if (ret >= 0) { - ScheduleOnInterval(10_ms); + if (ret != OK) { + return ret; } + // Set Baudrate and schedule for the first time + ret = setBaudrate(115200); + + if (ret != OK) { + return ret; + } + + ScheduleNow(); return OK; } -void VescDevice::print_info() +void VescDevice::printStatus() { printf("Using port '%s'\n", _port); printf("VESC version: %d.%d\n", _vesc_driver.getVersionMajor(), _vesc_driver.getVersionMinor()); + _mixing_output.printStatus(); perf_print_counter(_cycle_perf); } +bool VescDevice::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + unsigned num_control_groups_updated) +{ + // for (unsigned i = 0; i < num_outputs; i++) { + // if (!stop_motors && outputs[i] != DISARM_VALUE) { + // _vesc_driver.commandDutyCycle(static_cast(outputs[i]) / 1e3f); + // } + // } + + if (!stop_motors) { + _vesc_driver.commandDutyCycle(static_cast(outputs[0]) / 1e3f); + _vesc_driver.commandDutyCycle(static_cast(outputs[1]) / 1e3f, 11); + _vesc_driver.commandDutyCycle(static_cast(outputs[2]) / 1e3f, 7); + _vesc_driver.commandDutyCycle(static_cast(outputs[3]) / 1e3f, 107); + + } else { + _vesc_driver.commandDutyCycle(0.f); + _vesc_driver.commandDutyCycle(0.f, 11); + _vesc_driver.commandDutyCycle(0.f, 7); + _vesc_driver.commandDutyCycle(0.f, 107); + } + + return true; +} + void VescDevice::Run() { perf_begin(_cycle_perf); @@ -87,12 +127,11 @@ void VescDevice::Run() _vesc_driver.requestFirmwareVersion(); } - _vesc_driver.commandDutyCycle(.05f); + _mixing_output.update(); // Check the number of bytes available in the buffer int bytes_available{0}; int ret = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available); - printf("ioctl: %d %d\n", ret, _serial_fd); while (ret >= 0 && bytes_available > 0) { printf("read: "); @@ -114,7 +153,8 @@ void VescDevice::Run() bytes_available -= ret; } - // TODO: ::close(_serial_fd); on exit + _mixing_output.updateSubscriptions(true); + perf_end(_cycle_perf); } @@ -227,3 +267,37 @@ size_t VescDevice::writeCallback(const uint8_t *buffer, const uint16_t length) printf("write result: %d %d\n", ret, _serial_fd); return ret; } + +int VescDevice::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + PX4_DEBUG("ioctl cmd: %d, arg: %ld", cmd, arg); + + lock(); + + switch (cmd) { + case MIXERIOCRESET: + _mixing_output.resetMixerThreadSafe(); + break; + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strlen(buf); + ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + if (ret == -ENOTTY) { + ret = CDev::ioctl(filp, cmd, arg); + } + + return ret; +} diff --git a/src/drivers/vesc/VescSerialDevice.hpp b/src/drivers/vesc/VescSerialDevice.hpp index 20ef5145ec..8ce14d191f 100644 --- a/src/drivers/vesc/VescSerialDevice.hpp +++ b/src/drivers/vesc/VescSerialDevice.hpp @@ -38,24 +38,36 @@ */ #include "VescDriver/VescDriver.hpp" +#include +#include +#include +#include #include -class VescDevice : public px4::ScheduledWorkItem, public VescWritableInterface +class VescDevice : public VescWritableInterface, public cdev::CDev, public OutputModuleInterface { public: VescDevice(const char *port); ~VescDevice(); int init(); - void print_info(); + void printStatus(); + int ioctl(device::file_t *filp, int cmd, unsigned long arg); private: void Run(); size_t writeCallback(const uint8_t *buffer, const uint16_t length) override; int setBaudrate(const unsigned baudrate); + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + unsigned num_control_groups_updated) override; static constexpr size_t READ_BUFFER_SIZE{256}; + static constexpr int DISARM_VALUE = 0; + static constexpr int MIN_THROTTLE = 100; + static constexpr int MAX_THROTTLE = 1000; + char _port[20] {}; int _serial_fd{-1}; VescDriver _vesc_driver; + MixingOutput _mixing_output{4, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; }; diff --git a/src/drivers/vesc/vesc_main.cpp b/src/drivers/vesc/vesc_main.cpp index 2ceec2c7fe..930c3a8852 100644 --- a/src/drivers/vesc/vesc_main.cpp +++ b/src/drivers/vesc/vesc_main.cpp @@ -83,7 +83,7 @@ status() } printf("state @ %p\n", g_dev); - g_dev->print_info(); + g_dev->printStatus(); return 0; } diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index e1eff51dc3..0101b42b27 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -439,8 +439,7 @@ bool MixingOutput::update() return true; } -void -MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs) +void MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs) { actuator_outputs.noutputs = num_outputs; @@ -452,8 +451,7 @@ MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_output _outputs_pub.publish(actuator_outputs); } -void -MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs) +void MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs) { MultirotorMixer::saturation_status saturation_status; saturation_status.value = _mixers->get_saturation_status(); @@ -467,8 +465,7 @@ MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs) } } -void -MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs) +void MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs) { // use first valid timestamp_sample for latency tracking for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { @@ -482,37 +479,27 @@ MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_output } } -void -MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS]) +void MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS]) { if (MAX_ACTUATORS < 4) { return; } - if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) { - /* - * Betaflight default motor ordering: - * 4 2 - * ^ - * 3 1 - */ - const uint16_t value_tmp[4] = {values[0], values[1], values[2], values[3] }; - values[0] = value_tmp[3]; - values[1] = value_tmp[0]; - values[2] = value_tmp[1]; - values[3] = value_tmp[2]; - } - - /* else: PX4, no need to reorder - * 3 1 - * ^ - * 2 4 - */ + const uint16_t value_tmp[4] = {values[0], values[1], values[2], values[3]}; + values[reorderedMotorIndex(0)] = value_tmp[0]; + values[reorderedMotorIndex(1)] = value_tmp[1]; + values[reorderedMotorIndex(2)] = value_tmp[2]; + values[reorderedMotorIndex(3)] = value_tmp[3]; } int MixingOutput::reorderedMotorIndex(int index) const { if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) { + /* Betaflight default motor ordering: + * 4 2 + * ^ + * 3 1 + */ switch (index) { case 0: return 1; @@ -524,6 +511,29 @@ int MixingOutput::reorderedMotorIndex(int index) const } } + if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Clockwise) { + /* Clockwise motor ordering: + * 4 1 + * ^ + * 3 2 + */ + switch (index) { + case 0: return 0; + + case 1: return 2; + + case 2: return 3; + + case 3: return 1; + } + } + + /* else: PX4, no need to reorder + * 3 1 + * ^ + * 2 4 + */ + return index; } diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 7cddbc79ee..f582322b2d 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -203,7 +203,8 @@ private: enum class MotorOrdering : int32_t { PX4 = 0, - Betaflight = 1 + Betaflight = 1, + Clockwise = 2 }; struct Command { @@ -281,6 +282,5 @@ private: (ParamFloat) _param_mot_slew_max, (ParamFloat) _param_thr_mdl_fac, ///< thrust to motor control signal modelling factor (ParamInt) _param_mot_ordering - ) }; diff --git a/src/lib/mixer_module/params.c b/src/lib/mixer_module/params.c index 040c77b74b..e12733aeb1 100644 --- a/src/lib/mixer_module/params.c +++ b/src/lib/mixer_module/params.c @@ -29,6 +29,7 @@ PARAM_DEFINE_INT32(MC_AIRMODE, 0); * * @value 0 PX4 * @value 1 Betaflight / Cleanflight + * @value 2 Clockwise * * @group Mixer Output */