Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

uavcan_v1: Add rough ESC Client; Reorg Subscribers

Browse Source
This commit is contained in:
JacobCrabill
2021-02-21 12:33:37 -08:00
committed by Lorenz Meier
parent 00814815f4
commit 170345040a
8 changed files with 271 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan_v1/CMakeLists.txt
-2
View File
@@ -74,8 +74,6 @@ px4_add_module(
SRCS
ParamManager.hpp
ParamManager.cpp
Subscriber.cpp
Subscriber.hpp
Uavcan.cpp
Uavcan.hpp
${SRCS}
src/drivers/uavcan_v1/ParamManager.cpp
+2 -2
View File
@@ -55,7 +55,7 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_
switch (param_type(param_handle)) {
case PARAM_TYPE_INT32: {
int32_t out_val {};
param_set(param_handle, &out_val);
param_get(param_handle, &out_val);
value.integer32.value.elements[0] = out_val;
uavcan_register_Value_1_0_select_integer32_(&value);
break;
@@ -63,7 +63,7 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_
case PARAM_TYPE_FLOAT: {
float out_val {};
param_set(param_handle, &out_val);
param_get(param_handle, &out_val);
value.natural32.value.elements[0] = out_val;
uavcan_register_Value_1_0_select_natural32_(&value);
break;
src/drivers/uavcan_v1/Subscriber.cpp
-102
View File
@@ -1,102 +0,0 @@
/****************************************************************************
*
* 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 Subscriber.cpp
*
* Implements basic functionality of UAVCAN v1 subscriber class
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
#include "Subscriber.hpp"
/** ----- GPS Position Subscription ----- */
void UavcanGpsSubscription::subscribe()
{
// Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_port_id,
reg_drone_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_canard_sub);
}
void UavcanGpsSubscription::callback(const CanardTransfer &receive)
{
// 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 1500.reg.drone.physics.kinematics.geodetic.Point.0.1 '{latitude: 1.234, longitude: 2.34, altitude: {meter: 0.5}}'
PX4_INFO("GpsCallback");
reg_drone_physics_kinematics_geodetic_Point_0_1 geo {};
size_t geo_size_in_bits = receive.payload_size;
reg_drone_physics_kinematics_geodetic_Point_0_1_deserialize_(&geo, (const uint8_t *)receive.payload, &geo_size_in_bits);
double lat = geo.latitude;
double lon = geo.longitude;
double alt = geo.altitude.meter;
PX4_INFO("Latitude: %f, Longitude: %f, Altitude: %f", lat, lon, alt);
/// do something with the data
}
/** ----- Battery Status Subscription ----- */
void UavcanBmsSubscription::subscribe()
{
// Subscribe to messages reg.drone.service.battery.Status.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_port_id,
reg_drone_service_battery_Status_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_canard_sub);
}
void UavcanBmsSubscription::callback(const CanardTransfer &receive)
{
PX4_INFO("BmsCallback");
reg_drone_service_battery_Status_0_1 bat {};
size_t bat_size_in_bits = receive.payload_size;
reg_drone_service_battery_Status_0_1_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bits);
uavcan_si_unit_voltage_Scalar_1_0 V_Min = bat.cell_voltage_min_max[0];
uavcan_si_unit_voltage_Scalar_1_0 V_Max = bat.cell_voltage_min_max[1];
double vmin = static_cast<double>(V_Min.volt);
double vmax = static_cast<double>(V_Max.volt);
PX4_INFO("Min voltage: %f, Max Voltage: %f", vmin, vmax);
/// do something with the data
}
src/drivers/uavcan_v1/Subscribers/Battery.hpp
+83
View File
@@ -0,0 +1,83 @@
/****************************************************************************
*
* 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 Battery.hpp
*
* Defines basic functionality of UAVCAN v1 Battery subscription
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
#pragma once
// DS-15 Specification Messages
#include <reg/drone/service/battery/Parameters_0_1.h>
#include <reg/drone/service/battery/Status_0_1.h>
#include "Subscriber.hpp"
class UavcanBmsSubscription : public UavcanSubscription
{
public:
UavcanBmsSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) :
UavcanSubscription(ins, pmgr, uavcan_pname) { };
void subscribe() override
{
// Subscribe to messages reg.drone.service.battery.Status.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_port_id,
reg_drone_service_battery_Status_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_canard_sub);
};
void callback(const CanardTransfer &receive) override
{
PX4_INFO("BmsCallback");
reg_drone_service_battery_Status_0_1 bat {};
size_t bat_size_in_bits = receive.payload_size;
reg_drone_service_battery_Status_0_1_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bits);
uavcan_si_unit_voltage_Scalar_1_0 V_Min = bat.cell_voltage_min_max[0];
uavcan_si_unit_voltage_Scalar_1_0 V_Max = bat.cell_voltage_min_max[1];
double vmin = static_cast<double>(V_Min.volt);
double vmax = static_cast<double>(V_Max.volt);
PX4_INFO("Min voltage: %f, Max Voltage: %f", vmin, vmax);
/// do something with the data
};
};
src/drivers/uavcan_v1/Subscribers/Gnss.hpp
+99
View File
@@ -0,0 +1,99 @@
/****************************************************************************
*
* 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 Gnss.hpp
*
* Defines basic functionality of UAVCAN v1 GNSS subscription
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
#pragma once
// DS-15 Specification Messages
#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
#include "Subscriber.hpp"
class UavcanGpsSubscription : public UavcanSubscription
{
public:
UavcanGpsSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) :
UavcanSubscription(ins, pmgr, uavcan_pname) { };
void subscribe() override
{
// Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_port_id,
reg_drone_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_canard_sub);
/** TODO: Add additional GPS-data messages: (reg.drone.service.gnss._.0.1.uavcan):
* # A compliant implementation of this service should publish the following subjects:
* #
* # PUBLISHED SUBJECT NAME SUBJECT TYPE TYP. RATE [Hz]
* # point_kinematics reg.drone.physics.kinematics.geodetic.PointStateVarTs 1...100
* # time reg.drone.service.gnss.Time 1...10
* # heartbeat reg.drone.service.gnss.Heartbeat ~1
* # sensor_status reg.drone.service.sensor.Status ~1
*
* Not mentioned, but should also be included: Dilution of Precision
* (reg.drone.service.gnss.DilutionOfPrecision.0.1.uavcan)
* For PX4, only the PointStateVarTs, DilutionOfPrecision, and perhaps Time would be needed
* to publish 'sensor_gps'
*/
};
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 1500.reg.drone.physics.kinematics.geodetic.Point.0.1 '{latitude: 1.234, longitude: 2.34, altitude: {meter: 0.5}}'
PX4_INFO("GpsCallback");
reg_drone_physics_kinematics_geodetic_Point_0_1 geo {};
size_t geo_size_in_bits = receive.payload_size;
reg_drone_physics_kinematics_geodetic_Point_0_1_deserialize_(&geo, (const uint8_t *)receive.payload, &geo_size_in_bits);
double lat = geo.latitude;
double lon = geo.longitude;
double alt = geo.altitude.meter;
PX4_INFO("Latitude: %f, Longitude: %f, Altitude: %f", lat, lon, alt);
/// do something with the data
};
};
src/drivers/uavcan_v1/Subscriber.hpp → src/drivers/uavcan_v1/Subscribers/Subscriber.hpp
+2 -41
View File
@@ -49,11 +49,6 @@
#include <lib/parameters/param.h>
#include "o1heap/o1heap.h"
#include <canard.h>
#include <canard_dsdl.h>
#include <uavcan/_register/Access_1_0.h>
#include <uavcan/_register/List_1_0.h>
#include <uavcan/_register/Value_1_0.h>
@@ -64,13 +59,8 @@
#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
// DS-15 Specification Messages
#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
#include <reg/drone/service/battery/Parameters_0_1.h>
#include <reg/drone/service/battery/Status_0_1.h>
#include "CanardInterface.hpp"
#include "ParamManager.hpp"
#include "../CanardInterface.hpp"
#include "../ParamManager.hpp"
class UavcanSubscription
{
@@ -88,7 +78,6 @@ public:
void updateParam()
{
// Set _port_id from _uavcan_param
uavcan_register_Value_1_0 value;
_param_manager.GetParamByName(_uavcan_param, value);
int32_t new_id = value.integer32.value.elements[0];
@@ -128,31 +117,3 @@ protected:
CanardPortID _port_id {0};
};
class UavcanGpsSubscription : public UavcanSubscription
{
public:
UavcanGpsSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) :
UavcanSubscription(ins, pmgr, uavcan_pname) { };
void subscribe() override;
void callback(const CanardTransfer &msg) override;
private:
};
class UavcanBmsSubscription : public UavcanSubscription
{
public:
UavcanBmsSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) :
UavcanSubscription(ins, pmgr, uavcan_pname) { };
void subscribe() override;
void callback(const CanardTransfer &msg) override;
private:
};
src/drivers/uavcan_v1/Uavcan.cpp
+33 -6
View File
@@ -56,12 +56,8 @@ static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree
UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
_can_interface(interface),
_gps0_sub(_canard_instance, _param_manager, "uavcan.sub.gps.0.id"),
_gps1_sub(_canard_instance, _param_manager, "uavcan.sub.gps.1.id"),
_bms0_sub(_canard_instance, _param_manager, "uavcan.sub.bms.0.id"),
_bms1_sub(_canard_instance, _param_manager, "uavcan.sub.bms.1.id"),
_subscribers{&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub}
_can_interface(interface)//,
// _subscribers{&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub}
{
pthread_mutex_init(&_node_mutex, nullptr);
@@ -389,6 +385,8 @@ void UavcanNode::print_info()
subscriber->printInfo();
}
_mixing_output.printInfo();
pthread_mutex_unlock(&_node_mutex);
}
@@ -479,6 +477,13 @@ void UavcanNode::sendHeartbeat()
}
_uavcan_node_heartbeat_last = transfer.timestamp_usec;
uint16_t outputs[8] {};
outputs[0] = 1000;
outputs[1] = 2000;
outputs[2] = 3000;
outputs[3] = 4000;
_mixing_output.updateOutputs(false, outputs, 4, 1);
}
}
@@ -720,3 +725,25 @@ int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive)
return _sensor_gps_pub.publish(*gps_msg) ? 0 : -1;
}
bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
// Note: This gets called from MixingOutput from within its update() function
// Hence, the mutex lock in UavcanMixingInterface::Run() is in effect
/// TODO: Need esc/servo metadata / bitmask(s)
_esc_controller.update_outputs(stop_motors, outputs, num_outputs);
// _servo_controller.update_outputs(stop_motors, outputs, num_outputs);
return true;
}
void UavcanMixingInterface::Run()
{
pthread_mutex_lock(&_node_mutex);
_mixing_output.update();
_mixing_output.updateSubscriptions(false);
pthread_mutex_unlock(&_node_mutex);
}
src/drivers/uavcan_v1/Uavcan.hpp
+52 -8
View File
@@ -40,6 +40,7 @@
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
@@ -76,7 +77,47 @@
#define PNP2_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_2_0_EXTENT_BYTES_
#include "CanardInterface.hpp"
#include "Subscriber.hpp"
#include "Publishers/Publisher.hpp"
#include "Subscribers/Subscriber.hpp"
#include "Subscribers/Gnss.hpp"
#include "Subscribers/Battery.hpp"
#include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service
/**
* UAVCAN mixing class.
* It is separate from UavcanNode to have 2 WorkItems and therefore allowing independent scheduling
* (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas UavcanNode runs at
* a fixed rate or upon bus updates).
* Both work items are expected to run on the same work queue.
*/
class UavcanMixingInterface : public OutputModuleInterface
{
public:
UavcanMixingInterface(pthread_mutex_t &node_mutex,
UavcanEscController &esc_controller) //, UavcanServoController &servo_controller)
: OutputModuleInterface(MODULE_NAME "-actuators", px4::wq_configurations::uavcan),
_node_mutex(node_mutex),
_esc_controller(esc_controller)/*,
_servo_controller(servo_controller)*/ {}
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
void mixerChanged() override {};
void printInfo() { _mixing_output.printStatus(); }
MixingOutput &mixingOutput() { return _mixing_output; }
protected:
void Run() override;
private:
friend class UavcanNode;
pthread_mutex_t &_node_mutex;
UavcanEscController &_esc_controller;
// UavcanServoController &_servo_controller;
MixingOutput _mixing_output{MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
};
class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem
{
@@ -184,13 +225,6 @@ private:
hrt_abstime _regulated_drone_sensor_bmsstatus_last{0};
CanardTransferID _regulated_drone_sensor_bmsstatus_transfer_id{0};
UavcanGpsSubscription _gps0_sub;
UavcanGpsSubscription _gps1_sub;
UavcanBmsSubscription _bms0_sub;
UavcanBmsSubscription _bms1_sub;
UavcanSubscription *_subscribers[4]; /// TODO: turn into List<UavcanSubscription*>
DEFINE_PARAMETERS(
(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id,
@@ -200,4 +234,14 @@ private:
)
UavcanParamManager _param_manager;
UavcanGpsSubscription _gps0_sub {_canard_instance, _param_manager, "uavcan.sub.gps.0.id"};
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"};
UavcanSubscription *_subscribers[4] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub}; /// TODO: turn into List<UavcanSubscription*>
UavcanEscController _esc_controller {_canard_instance, 22};
UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
};