forked from Archive/PX4-Autopilot
uavcan_v1: Working ESC setpoint pub/sub
This commit is contained in:
JacobCrabill
Lorenz Meier
parent
170345040a
commit
35f822fca6
|
|
@ -427,6 +427,11 @@ else
|
|||
fi
|
||||
fi
|
||||
|
||||
if param greater -s UAVCAN_V1_ENABLE 0
|
||||
then
|
||||
uavcan_v1 start
|
||||
fi
|
||||
|
||||
#
|
||||
# Optional board mavlink streams: rc.board_mavlink
|
||||
#
|
||||
|
|
|
|||
|
|
@ -0,0 +1,119 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2014 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file esc.hpp
|
||||
*
|
||||
* UAVCAN <--> ORB bridge for ESC messages:
|
||||
* uavcan.equipment.esc.RawCommand
|
||||
* uavcan.equipment.esc.RPMCommand
|
||||
* uavcan.equipment.esc.Status
|
||||
*
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/mixer_module/mixer_module.hpp>
|
||||
|
||||
#include <reg/drone/service/actuator/common/sp/Vector31_0_1.h>
|
||||
#include <reg/drone/service/common/Readiness_0_1.h>
|
||||
|
||||
class UavcanEscController
|
||||
{
|
||||
public:
|
||||
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
|
||||
static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable
|
||||
static constexpr uint16_t DISARMED_OUTPUT_VALUE = UINT16_MAX;
|
||||
|
||||
UavcanEscController(CanardInstance &ins, const CanardPortID &baseID) :
|
||||
_canard_instance(ins), _base_port_id(baseID) {};
|
||||
|
||||
~UavcanEscController() {};
|
||||
|
||||
void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
|
||||
{
|
||||
if (_base_port_id > 0) {
|
||||
/// TODO: Update readiness
|
||||
// reg_drone_service_common_Readiness_0_1 msg_arming;
|
||||
reg_drone_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
|
||||
|
||||
for (uint8_t i = 0; i < num_outputs; i++) {
|
||||
msg_sp.value[i] = outputs[i];
|
||||
}
|
||||
|
||||
uint8_t esc_sp_payload_buffer[reg_drone_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||
|
||||
CanardTransfer transfer = {
|
||||
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
|
||||
.priority = CanardPriorityNominal,
|
||||
.transfer_kind = CanardTransferKindMessage,
|
||||
.port_id = _base_port_id, // This is the subject-ID.
|
||||
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET.
|
||||
.transfer_id = _esc_setpoint_transfer_id,
|
||||
.payload_size = reg_drone_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
||||
.payload = &esc_sp_payload_buffer,
|
||||
};
|
||||
|
||||
int result = reg_drone_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer,
|
||||
&transfer.payload_size);
|
||||
|
||||
if (result == 0) {
|
||||
// set the data ready in the buffer and chop if needed
|
||||
++_esc_setpoint_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
|
||||
result = canardTxPush(&_canard_instance, &transfer);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Sets the number of rotors
|
||||
*/
|
||||
void set_rotor_count(uint8_t count) { _rotor_count = count; }
|
||||
|
||||
private:
|
||||
/**
|
||||
* ESC status message reception will be reported via this callback.
|
||||
*/
|
||||
void esc_status_sub_cb(const CanardTransfer &msg);
|
||||
|
||||
CanardInstance &_canard_instance;
|
||||
CanardPortID _base_port_id {0};
|
||||
uint8_t _rotor_count {0};
|
||||
CanardTransferID _esc_setpoint_transfer_id {0};
|
||||
};
|
||||
|
|
@ -65,12 +65,13 @@ private:
|
|||
/// TODO:
|
||||
/// use qsort() to order alphabetically by UAVCAN name
|
||||
/// copy over Ken's parameter find/get/set code
|
||||
const UavcanParamBinder _uavcan_params[6] {
|
||||
const UavcanParamBinder _uavcan_params[7] {
|
||||
{"uavcan.pub.esc.0.id", "UCAN1_ESC0_PID"},
|
||||
{"uavcan.pub.servo.0.id", "UCAN1_SERVO0_PID"},
|
||||
{"uavcan.sub.gps.0.id", "UCAN1_GPS0_PID"},
|
||||
{"uavcan.sub.gps.1.id", "UCAN1_GPS1_PID"},
|
||||
{"uavcan.sub.bms.0.id", "UCAN1_BMS0_PID"},
|
||||
{"uavcan.sub.bms.1.id", "UCAN1_BMS1_PID"},
|
||||
{"uavcan.sub.esc.0.id", "UCAN1_ESC_PID"},
|
||||
};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -0,0 +1,92 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Esc.hpp
|
||||
*
|
||||
* Defines basic functionality of UAVCAN v1 ESC setpoint subscription
|
||||
* (For use on a CAN-->PWM node)
|
||||
*
|
||||
* @author Jacob Crabill <jacob@flyvoly.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
// DS-15 Specification Messages
|
||||
#include <reg/drone/service/actuator/common/sp/Vector8_0_1.h>
|
||||
|
||||
#include "Subscriber.hpp"
|
||||
|
||||
class UavcanEscSubscription : public UavcanSubscription
|
||||
{
|
||||
public:
|
||||
UavcanEscSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) :
|
||||
UavcanSubscription(ins, pmgr, uavcan_pname) { };
|
||||
|
||||
void subscribe() override
|
||||
{
|
||||
// Subscribe to messages reg.drone.service.actuator.common.sp.Vector8.0.1
|
||||
canardRxSubscribe(&_canard_instance,
|
||||
CanardTransferKindMessage,
|
||||
_port_id,
|
||||
reg_drone_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_canard_sub);
|
||||
|
||||
/** TODO: Add additional ESC-service messages: reg.drone.service.common.Readiness
|
||||
*/
|
||||
};
|
||||
|
||||
void callback(const CanardTransfer &receive) override
|
||||
{
|
||||
// Test with Yakut:
|
||||
// export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)"
|
||||
// yakut pub 22.reg.drone.service.actuator.common.sp.Vector8.0.1 '{value: {1000, 2000, 3000, 4000}, longitude: 2.34, altitude: {meter: 0.5}}'
|
||||
PX4_INFO("EscCallback");
|
||||
|
||||
reg_drone_service_actuator_common_sp_Vector8_0_1 esc {};
|
||||
size_t esc_size_in_bits = receive.payload_size;
|
||||
reg_drone_service_actuator_common_sp_Vector8_0_1_deserialize_(&esc, (const uint8_t *)receive.payload,
|
||||
&esc_size_in_bits);
|
||||
|
||||
double val1 = static_cast<double>(esc.value[0]);
|
||||
double val2 = static_cast<double>(esc.value[1]);
|
||||
double val3 = static_cast<double>(esc.value[2]);
|
||||
double val4 = static_cast<double>(esc.value[3]);
|
||||
PX4_INFO("values[0-3] = {%f, %f, %f, %f}", val1, val2, val3, val4);
|
||||
/// do something with the data
|
||||
|
||||
/// TODO: Publish to output_control_mc
|
||||
};
|
||||
|
||||
};
|
||||
|
|
@ -478,6 +478,8 @@ void UavcanNode::sendHeartbeat()
|
|||
|
||||
_uavcan_node_heartbeat_last = transfer.timestamp_usec;
|
||||
|
||||
/// TESTING -- Remove before flight!
|
||||
// Since we don't have a mixer file here yet
|
||||
uint16_t outputs[8] {};
|
||||
outputs[0] = 1000;
|
||||
outputs[1] = 2000;
|
||||
|
|
|
|||
|
|
@ -79,8 +79,9 @@
|
|||
#include "CanardInterface.hpp"
|
||||
#include "Publishers/Publisher.hpp"
|
||||
#include "Subscribers/Subscriber.hpp"
|
||||
#include "Subscribers/Gnss.hpp"
|
||||
#include "Subscribers/Battery.hpp"
|
||||
#include "Subscribers/Esc.hpp"
|
||||
#include "Subscribers/Gnss.hpp"
|
||||
#include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service
|
||||
|
||||
/**
|
||||
|
|
@ -238,8 +239,9 @@ private:
|
|||
UavcanGpsSubscription _gps1_sub {_canard_instance, _param_manager, "uavcan.sub.gps.1.id"};
|
||||
UavcanBmsSubscription _bms0_sub {_canard_instance, _param_manager, "uavcan.sub.bms.0.id"};
|
||||
UavcanBmsSubscription _bms1_sub {_canard_instance, _param_manager, "uavcan.sub.bms.1.id"};
|
||||
UavcanEscSubscription _esc_sub {_canard_instance, _param_manager, "uavcan.sub.esc.0.id"};
|
||||
|
||||
UavcanSubscription *_subscribers[4] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub}; /// TODO: turn into List<UavcanSubscription*>
|
||||
UavcanSubscription *_subscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub}; /// TODO: turn into List<UavcanSubscription*>
|
||||
|
||||
UavcanEscController _esc_controller {_canard_instance, 22};
|
||||
|
||||
|
|
|
|||
|
|
@ -93,3 +93,4 @@ PARAM_DEFINE_INT32(UCAN1_GPS0_PID, 0);
|
|||
PARAM_DEFINE_INT32(UCAN1_GPS1_PID, 0);
|
||||
PARAM_DEFINE_INT32(UCAN1_BMS0_PID, 0);
|
||||
PARAM_DEFINE_INT32(UCAN1_BMS1_PID, 0);
|
||||
PARAM_DEFINE_INT32(UCAN1_ESC_PID, 0);
|
||||
|
|
|
|||
Loading…
Reference in New Issue