diff --git a/.gitmodules b/.gitmodules
index e162f05d54..4aed20c866 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -50,6 +50,15 @@
[submodule "sw/ext/matrix"]
path = sw/ext/matrix
url = https://github.com/PX4/Matrix.git
-[submodule "sw/ext/libcanard"]
- path = sw/ext/libcanard
- url = https://github.com/UAVCAN/libcanard.git
+[submodule "sw/ext/dronecan/DSDL"]
+ path = sw/ext/dronecan/DSDL
+ url = https://github.com/dronecan/DSDL.git
+[submodule "sw/ext/dronecan/dronecan_dsdlc"]
+ path = sw/ext/dronecan/dronecan_dsdlc
+ url = https://github.com/dronecan/dronecan_dsdlc.git
+[submodule "sw/ext/dronecan/pydronecan"]
+ path = sw/ext/dronecan/pydronecan
+ url = https://github.com/dronecan/pydronecan.git
+[submodule "sw/ext/dronecan/libcanard"]
+ path = sw/ext/dronecan/libcanard
+ url = https://github.com/dronecan/libcanard.git
diff --git a/conf/modules/actuators_uavcan.xml b/conf/modules/actuators_uavcan.xml
index bf1cb824bb..d28333a334 100644
--- a/conf/modules/actuators_uavcan.xml
+++ b/conf/modules/actuators_uavcan.xml
@@ -17,6 +17,14 @@
+
+
+
+
+
+
+
+
diff --git a/conf/modules/airspeed_uavcan.xml b/conf/modules/airspeed_uavcan.xml
index 9cfacc14a6..c200f1706b 100644
--- a/conf/modules/airspeed_uavcan.xml
+++ b/conf/modules/airspeed_uavcan.xml
@@ -30,5 +30,9 @@
+
+
+
+
diff --git a/conf/modules/can.xml b/conf/modules/can.xml
index c9137a01c1..a0075dd494 100644
--- a/conf/modules/can.xml
+++ b/conf/modules/can.xml
@@ -7,6 +7,7 @@
To activate a specific CAN peripheral, define flag USE_CANx where x is your CAN peripheral number
+
mcu
diff --git a/conf/modules/power_uavcan.xml b/conf/modules/power_uavcan.xml
index 5c4660a7e5..9382575c46 100644
--- a/conf/modules/power_uavcan.xml
+++ b/conf/modules/power_uavcan.xml
@@ -23,5 +23,11 @@
+
+
+
+
+
+
diff --git a/conf/modules/range_sensor_uavcan.xml b/conf/modules/range_sensor_uavcan.xml
index 6e286ffb59..6b5a2cfcae 100644
--- a/conf/modules/range_sensor_uavcan.xml
+++ b/conf/modules/range_sensor_uavcan.xml
@@ -16,5 +16,9 @@
+
+
+
+
diff --git a/conf/modules/uavcan.xml b/conf/modules/uavcan.xml
index de02f56247..511f05c14c 100644
--- a/conf/modules/uavcan.xml
+++ b/conf/modules/uavcan.xml
@@ -16,14 +16,22 @@
+
+
-
-
+
+
+
+
+
+
+
+
diff --git a/requirements.txt b/requirements.txt
index be1e52b730..3fa03b66e2 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -9,3 +9,6 @@ pyqtgraph
unidecode
matplotlib
distro
+empy==3.3.4
+pexpect
+
diff --git a/sw/airborne/arch/chibios/modules/uavcan/uavcan.c b/sw/airborne/arch/chibios/modules/uavcan/uavcan.c
index 9c0fee744d..559125df8b 100644
--- a/sw/airborne/arch/chibios/modules/uavcan/uavcan.c
+++ b/sw/airborne/arch/chibios/modules/uavcan/uavcan.c
@@ -88,6 +88,11 @@ struct uavcan_iface_t uavcan2 = {
};
#endif
+/*
+ * REMOVE ME - This is only here because the can driver is not yet used
+ */
+void can_init(void) { }
+
/*
* Receiver thread.
*/
diff --git a/sw/airborne/modules/actuators/actuators_uavcan.c b/sw/airborne/modules/actuators/actuators_uavcan.c
index 8678e984eb..39f6fcd24c 100644
--- a/sw/airborne/modules/actuators/actuators_uavcan.c
+++ b/sw/airborne/modules/actuators/actuators_uavcan.c
@@ -30,6 +30,13 @@
#include "modules/core/abi.h"
#include "modules/actuators/actuators.h"
+
+#include "uavcan.equipment.esc.Status.h"
+#include "uavcan.equipment.esc.RawCommand.h"
+#include "uavcan.equipment.actuator.Status.h"
+#include "uavcan.equipment.actuator.ArrayCommand.h"
+#include "uavcan.equipment.device.Temperature.h"
+
/* By default enable the usage of the current sensing in the ESC telemetry */
#ifndef UAVCAN_ACTUATORS_USE_CURRENT
#define UAVCAN_ACTUATORS_USE_CURRENT TRUE
@@ -83,31 +90,6 @@ static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2_NB] = {0};
/* UNUSED value for CMD */
#define UAVCAN_CMD_UNUSED (MIN_PPRZ-1)
-/* uavcan EQUIPMENT_ESC_STATUS message definition */
-#define UAVCAN_EQUIPMENT_ESC_STATUS_ID 1034
-#define UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE (0xA9AF28AEA2FBB254ULL)
-#define UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE ((110 + 7)/8)
-
-/* uavcan EQUIPMENT_ESC_RAWCOMMAND message definition */
-#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID 1030
-#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE (0x217F5C87D7EC951DULL)
-#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE ((285 + 7)/8)
-
-/* uavcan EQUIPMENT_ACTUATOR_STATUS message definition */
-#define UAVCAN_EQUIPMENT_ACTUATOR_STATUS_ID 1011
-#define UAVCAN_EQUIPMENT_ACTUATOR_STATUS_SIGNATURE (0x5E9BBA44FAF1EA04ULL)
-#define UAVCAN_EQUIPMENT_ACTUATOR_STATUS_MAX_SIZE ((64 + 7)/8)
-
-/* uavcan EQUIPMENT_ACTUATOR_ARRAYCOMMAND message definition */
-#define UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID 1010
-#define UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE (0xD8A7486238EC3AF3ULL)
-#define UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE ((484 + 7)/8)
-
-/* uavcan EQUIMPENT_DEVICE_TEMPERATURE message definition */
-#define UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID 1110
-#define UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_SIGNATURE (0x70261C28A94144C6ULL)
-#define UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_MAX_SIZE ((40 + 7)/8)
-
/* private variables */
static bool actuators_uavcan_initialized = false;
static uavcan_event esc_status_ev;
@@ -179,53 +161,101 @@ static void actuators_uavcan_send_esc(struct transport_tx *trans, struct link_de
}
#endif
+static struct actuators_uavcan_telem_t *get_actuator_telem(struct uavcan_iface_t *iface, uint8_t idx) {
+ struct actuators_uavcan_telem_t *telem = NULL;
+
+#ifdef UAVCAN1_TELEM_NB
+ if (iface == &uavcan1 && idx < UAVCAN1_TELEM_NB) {
+ telem = &uavcan1_telem[idx];
+ }
+#endif
+#ifdef UAVCAN2_TELEM_NB
+ if (iface == &uavcan2 && idx < UAVCAN2_TELEM_NB) {
+ telem = &uavcan2_telem[idx];
+ }
+#endif
+
+ return telem;
+}
+
+static uint8_t get_actuator_idx(struct uavcan_iface_t *iface, uint8_t idx) {
+ uint8_t actuator_idx = 255;
+
+#ifdef UAVCAN1_TELEM_NB
+ if(iface == &uavcan1) {
+#ifdef SERVOS_UAVCAN1_NB
+ // First try as RAW COMMAND
+ actuator_idx = get_servo_idx_UAVCAN1(idx);
+#endif
+#ifdef SERVOS_UAVCAN1CMD_NB
+ // Then try as ACTUATOR COMMAND
+ if(idx < SERVOS_UAVCAN1CMD_NB && actuators_uavcan1cmd_values[idx] != UAVCAN_CMD_UNUSED) {
+ actuator_idx = get_servo_idx_UAVCAN1CMD(idx);
+ }
+#endif
+ }
+#endif
+#ifdef UAVCAN2_TELEM_NB
+ if(iface == &uavcan2) {
+#ifdef SERVOS_UAVCAN2_NB
+ // First try as RAW COMMAND
+ actuator_idx = get_servo_idx_UAVCAN2(idx);
+#endif
+#ifdef SERVOS_UAVCAN2CMD_NB
+ // Then try as ACTUATOR COMMAND
+ if(idx < SERVOS_UAVCAN2CMD_NB && actuators_uavcan2cmd_values[idx] != UAVCAN_CMD_UNUSED) {
+ actuator_idx = get_servo_idx_UAVCAN2CMD(idx);
+ }
+#endif
+ }
+#endif
+
+ return actuator_idx;
+}
+
/**
* Whevener an ESC_STATUS message from the EQUIPMENT group is received
*/
static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
{
- uint8_t esc_idx;
- uint16_t tmp_float;
-
- struct actuators_uavcan_telem_t *telem = NULL;
- uint8_t max_id = 0;
-#ifdef UAVCAN1_TELEM_NB
- if (iface == &uavcan1) {
- telem = uavcan1_telem;
- max_id = UAVCAN1_TELEM_NB;
+ // Decode the message
+ struct uavcan_equipment_esc_Status msg;
+ if(uavcan_equipment_esc_Status_decode(transfer, &msg)) {
+ return; // Decoding error
}
-#endif
-#ifdef UAVCAN2_TELEM_NB
- if (iface == &uavcan2) {
- telem = uavcan2_telem;
- max_id = UAVCAN2_TELEM_NB;
- }
-#endif
- canardDecodeScalar(transfer, 105, 5, false, (void *)&esc_idx);
- //Could not find the right interface
- if (esc_idx >= max_id || telem == NULL || max_id == 0) {
+ // Get the correct telemetry
+ struct actuators_uavcan_telem_t *telem = get_actuator_telem(iface, msg.esc_index);
+ if(telem == NULL) {
return;
}
- telem[esc_idx].set = true;
- telem[esc_idx].node_id = transfer->source_node_id;
- telem[esc_idx].timestamp = get_sys_time_float();
- canardDecodeScalar(transfer, 0, 32, false, (void *)&telem[esc_idx].energy);
- canardDecodeScalar(transfer, 32, 16, true, (void *)&tmp_float);
- telem[esc_idx].voltage = canardConvertFloat16ToNativeFloat(tmp_float);
- canardDecodeScalar(transfer, 48, 16, true, (void *)&tmp_float);
- telem[esc_idx].current = canardConvertFloat16ToNativeFloat(tmp_float);
- canardDecodeScalar(transfer, 64, 16, true, (void *)&tmp_float);
- telem[esc_idx].temperature = canardConvertFloat16ToNativeFloat(tmp_float);
- canardDecodeScalar(transfer, 80, 18, true, (void *)&telem[esc_idx].rpm);
+
+ // Set the telemetry (FIXME: copy over fully)
+ telem->set = true;
+ telem->node_id = transfer->source_node_id;
+ telem->timestamp = get_sys_time_float();
+ telem->energy = msg.error_count;
+ telem->voltage = msg.voltage;
+ telem->current = msg.current;
+ telem->temperature = msg.temperature;
+ telem->rpm = msg.rpm;
/* Specification says Kelvin, but some are transmitting in Celsius */
- if (telem[esc_idx].temperature > 230.f) {
- telem[esc_idx].temperature -= 273.15;
+ if (telem->temperature > 230.f) {
+ telem->temperature -= 273.15;
}
+ // Feedback ABI RPM messages
+ struct act_feedback_t feedback = {0};
+ feedback.idx = get_actuator_idx(iface, msg.esc_index);
+ feedback.rpm = telem->rpm;
+ feedback.set.rpm = true;
+
+ // Send ABI message
+ AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
+
#if UAVCAN_ACTUATORS_USE_CURRENT
- // Update total current
+ // Update total current based on ESC telemetry
electrical.current = 0;
#ifdef UAVCAN1_TELEM_NB
for (uint8_t i = 0; i < UAVCAN1_TELEM_NB; ++i) {
@@ -237,46 +267,6 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR
electrical.current += uavcan2_telem[i].current;
}
#endif
-#endif
-
- // Feedback ABI RPM messages
-#ifdef UAVCAN1_TELEM_NB
- if (iface == &uavcan1) {
- struct act_feedback_t feedback = {0};
- feedback.rpm = telem[esc_idx].rpm;
- feedback.set.rpm = true;
-
-#ifdef SERVOS_UAVCAN1_NB
- feedback.idx = get_servo_idx_UAVCAN1(esc_idx);
-#endif
-#ifdef SERVOS_UAVCAN1CMD_NB
- if(esc_idx < SERVOS_UAVCAN1CMD_NB && actuators_uavcan1cmd_values[esc_idx] != UAVCAN_CMD_UNUSED) {
- feedback.idx = get_servo_idx_UAVCAN1CMD(esc_idx);
- }
-#endif
-
- // Send ABI message
- AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
- }
-#endif
-#ifdef UAVCAN2_TELEM_NB
- if (iface == &uavcan2) {
- struct act_feedback_t feedback = {0};
- feedback.rpm = telem[esc_idx].rpm;
- feedback.set.rpm = true;
-
-#ifdef SERVOS_UAVCAN2_NB
- feedback.idx = get_servo_idx_UAVCAN2(esc_idx);
-#endif
-#ifdef SERVOS_UAVCAN2CMD_NB
- if(esc_idx < SERVOS_UAVCAN2CMD_NB && actuators_uavcan2cmd_values[esc_idx] != UAVCAN_CMD_UNUSED) {
- feedback.idx = get_servo_idx_UAVCAN2CMD(esc_idx);
- }
-#endif
-
- // Send ABI message
- AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
- }
#endif
}
@@ -285,73 +275,29 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR
*/
static void actuators_uavcan_actuator_status_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
{
- uint8_t actuator_idx;
- uint16_t tmp_float;
-
- struct actuators_uavcan_telem_t *telem = NULL;
- uint8_t max_id = 0;
-#ifdef UAVCAN1_TELEM_NB
- if (iface == &uavcan1) {
- telem = uavcan1_telem;
- max_id = UAVCAN1_TELEM_NB;
+ // Decode the message
+ struct uavcan_equipment_actuator_Status msg;
+ if(uavcan_equipment_actuator_Status_decode(transfer, &msg)) {
+ return; // Decoding error
}
-#endif
-#ifdef UAVCAN2_TELEM_NB
- if (iface == &uavcan2) {
- telem = uavcan2_telem;
- max_id = UAVCAN2_TELEM_NB;
- }
-#endif
- canardDecodeScalar(transfer, 0, 8, false, (void *)&actuator_idx);
- //Could not find the right interface
- if (actuator_idx >= max_id || telem == NULL || max_id == 0) {
+ // Get the correct telemetry
+ struct actuators_uavcan_telem_t *telem = get_actuator_telem(iface, msg.actuator_id);
+ if(telem == NULL) {
return;
}
- //telem[actuator_idx].set = true;
- canardDecodeScalar(transfer, 8, 16, true, (void *)&tmp_float);
- telem[actuator_idx].position = canardConvertFloat16ToNativeFloat(tmp_float);
+ //telem[msg.actuator_id].set = true;
+ telem->position = msg.position;
-#ifdef UAVCAN1_TELEM_NB
- if (iface == &uavcan1) {
- struct act_feedback_t feedback = {0};
- feedback.position = telem[actuator_idx].position;
- feedback.set.position = true;
+ // Feedback ABI position messages
+ struct act_feedback_t feedback = {0};
+ feedback.idx = get_actuator_idx(iface, msg.actuator_id);
+ feedback.position = telem->position;
+ feedback.set.position = true;
-#ifdef SERVOS_UAVCAN1_NB
- feedback.idx = get_servo_idx_UAVCAN1(actuator_idx);
-#endif
-#ifdef SERVOS_UAVCAN1CMD_NB
- if(actuator_idx < SERVOS_UAVCAN1CMD_NB && actuators_uavcan1cmd_values[actuator_idx] != UAVCAN_CMD_UNUSED) {
- feedback.idx = get_servo_idx_UAVCAN1CMD(actuator_idx);
- }
-#endif
-
- // Send ABI message
- AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
- }
-#endif
-
-#ifdef UAVCAN2_TELEM_NB
- if (iface == &uavcan2) {
- struct act_feedback_t feedback = {0};
- feedback.position = telem[actuator_idx].position;
- feedback.set.position = true;
-
-#ifdef SERVOS_UAVCAN2_NB
- feedback.idx = get_servo_idx_UAVCAN2(actuator_idx);
-#endif
-#ifdef SERVOS_UAVCAN2CMD_NB
- if(actuator_idx < SERVOS_UAVCAN2CMD_NB && actuators_uavcan2cmd_values[actuator_idx] != UAVCAN_CMD_UNUSED) {
- feedback.idx = get_servo_idx_UAVCAN2CMD(actuator_idx);
- }
-#endif
-
- // Send ABI message
- AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
- }
-#endif
+ // Send ABI message
+ AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
}
/**
@@ -359,34 +305,20 @@ static void actuators_uavcan_actuator_status_cb(struct uavcan_iface_t *iface, Ca
*/
static void actuators_uavcan_device_temperature_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
{
- uint16_t device_id;
- uint16_t tmp_float;
-
- struct actuators_uavcan_telem_t *telem = NULL;
- uint8_t max_id = 0;
-#ifdef UAVCAN1_TELEM_NB
- if (iface == &uavcan1) {
- telem = uavcan1_telem;
- max_id = UAVCAN1_TELEM_NB;
+ // Decode the message
+ struct uavcan_equipment_device_Temperature msg;
+ if(uavcan_equipment_device_Temperature_decode(transfer, &msg)) {
+ return; // Decoding error
}
-#endif
-#ifdef UAVCAN2_TELEM_NB
- if (iface == &uavcan2) {
- telem = uavcan2_telem;
- max_id = UAVCAN2_TELEM_NB;
- }
-#endif
-
- canardDecodeScalar(transfer, 0, 16, false, (void*)&device_id);
- //Could not find the right interface
- if (device_id >= max_id || telem == NULL || max_id == 0) {
+ // Get the correct telemetry
+ struct actuators_uavcan_telem_t *telem = get_actuator_telem(iface, msg.device_id);
+ if(telem == NULL) {
return;
}
- telem[device_id].set = true;
- canardDecodeScalar(transfer, 16, 16, false, (void*)&tmp_float);
- telem[device_id].temperature_dev = canardConvertFloat16ToNativeFloat(tmp_float) - 273.15;
+ telem->set = true;
+ telem->temperature_dev = msg.temperature - 273.15;
}
@@ -413,7 +345,7 @@ void actuators_uavcan_init(struct uavcan_iface_t *iface __attribute__((unused)))
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ESC, actuators_uavcan_send_esc);
#endif
- // Set default to not set
+ // Set default to not used
#ifdef SERVOS_UAVCAN1CMD_NB
for(uint8_t i = 0; i < SERVOS_UAVCAN1CMD_NB; i++)
actuators_uavcan1cmd_values[i] = UAVCAN_CMD_UNUSED;
@@ -435,18 +367,20 @@ void actuators_uavcan_init(struct uavcan_iface_t *iface __attribute__((unused)))
*/
void actuators_uavcan_commit(struct uavcan_iface_t *iface, int16_t *values, uint8_t nb)
{
- uint8_t buffer[UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE];
- uint32_t offset = 0;
-
- // Encode the values as 14-bit signed integers
+ // Generate the message
+ struct uavcan_equipment_esc_RawCommand msg = {0};
+ msg.cmd.len = nb;
for (uint8_t i = 0; i < nb; i++) {
- canardEncodeScalar(buffer, offset, 14, (void *)&values[i]);
- offset += 14;
+ msg.cmd.data[i] = values[i];
}
+ // Encode the mssage
+ uint8_t buffer[UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE];
+ uint32_t total_size = uavcan_equipment_esc_RawCommand_encode(&msg, buffer);
+
// Broadcast the raw command message on the interface
uavcan_broadcast(iface, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID,
- CANARD_TRANSFER_PRIORITY_HIGH, buffer, (offset + 7) / 8);
+ CANARD_TRANSFER_PRIORITY_HIGH, buffer, total_size);
}
/**
@@ -454,31 +388,26 @@ void actuators_uavcan_commit(struct uavcan_iface_t *iface, int16_t *values, uint
*/
void actuators_uavcan_cmd_commit(struct uavcan_iface_t *iface, int16_t *values, uint8_t nb)
{
- uint8_t buffer[UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE];
- uint32_t offset = 0;
- uint8_t command_type = 0; // 0:UNITLESS, 1:meter or radian, 2:N or Nm, 3:m/s or rad/s
+ struct uavcan_equipment_actuator_ArrayCommand msg = {0};
+ msg.commands.len = 0;
// Encode the values for each command
for (uint8_t i = 0; i < nb; i++) {
// Skip unused commands
if(values[i] == UAVCAN_CMD_UNUSED || values[i] < MIN_PPRZ || values[i] > MAX_PPRZ)
continue;
-
- // Set the command id
- canardEncodeScalar(buffer, offset, 8, (void*)&i); // 255
- offset += 8;
-
- // Set the command type
- canardEncodeScalar(buffer, offset, 8, (void*)&command_type); // 255
- offset += 8;
-
- // Set the command value
- uint16_t tmp_float = canardConvertNativeFloatToFloat16((float)values[i] / (float)MAX_PPRZ);
- canardEncodeScalar(buffer, offset, 16, (void*)&tmp_float); // 32767
- offset += 16;
+
+ msg.commands.data[msg.commands.len].actuator_id = i;
+ msg.commands.data[msg.commands.len].command_type = UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_UNITLESS;
+ msg.commands.data[msg.commands.len].command_value = (float)values[i] / (float)MAX_PPRZ;
+ msg.commands.len++;
}
+ // Encode the message
+ uint8_t buffer[UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE];
+ uint32_t total_size = uavcan_equipment_actuator_ArrayCommand_encode(&msg, buffer);
+
// Broadcast the raw command message on the interface
uavcan_broadcast(iface, UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID,
- CANARD_TRANSFER_PRIORITY_HIGH, buffer, (offset + 7) / 8);
+ CANARD_TRANSFER_PRIORITY_HIGH, buffer, total_size);
}
diff --git a/sw/airborne/modules/sensors/airspeed_uavcan.c b/sw/airborne/modules/sensors/airspeed_uavcan.c
index b093c9b5ae..5f7408b007 100644
--- a/sw/airborne/modules/sensors/airspeed_uavcan.c
+++ b/sw/airborne/modules/sensors/airspeed_uavcan.c
@@ -26,6 +26,7 @@
#include "airspeed_uavcan.h"
#include "uavcan/uavcan.h"
#include "core/abi.h"
+#include "uavcan.equipment.air_data.RawAirData.h"
/* Enable ABI sending */
#ifndef AIRSPEED_UAVCAN_SEND_ABI
@@ -52,11 +53,6 @@
static Butterworth2LowPass airspeed_filter;
#endif /* USE_AIRSPEED_UAVCAN_LOWPASS_FILTER */
-/* uavcan EQUIPMENT_ESC_STATUS message definition */
-#define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID 1027
-#define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE (0xC77DF38BA122F5DAULL)
-#define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE ((397 + 7)/8)
-
/* Local variables */
struct airspeed_uavcan_t airspeed_uavcan = {0};
static uavcan_event airspeed_uavcan_ev;
@@ -85,21 +81,16 @@ static void airspeed_uavcan_downlink(struct transport_tx *trans, struct link_dev
#endif /* PERIODIC_TELEMETRY */
static void airspeed_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer) {
- uint16_t tmp_float = 0;
- float diff_p;
+
+ struct uavcan_equipment_air_data_RawAirData msg;
+
+ if(uavcan_equipment_air_data_RawAirData_decode(transfer, &msg)) {
+ return; // decode error
+ }
+
+ float diff_p = msg.differential_pressure;
+ float static_air_temp = msg.static_air_temperature;
- /* Decode the message */
- //canardDecodeScalar(transfer, (uint32_t)0, 8, false, (void*)&dest->flags);
- //canardDecodeScalar(transfer, (uint32_t)8, 32, false, (void*)&static_p);
- canardDecodeScalar(transfer, (uint32_t)40, 32, false, (void*)&diff_p);
- //canardDecodeScalar(transfer, (uint32_t)72, 16, false, (void*)&tmp_float);
- //float static_temp = canardConvertFloat16ToNativeFloat(tmp_float);
- //canardDecodeScalar(transfer, (uint32_t)88, 16, false, (void*)&tmp_float);
- //float diff_temp = canardConvertFloat16ToNativeFloat(tmp_float);
- canardDecodeScalar(transfer, (uint32_t)104, 16, false, (void*)&tmp_float);
- float static_air_temp = canardConvertFloat16ToNativeFloat(tmp_float);
- //canardDecodeScalar(transfer, (uint32_t)120, 16, false, (void*)&tmp_float);
- //float pitot_temp = canardConvertFloat16ToNativeFloat(tmp_float);
if(!isnan(diff_p)) {
// Remove the offset and apply a scaling factor
diff --git a/sw/airborne/modules/sensors/power_uavcan.c b/sw/airborne/modules/sensors/power_uavcan.c
index fd2603c3cf..33604ad6d8 100644
--- a/sw/airborne/modules/sensors/power_uavcan.c
+++ b/sw/airborne/modules/sensors/power_uavcan.c
@@ -27,16 +27,9 @@
#include "uavcan/uavcan.h"
#include "modules/energy/electrical.h"
#include "math/pprz_random.h"
+#include "uavcan.equipment.power.BatteryInfo.h"
+#include "uavcan.equipment.power.CircuitStatus.h"
-/* uavcan EQUIPMENT_ESC_STATUS message definition */
-#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID 1092
-#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE (0x249C26548A711966ULL)
-#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE 55
-
-/* uavcan EQUIPMENT_POWER_CIRCUITSTATUS message definition */
-#define UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ID 1091
-#define UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_SIGNATURE (0x8313D33D0DDDA115ULL)
-#define UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_MAX_SIZE 7
/* Default maximum amount of batteries */
#ifndef POWER_UAVCAN_BATTERIES_MAX
@@ -57,40 +50,24 @@
static uavcan_event power_uavcan_ev;
static uavcan_event circuit_uavcan_ev;
-/* Batteries */
-struct uavcan_equipment_power_BatteryInfo {
+struct battery_info
+{
bool set;
uint8_t node_id;
-
- float temperature;
- float voltage;
- float current;
- float average_power_10sec;
- float remaining_capacity_wh;
- float full_charge_capacity_wh;
- float hours_to_full_charge;
- uint16_t status_flags;
- uint8_t state_of_health_pct;
- uint8_t state_of_charge_pct;
- uint8_t state_of_charge_pct_stdev;
- uint8_t battery_id;
- uint32_t model_instance_id;
-// struct { uint8_t len; uint8_t data[31]; }model_name;
+ struct uavcan_equipment_power_BatteryInfo info;
};
-static struct uavcan_equipment_power_BatteryInfo batteries[POWER_UAVCAN_BATTERIES_MAX] = {0};
/* Circuits */
-struct uavcan_equipment_power_CircuitStatus {
+struct circuit_status {
bool set;
uint8_t node_id;
bool is_battery;
-
- uint16_t circuit_id;
- float voltage;
- float current;
- uint8_t error_flags;
+ struct uavcan_equipment_power_CircuitStatus status;
};
-static struct uavcan_equipment_power_CircuitStatus circuits[POWER_UAVCAN_CIRCUITS_MAX] = {0};
+
+
+static struct battery_info batteries[POWER_UAVCAN_BATTERIES_MAX] = {0};
+static struct circuit_status circuits[POWER_UAVCAN_CIRCUITS_MAX] = {0};
/* Battery circuits */
struct uavcan_circuit_battery_t {
@@ -107,8 +84,8 @@ static void power_uavcan_send_power_device(struct transport_tx *trans, struct li
static uint8_t idx = 0;
// Send the circuit status
if(circuits[idx].set) {
- uint8_t cid = circuits[idx].circuit_id;
- pprz_msg_send_POWER_DEVICE(trans, dev, AC_ID, &circuits[idx].node_id, &cid, &circuits[idx].current, &circuits[idx].voltage);
+ uint8_t cid = circuits[idx].status.circuit_id;
+ pprz_msg_send_POWER_DEVICE(trans, dev, AC_ID, &circuits[idx].node_id, &cid, &circuits[idx].status.current, &circuits[idx].status.voltage);
}
// Go to the next
@@ -124,41 +101,16 @@ static void power_uavcan_send_power_device(struct transport_tx *trans, struct li
static void power_uavcan_battery_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer)
{
- uint16_t tmp_float = 0;
-
- /* Decode the message */
- canardDecodeScalar(transfer, (uint32_t)0, 16, true, (void *)&tmp_float);
- float temperature = canardConvertFloat16ToNativeFloat(tmp_float);
- canardDecodeScalar(transfer, (uint32_t)16, 16, true, (void *)&tmp_float);
- float voltage = canardConvertFloat16ToNativeFloat(tmp_float);
- canardDecodeScalar(transfer, (uint32_t)32, 16, true, (void *)&tmp_float);
- float current = canardConvertFloat16ToNativeFloat(tmp_float);
- canardDecodeScalar(transfer, (uint32_t)48, 16, true, (void *)&tmp_float);
- float average_power_10sec = canardConvertFloat16ToNativeFloat(tmp_float);
- canardDecodeScalar(transfer, (uint32_t)64, 16, true, (void *)&tmp_float);
- float remaining_capacity_wh = canardConvertFloat16ToNativeFloat(tmp_float);
- canardDecodeScalar(transfer, (uint32_t)80, 16, true, (void *)&tmp_float);
- float full_charge_capacity_wh = canardConvertFloat16ToNativeFloat(tmp_float);
- canardDecodeScalar(transfer, (uint32_t)96, 16, true, (void *)&tmp_float);
- float hours_to_full_charge = canardConvertFloat16ToNativeFloat(tmp_float);
- uint16_t status_flags = 0;
- canardDecodeScalar(transfer, (uint32_t)112, 11, false, (void *)&status_flags);
- uint8_t state_of_health_pct = 0;
- canardDecodeScalar(transfer, (uint32_t)123, 7, false, (void *)&state_of_health_pct);
- uint8_t state_of_charge_pct = 0;
- canardDecodeScalar(transfer, (uint32_t)130, 7, false, (void *)&state_of_charge_pct);
- uint8_t state_of_charge_pct_stdev = 0;
- canardDecodeScalar(transfer, (uint32_t)137, 7, false, (void *)&state_of_charge_pct_stdev);
- uint8_t battery_id = 0;
- canardDecodeScalar(transfer, (uint32_t)144, 8, false, (void *)&battery_id);
- uint32_t model_instance_id = 0;
- canardDecodeScalar(transfer, (uint32_t)152, 32, false, (void *)&model_instance_id);
+ struct uavcan_equipment_power_BatteryInfo msg;
+ if(uavcan_equipment_power_BatteryInfo_decode(transfer, &msg)) {
+ return; // decode error
+ }
// Search for the battery or free spot
uint8_t battery_idx = POWER_UAVCAN_BATTERIES_MAX;
for (uint8_t i = 0; i < POWER_UAVCAN_BATTERIES_MAX; i++) {
if (batteries[i].set && batteries[i].node_id == transfer->source_node_id &&
- batteries[i].battery_id == battery_id && batteries[i].model_instance_id == model_instance_id) {
+ batteries[i].info.battery_id == msg.battery_id && batteries[i].info.model_instance_id == msg.model_instance_id) {
battery_idx = i;
break;
}
@@ -176,51 +128,33 @@ static void power_uavcan_battery_cb(struct uavcan_iface_t *iface __attribute__((
// Set the battery info
batteries[battery_idx].set = true;
batteries[battery_idx].node_id = transfer->source_node_id;
- batteries[battery_idx].temperature = temperature;
- batteries[battery_idx].voltage = voltage;
- batteries[battery_idx].current = current;
- batteries[battery_idx].average_power_10sec = average_power_10sec;
- batteries[battery_idx].remaining_capacity_wh = remaining_capacity_wh;
- batteries[battery_idx].full_charge_capacity_wh = full_charge_capacity_wh;
- batteries[battery_idx].hours_to_full_charge = hours_to_full_charge;
- batteries[battery_idx].status_flags = status_flags;
- batteries[battery_idx].state_of_health_pct = state_of_health_pct;
- batteries[battery_idx].state_of_charge_pct = state_of_charge_pct;
- batteries[battery_idx].state_of_charge_pct_stdev = state_of_charge_pct_stdev;
- batteries[battery_idx].battery_id = battery_id;
- batteries[battery_idx].model_instance_id = model_instance_id;
+ batteries[battery_idx].info = msg;
// Sum the battery currents
float current_sum = 0;
for (uint8_t i = 0; i < POWER_UAVCAN_BATTERIES_MAX; i++) {
if (batteries[i].set) {
- current_sum += batteries[i].current;
+ current_sum += batteries[i].info.current;
}
}
electrical.current = current_sum;
- if (voltage > 0) {
- electrical.vsupply = voltage;
+ if (msg.voltage > 0) {
+ electrical.vsupply = msg.voltage;
}
}
static void power_uavcan_circuit_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer)
{
- uint16_t tmp_float = 0;
- /* Decode the message */
- uint16_t circuit_id = 0;
- canardDecodeScalar(transfer, (uint32_t)0, 16, false, (void *)&circuit_id);
- canardDecodeScalar(transfer, (uint32_t)16, 16, true, (void *)&tmp_float);
- float voltage = canardConvertFloat16ToNativeFloat(tmp_float);
- canardDecodeScalar(transfer, (uint32_t)32, 16, true, (void *)&tmp_float);
- float current = canardConvertFloat16ToNativeFloat(tmp_float);
- uint8_t error_flags = 0;
- canardDecodeScalar(transfer, (uint32_t)48, 8, false, (void *)&error_flags);
+ struct uavcan_equipment_power_CircuitStatus msg;
+ if(uavcan_equipment_power_CircuitStatus_decode(transfer, &msg)) {
+ return; // decode error
+ }
// Search for the circuit or free spot
uint8_t circuit_idx = POWER_UAVCAN_CIRCUITS_MAX;
for (uint8_t i = 0; i < POWER_UAVCAN_CIRCUITS_MAX; i++) {
- if (circuits[i].set && circuits[i].node_id == transfer->source_node_id && circuits[i].circuit_id == circuit_id) {
+ if (circuits[i].set && circuits[i].node_id == transfer->source_node_id && circuits[i].status.circuit_id == msg.circuit_id) {
circuit_idx = i;
break;
}
@@ -238,21 +172,18 @@ static void power_uavcan_circuit_cb(struct uavcan_iface_t *iface __attribute__((
// Set the circuit info
circuits[circuit_idx].set = true;
circuits[circuit_idx].node_id = transfer->source_node_id;
- circuits[circuit_idx].circuit_id = circuit_id;
- circuits[circuit_idx].voltage = voltage;
- circuits[circuit_idx].current = current;
- circuits[circuit_idx].error_flags = error_flags;
+ circuits[circuit_idx].status = msg;
// Sum the 'battery' circuit currents
float current_sum = 0;
for (uint8_t i = 0; i < POWER_UAVCAN_CIRCUITS_MAX; i++) {
if (circuits[i].set && circuits[i].is_battery) {
- current_sum += circuits[i].current;
+ current_sum += circuits[i].status.current;
}
}
electrical.current = current_sum;
- if (voltage > 0 && circuits[circuit_idx].is_battery) {
- electrical.vsupply = voltage;
+ if (msg.voltage > 0 && circuits[circuit_idx].is_battery) {
+ electrical.vsupply = msg.voltage;
}
}
@@ -262,7 +193,7 @@ void power_uavcan_init(void)
for (uint8_t i = 0; i < sizeof(battery_circuits) / sizeof(struct uavcan_circuit_battery_t); i++) {
circuits[i].set = true;
circuits[i].node_id = battery_circuits[i].node_id;
- circuits[i].circuit_id = battery_circuits[i].circuit_id;
+ circuits[i].status.circuit_id = battery_circuits[i].circuit_id;
circuits[i].is_battery = true;
}
diff --git a/sw/airborne/modules/sensors/range_sensor_uavcan.c b/sw/airborne/modules/sensors/range_sensor_uavcan.c
index c3a33f6938..113b576ac4 100644
--- a/sw/airborne/modules/sensors/range_sensor_uavcan.c
+++ b/sw/airborne/modules/sensors/range_sensor_uavcan.c
@@ -26,21 +26,11 @@
#include "range_sensor_uavcan.h"
#include "uavcan/uavcan.h"
#include "core/abi.h"
+#include "uavcan.equipment.range_sensor.Measurement.h"
-/* uavcan EQUIPMENT_RANGE_SENSOR_MEASUREMENT message definition */
-#define UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID 1050
-#define UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE (0x68FFFE70FC771952ULL)
-#define UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE ((120 + 7)/8)
-
-/* Local structure */
-struct range_sensor_uavcan_t {
- float range;
- uint8_t reading_type;
-};
-
/* Local variables */
-static struct range_sensor_uavcan_t range_sensor_uavcan = {0};
+static struct uavcan_equipment_range_sensor_Measurement range_sensor_uavcan = {0};
static uavcan_event range_sensor_uavcan_ev;
#if PERIODIC_TELEMETRY
@@ -58,21 +48,9 @@ static void range_sensor_uavcan_send_lidar(struct transport_tx *trans, struct li
#endif
static void range_sensor_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer) {
- uint16_t tmp_float = 0;
-
- /* Decode the message */
- //canardDecodeScalar(transfer, (uint32_t)0, 56, false, (void*)&dest->usec);
- //canardDecodeScalar(transfer, (uint32_t)56, 8, false, (void*)&dest->sensor_id);
- //canardDecodeScalar(transfer, (uint32_t)64, 5, true, (void*)(dest->fixed_axis_roll_pitch_yaw + 0));
- //canardDecodeScalar(transfer, (uint32_t)69, 5, true, (void*)(dest->fixed_axis_roll_pitch_yaw + 1));
- //canardDecodeScalar(transfer, (uint32_t)74, 5, true, (void*)(dest->fixed_axis_roll_pitch_yaw + 2));
- //canardDecodeScalar(transfer, (uint32_t)79, 1, false, (void*)&dest->orientation_defined);
- //canardDecodeScalar(transfer, (uint32_t)80, 16, false, (void*)&tmp_float);
- //float fov = canardConvertFloat16ToNativeFloat(tmp_float);
- //canardDecodeScalar(transfer, (uint32_t)96, 5, false, (void*)&dest->sensor_type);
- canardDecodeScalar(transfer, (uint32_t)101, 3, false, (void*)&range_sensor_uavcan.reading_type);
- canardDecodeScalar(transfer, (uint32_t)104, 16, false, (void*)&tmp_float);
- range_sensor_uavcan.range = canardConvertFloat16ToNativeFloat(tmp_float);
+ if(uavcan_equipment_range_sensor_Measurement_decode(transfer, &range_sensor_uavcan)) {
+ return; // decode error
+ }
// Send the range over ABI
if(!isnan(range_sensor_uavcan.range)) {
diff --git a/sw/ext/Makefile b/sw/ext/Makefile
index fae462be92..c56f35ab7a 100644
--- a/sw/ext/Makefile
+++ b/sw/ext/Makefile
@@ -35,7 +35,7 @@ include $(PAPARAZZI_SRC)/conf/Makefile.arm-embedded-toolchain
MY_PYTHON := $(shell echo `which python3`)
MY_MAVLINKTOOLS := $(shell $(MY_PYTHON) -c 'import imp; import future' 2>&1)
-all: libopencm3 luftboot chibios fatfs libsbp TRICAL hacl-c key_generator rustlink ecl matrix mavlink
+all: libopencm3 luftboot chibios fatfs libsbp TRICAL hacl-c key_generator rustlink ecl matrix mavlink dronecan
# update (and init if needed) all submodules
update_submodules:
@@ -112,6 +112,9 @@ opencv_bebop: opencv_bebop.update opencv_bebop.build
opencv_bebop.build:
$(Q)$(MAKE) -C opencv_bebop
+dronecan:
+ $(Q)$(MAKE) -C dronecan
+
clean:
$(Q)if [ -f libopencm3/Makefile ]; then \
$(MAKE) -C $(EXT_DIR)/libopencm3 clean; \
@@ -126,4 +129,7 @@ clean_opencv_bebop:
fi
.NOTPARALLEL: libopencm3 luftboot
-.PHONY: all clean update_submodules libopencm3 luftboot chibios fatfs luftboot_flash libopencm3.build luftboot.build mavlink.build libsbp pprzlink pprzlink.build opencv_bebop opencv_bebop.build clean_opencv_bebop TRICAL
+.PHONY: all clean update_submodules libopencm3 luftboot \
+ chibios fatfs luftboot_flash libopencm3.build luftboot.build \
+ mavlink.build libsbp pprzlink pprzlink.build opencv_bebop \
+ opencv_bebop.build clean_opencv_bebop TRICAL dronecan
diff --git a/sw/ext/dronecan/DSDL b/sw/ext/dronecan/DSDL
new file mode 160000
index 0000000000..30d02e17c5
--- /dev/null
+++ b/sw/ext/dronecan/DSDL
@@ -0,0 +1 @@
+Subproject commit 30d02e17c52920b4f4c5a93857c1bb9b5fa3f325
diff --git a/sw/ext/dronecan/Makefile b/sw/ext/dronecan/Makefile
new file mode 100644
index 0000000000..cb2a20c40f
--- /dev/null
+++ b/sw/ext/dronecan/Makefile
@@ -0,0 +1,67 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Copyright (C) 2012 The Paparazzi Team
+#
+# This file is part of paparazzi.
+#
+# paparazzi is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2, or (at your option)
+# any later version.
+#
+# paparazzi is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with paparazzi; see the file COPYING. If not, write to
+# the Free Software Foundation, 59 Temple Place - Suite 330,
+# Boston, MA 02111-1307, USA.
+
+# The default is to produce a quiet echo of compilation commands
+# Launch with "make Q=''" to get full echo
+
+Q=@
+
+PAPARAZZI_SRC=../../..
+DRONECAN_DIR=$(PAPARAZZI_SRC)/sw/ext/dronecan
+DSDL_DIR := $(DRONECAN_DIR)/DSDL
+
+all: libcanard DSDL dronecan_dsdlc pydronecan
+
+libcanard: libcanard.sync libcanard.update
+
+DSDL : DSDL.sync DSDL.update
+
+dronecan_dsdlc : dronecan_dsdlc.sync dronecan_dsdlc.update dronecan_dsdlc.build
+
+pydronecan : pydronecan.sync pydronecan.update
+
+
+
+dronecan_dsdlc.build:
+ $(eval DSDL_ROOTS_PART := $(sort $(dir $(wildcard $(DSDL_DIR)/*/))))
+ $(eval DSDL_ROOTS := $(filter-out $(DRONECAN_DIR)/DSDL/tests/ $(DRONECAN_DIR)/DSDL/,$(DSDL_ROOTS_PART)))
+ @echo Generating dsdl code from $(DSDL_ROOTS) to $(PAPARAZZI_SRC)/var/include/DSDLcode
+ $(Q) python $(DRONECAN_DIR)/dronecan_dsdlc/dronecan_dsdlc.py -O $(PAPARAZZI_SRC)/var/include/DSDLcode $(DSDL_ROOTS)
+ @echo Done
+
+
+# update (and init if needed) a specific submodule
+%.update:
+ $(Q)if [ -d $(PAPARAZZI_SRC)/.git ]; then \
+ cd $(PAPARAZZI_SRC) && git submodule update --init --recursive sw/ext/dronecan/$*; \
+ fi
+
+# sync a specific submodule
+%.sync:
+ $(Q)if [ -d $(PAPARAZZI_SRC)/.git ]; then \
+ cd $(PAPARAZZI_SRC) && git submodule sync --recursive sw/ext/dronecan/$*; \
+ fi
+
+
+clean:
+ $(Q)rm -r $(PAPARAZZI_SRC)/var/include/DSDLcode
+
+.PHONY: all clean pydronecan dronecan dronecan_dsdlc.build DSDL libcanard
diff --git a/sw/ext/dronecan/dronecan_dsdlc b/sw/ext/dronecan/dronecan_dsdlc
new file mode 160000
index 0000000000..d71b61083d
--- /dev/null
+++ b/sw/ext/dronecan/dronecan_dsdlc
@@ -0,0 +1 @@
+Subproject commit d71b61083d5c8844a8bf97b7fd2e47803e1b6958
diff --git a/sw/ext/dronecan/libcanard b/sw/ext/dronecan/libcanard
new file mode 160000
index 0000000000..6f74bc6765
--- /dev/null
+++ b/sw/ext/dronecan/libcanard
@@ -0,0 +1 @@
+Subproject commit 6f74bc67656882a4ee51966c7c0022d04fa1a3fb
diff --git a/sw/ext/dronecan/pydronecan b/sw/ext/dronecan/pydronecan
new file mode 160000
index 0000000000..19ac176d6c
--- /dev/null
+++ b/sw/ext/dronecan/pydronecan
@@ -0,0 +1 @@
+Subproject commit 19ac176d6c9d00f47c9ed7f99c84e58d8d3d9ec8
diff --git a/sw/ext/libcanard b/sw/ext/libcanard
deleted file mode 160000
index 4244b8c0b3..0000000000
--- a/sw/ext/libcanard
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 4244b8c0b303ac02a20fd603844179809bee59b3