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

TEMP

Browse Source
This commit is contained in:
Matthias Grob 2021-04-30 09:11:25 +02:00
parent 4c52798b18
commit 912baafd5b
9 changed files with 147 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ROMFS/px4fmu_common/init.d/rc.interface
View File

@ -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}"

5
ROMFS/px4fmu_common/init.d/rcS
View File

@ -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

1
src/drivers/dshot/DShot.cpp
View File

@ -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()

90
src/drivers/vesc/VescSerialDevice.cpp
View File

@ -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<float>(outputs[i]) / 1e3f);
// }
// }
if (!stop_motors) {
_vesc_driver.commandDutyCycle(static_cast<float>(outputs[0]) / 1e3f);
_vesc_driver.commandDutyCycle(static_cast<float>(outputs[1]) / 1e3f, 11);
_vesc_driver.commandDutyCycle(static_cast<float>(outputs[2]) / 1e3f, 7);
_vesc_driver.commandDutyCycle(static_cast<float>(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;
}

16
src/drivers/vesc/VescSerialDevice.hpp
View File

@ -38,24 +38,36 @@
*/
#include "VescDriver/VescDriver.hpp"
#include <drivers/device/device.h>
#include <drivers/drv_mixer.h>
#include <lib/cdev/CDev.hpp>
#include <lib/mixer_module/mixer_module.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
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")};
};

2
src/drivers/vesc/vesc_main.cpp
View File

@ -83,7 +83,7 @@ status()
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
g_dev->printStatus();
return 0;
}

64
src/lib/mixer_module/mixer_module.cpp
View File

@ -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;
}

4
src/lib/mixer_module/mixer_module.hpp
View File

@ -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<px4::params::MOT_SLEW_MAX>) _param_mot_slew_max,
(ParamFloat<px4::params::THR_MDL_FAC>) _param_thr_mdl_fac, ///< thrust to motor control signal modelling factor
(ParamInt<px4::params::MOT_ORDERING>) _param_mot_ordering
)
};

1
src/lib/mixer_module/params.c
View File

@ -29,6 +29,7 @@ PARAM_DEFINE_INT32(MC_AIRMODE, 0);
*
* @value 0 PX4
* @value 1 Betaflight / Cleanflight
* @value 2 Clockwise
*
* @group Mixer Output
*/