diff --git a/.gitmodules b/.gitmodules index e386fc5260..71b3af1e13 100644 --- a/.gitmodules +++ b/.gitmodules @@ -50,3 +50,6 @@ [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 diff --git a/conf/airframes/tudelft/neddrone4.xml b/conf/airframes/tudelft/neddrone4.xml new file mode 100644 index 0000000000..dce6ce8a07 --- /dev/null +++ b/conf/airframes/tudelft/neddrone4.xml @@ -0,0 +1,457 @@ + + + + + + Neddrone4 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + +
+ +
+ + + + + +
+ +
+ + + + +
+ +
+ + + + +
+ + diff --git a/conf/modules/actuators_uavcan.xml b/conf/modules/actuators_uavcan.xml new file mode 100644 index 0000000000..df9bed532b --- /dev/null +++ b/conf/modules/actuators_uavcan.xml @@ -0,0 +1,19 @@ + + + + + + UAVCan actuators + + + + uavcan + +
+ +
+ + + + + diff --git a/conf/modules/uavcan.xml b/conf/modules/uavcan.xml new file mode 100644 index 0000000000..e95efd3667 --- /dev/null +++ b/conf/modules/uavcan.xml @@ -0,0 +1,32 @@ + + + + + + UAVCan interface for transmitting/receiving uavcan messages on CAN busses + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + diff --git a/conf/simulator/jsbsim/aircraft/Systems/aerodynamics_nederdrone.xml b/conf/simulator/jsbsim/aircraft/Systems/aerodynamics_nederdrone.xml new file mode 100644 index 0000000000..d02fd3e7ab --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/Systems/aerodynamics_nederdrone.xml @@ -0,0 +1,80 @@ + + + + + + Lift due to alpha + + aero/qbar-psf + metrics/Sw-sqft + + aero/alpha-deg + + -180 0.14 + -140 -0.80 + -110 -0.63 + -102 -0.74 + -98 -0.64 + -90 0.07 + -79 1.15 + -76 1.2 + -72 1.1 + -45 0.8 + 0 0.0 + 45 0.8 + 90 0.0 + 135 -0.8 + 180 0.0 + +
+ + + + + + + + + Drag + + aero/qbar-psf + metrics/Sw-sqft + + aero/alpha-deg + + -180 1.7 + -160 1.62 + -100 0.4 + -95 0.27 + -92 0.24 + -88 0.225 + -83 0.235 + -78 0.29 + -65 0.6128 + -20 1.36 + -4 1.45 + 15 1.35 + 30 1.12 + +
+ 1.2 + + + + + + + + + Side force due to beta + + aero/qbar-psf + metrics/Sw-sqft + aero/beta-rad + -4 + + + + + + \ No newline at end of file diff --git a/conf/simulator/jsbsim/aircraft/nederdrone.xml b/conf/simulator/jsbsim/aircraft/nederdrone.xml new file mode 100644 index 0000000000..b6e282f89e --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/nederdrone.xml @@ -0,0 +1,606 @@ + + + + + + Ewoud Smeur + 23-04-2020 + Version 0.9 - beta + Nederdrone + + + + 1 + 2 + 0.25 + 0 + 0 + 0 + 0 + 90 + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + + + 2.25 + 0.3322 + 4.0 + 0. + 0. + 0. + 13.0 + + 0 + 0 + 0 + + + + + + + 0.41 + 0.2 + -0.11 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0.41 + -0.2 + -0.11 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0.11 + 0.2 + 0.41 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0.11 + -0.2 + 0.41 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + + + + + fcs/m1 + + 0 + 1 + + 18.0 + fcs/m1_lag + + + fcs/m2 + + 0 + 1 + + 18.0 + fcs/m2_lag + + + fcs/m3 + + 0 + 1 + + 18.0 + fcs/m3_lag + + + fcs/m4 + + 0 + 1 + + 18.0 + fcs/m4_lag + + + fcs/m5 + + 0 + 1 + + 18.0 + fcs/m5_lag + + + fcs/m6 + + 0 + 1 + + 18.0 + fcs/m6_lag + + + fcs/m7 + + 0 + 1 + + 18.0 + fcs/m7_lag + + + fcs/m8 + + 0 + 1 + + 18.0 + fcs/m8_lag + + + fcs/m9 + + 0 + 1 + + 18.0 + fcs/m9_lag + + + fcs/m10 + + 0 + 1 + + 18.0 + fcs/m10_lag + + + fcs/m11 + + 0 + 1 + + 18.0 + fcs/m11_lag + + + fcs/m12 + + 0 + 1 + + 18.0 + fcs/m12_lag + + + + + + + fcs/m1 + fcs/m2 + fcs/m3 + fcs/m4 + fcs/m5 + fcs/m6 + fcs/m7 + fcs/m8 + fcs/m9 + fcs/m10 + fcs/m11 + fcs/m12 + fcs/m1_lag + fcs/m2_lag + fcs/m3_lag + fcs/m4_lag + fcs/m5_lag + fcs/m6_lag + fcs/m7_lag + fcs/m8_lag + fcs/m9_lag + fcs/m10_lag + fcs/m11_lag + fcs/m12_lag + fcs/ail1 + fcs/ail2 + fcs/ail3 + fcs/ail4 + fcs/flap1 + fcs/flap2 + fcs/flap3 + fcs/flap4 + + + + + + + + fcs/m1_lag + 3.822 + + + + -0.141 + -1.105 + 0 + + + 0 + 0 + -1 + + + + + + fcs/m2_lag + 3.822 + + + + -0.141 + -0.735 + 0 + + + 0 + 0 + -1 + + + + + + fcs/m3_lag + 3.822 + + + + -0.141 + -0.320 + 0 + + + 0 + 0 + -1 + + + + + + fcs/m4_lag + 3.822 + + + + -0.141 + 0.320 + 0 + + + 0 + 0 + -1 + + + + + + fcs/m5_lag + 3.822 + + + + -0.141 + 0.735 + 0 + + + 0 + 0 + -1 + + + + + + fcs/m6_lag + 3.822 + + + + -0.141 + 1.105 + 0 + + + 0 + 0 + -1 + + + + + + fcs/m7_lag + 3.822 + + + + 0.141 + 1.105 + 0 + + + 0 + 0 + -1 + + + + + + fcs/m8_lag + 3.822 + + + + 0.141 + 0.735 + 0 + + + 0 + 0 + -1 + + + + + + fcs/m9_lag + 3.822 + + + + 0.141 + 0.320 + 0 + + + 0 + 0 + -1 + + + + + + fcs/m10_lag + 3.822 + + + + 0.141 + -0.320 + 0 + + + 0 + 0 + -1 + + + + + + fcs/m11_lag + 3.822 + + + + 0.141 + -0.735 + 0 + + + 0 + 0 + -1 + + + + + + fcs/m12_lag + 3.822 + + + + 0.141 + -1.105 + 0 + + + 0 + 0 + -1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + fcs/m1_lag + fcs/m3_lag + fcs/m5_lag + fcs/m7_lag + fcs/m9_lag + fcs/m11_lag + + + fcs/m2_lag + fcs/m4_lag + fcs/m6_lag + fcs/m8_lag + fcs/m10_lag + fcs/m12_lag + + -1 + + + 1.2 + + + + 0 + -1 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/m1_lag + fcs/m3_lag + fcs/m5_lag + fcs/m7_lag + fcs/m9_lag + fcs/m11_lag + + + fcs/m2_lag + fcs/m4_lag + fcs/m6_lag + fcs/m8_lag + fcs/m10_lag + fcs/m12_lag + + -1 + + + 1.2 + + + 0 + 1 + 0 + + + -1 + 0 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml new file mode 100644 index 0000000000..48028d1e5d --- /dev/null +++ b/conf/telemetry/highspeed_rotorcraft.xml @@ -0,0 +1,230 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 14bd8aac06..9a6235c893 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -285,6 +285,17 @@ gui_color="white" release="997fa535902c4d8f73bc34c02c862652cd47cae5" /> + + * + * 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. + */ +/** + * @file arch/chibios/modules/uavcan/uavcan.c + * Interface from actuators to ChibiOS CAN driver using UAVCan + * + */ + +#include "uavcan.h" + +#ifndef UAVCAN_NODE_ID +#define UAVCAN_NODE_ID 100 +#endif + +#ifndef UAVCAN_BAUDRATE +#define UAVCAN_BAUDRATE 1000000 +#endif + +static uavcan_event *uavcan_event_hd = NULL; + +#if UAVCAN_USE_CAN1 +#ifndef UAVCAN_CAN1_NODE_ID +#define UAVCAN_CAN1_NODE_ID UAVCAN_NODE_ID +#endif + +#ifndef UAVCAN_CAN1_BAUDRATE +#define UAVCAN_CAN1_BAUDRATE UAVCAN_BAUDRATE +#endif + +static THD_WORKING_AREA(uavcan1_rx_wa, 1024*2); +static THD_WORKING_AREA(uavcan1_tx_wa, 1024*2); + +struct uavcan_iface_t uavcan1 = { + .can_driver = &CAND1, + .can_cfg = { + CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP, + CAN_BTR_SJW(0) | CAN_BTR_TS2(1) | + CAN_BTR_TS1(14) | CAN_BTR_BRP((STM32_PCLK1/18)/UAVCAN_CAN1_BAUDRATE - 1) + }, + .thread_rx_wa = uavcan1_rx_wa, + .thread_rx_wa_size = sizeof(uavcan1_rx_wa), + .thread_tx_wa = uavcan1_tx_wa, + .thread_tx_wa_size = sizeof(uavcan1_tx_wa), + .node_id = UAVCAN_CAN1_NODE_ID, + .transfer_id = 0, + .initialized = false +}; +#endif + +#if UAVCAN_USE_CAN2 +#ifndef UAVCAN_CAN2_NODE_ID +#define UAVCAN_CAN2_NODE_ID UAVCAN_NODE_ID +#endif + +#ifndef UAVCAN_CAN2_BAUDRATE +#define UAVCAN_CAN2_BAUDRATE UAVCAN_BAUDRATE +#endif + +static THD_WORKING_AREA(uavcan2_rx_wa, 1024*2); +static THD_WORKING_AREA(uavcan2_tx_wa, 1024*2); + +struct uavcan_iface_t uavcan2 = { + .can_driver = &CAND2, + .can_cfg = { + CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP, + CAN_BTR_SJW(0) | CAN_BTR_TS2(1) | + CAN_BTR_TS1(14) | CAN_BTR_BRP((STM32_PCLK1/18)/UAVCAN_CAN2_BAUDRATE - 1) + }, + .thread_rx_wa = uavcan2_rx_wa, + .thread_rx_wa_size = sizeof(uavcan2_rx_wa), + .thread_tx_wa = uavcan2_tx_wa, + .thread_tx_wa_size = sizeof(uavcan2_tx_wa), + .node_id = UAVCAN_CAN2_NODE_ID, + .transfer_id = 0, + .initialized = false +}; +#endif + +/* + * Receiver thread. + */ +static THD_FUNCTION(uavcan_rx, p) { + event_listener_t el; + CANRxFrame rx_msg; + CanardCANFrame rx_frame; + struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p; + + chRegSetThreadName("uavcan_rx"); + chEvtRegister(&iface->can_driver->rxfull_event, &el, EVENT_MASK(0)); + while (true) { + if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0) + continue; + chMtxLock(&iface->mutex); + + // Wait until a CAN message is received + while (canReceive(iface->can_driver, CAN_ANY_MAILBOX, &rx_msg, TIME_IMMEDIATE) == MSG_OK) { + // Process message. + const uint32_t timestamp = TIME_I2US(chVTGetSystemTimeX()); + memcpy(rx_frame.data, rx_msg.data8, 8); + rx_frame.data_len = rx_msg.DLC; + if(rx_msg.IDE) { + rx_frame.id = CANARD_CAN_FRAME_EFF | rx_msg.EID; + } else { + rx_frame.id = rx_msg.SID; + } + + // Let canard handle the frame + canardHandleRxFrame(&iface->canard, &rx_frame, timestamp); + } + chMtxUnlock(&iface->mutex); + } + chEvtUnregister(&iface->can_driver->rxfull_event, &el); +} + +/* + * Transmitter thread. + */ +static THD_FUNCTION(uavcan_tx, p) { + event_listener_t txc, txe, txr; + struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p; + uint8_t err_cnt = 0; + + chRegSetThreadName("uavcan_tx"); + chEvtRegister(&iface->can_driver->txempty_event, &txc, EVENT_MASK(0)); + chEvtRegister(&iface->can_driver->error_event, &txe, EVENT_MASK(1)); + chEvtRegister(&iface->tx_request, &txr, EVENT_MASK(2)); + + while (true) { + eventmask_t evts = chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)); + // Succesfull transmit + if (evts == 0) + continue; + + // Transmit error + if(evts & EVENT_MASK(1)) + { + chEvtGetAndClearFlags(&txe); + continue; + } + + chMtxLock(&iface->mutex); + for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&iface->canard)) != NULL;) { + CANTxFrame tx_msg; + tx_msg.DLC = txf->data_len; + memcpy(tx_msg.data8, txf->data, 8); + tx_msg.EID = txf->id & CANARD_CAN_EXT_ID_MASK; + tx_msg.IDE = CAN_IDE_EXT; + tx_msg.RTR = CAN_RTR_DATA; + if (canTransmit(iface->can_driver, CAN_ANY_MAILBOX, &tx_msg, TIME_IMMEDIATE) == MSG_OK) { + err_cnt = 0; + canardPopTxQueue(&iface->canard); + } else { + // After 5 retries giveup + if(err_cnt >= 5) { + err_cnt = 0; + canardPopTxQueue(&iface->canard); + continue; + } + + // Timeout - just exit and try again later + chMtxUnlock(&iface->mutex); + err_cnt++; + canardPopTxQueue(&iface->canard); //FIXME (This needs to be here, don't know why) + chThdSleepMilliseconds(err_cnt * 5); + chMtxLock(&iface->mutex); + continue; + } + } + chMtxUnlock(&iface->mutex); + } +} + +/** + * Whenever a valid and 'accepted' transfer is received + */ +static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer) { + struct uavcan_iface_t *iface = (struct uavcan_iface_t *)ins->user_reference; + + // Go through all registered callbacks and call function callback if found + for(uavcan_event *ev = uavcan_event_hd; ev; ev = ev->next) { + if(transfer->data_type_id == ev->data_type_id) + ev->cb(iface, transfer); + } +} + +/** + * If we should accept this transfer + */ +static bool shouldAcceptTransfer(const CanardInstance* ins __attribute__((unused)), + uint64_t* out_data_type_signature, + uint16_t data_type_id, + CanardTransferType transfer_type __attribute__((unused)), + uint8_t source_node_id __attribute__((unused))) { + + // Go through all registered callbacks and return signature if found + for(uavcan_event *ev = uavcan_event_hd; ev; ev = ev->next) { + if(data_type_id == ev->data_type_id) { + *out_data_type_signature = ev->data_type_signature; + return true; + } + } + + // No callback found return + return false; +} + +/** + * Initialize uavcan interface + */ +static void uavcanInitIface(struct uavcan_iface_t *iface) { + // Initialize mutexes/events for multithread locking + chMtxObjectInit(&iface->mutex); + chEvtObjectInit(&iface->tx_request); + + // Initialize canard + canardInit(&iface->canard, iface->canard_memory_pool, sizeof(iface->canard_memory_pool), + onTransferReceived, shouldAcceptTransfer, iface); + // Update the uavcan node ID + canardSetLocalNodeID(&iface->canard, iface->node_id); + + // Start the can interface + canStart(iface->can_driver, &iface->can_cfg); + + // Start the receiver and transmitter thread + chThdCreateStatic(iface->thread_rx_wa, iface->thread_rx_wa_size, NORMALPRIO + 8, uavcan_rx, (void*)iface); + chThdCreateStatic(iface->thread_tx_wa, iface->thread_tx_wa_size, NORMALPRIO + 7, uavcan_tx, (void*)iface); + iface->initialized = true; +} + +/** + * Initialize all uavcan interfaces + */ +void uavcan_init(void) +{ +#if UAVCAN_USE_CAN1 + uavcanInitIface(&uavcan1); +#endif +#if UAVCAN_USE_CAN2 + uavcanInitIface(&uavcan2); +#endif +} + +/** + * Bind to a receiving message from uavcan + */ +void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb) +{ + // Configure the event + ev->data_type_id = data_type_id, + ev->data_type_signature = data_type_signature, + ev->cb = cb, + ev->next = uavcan_event_hd; + + // Switch the head + uavcan_event_hd = ev; +} + +/** + * Broadcast an uavcan message to a specific interface + */ +void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority, const void* payload, + uint16_t payload_len) { + if(!iface->initialized) return; + + chMtxLock(&iface->mutex); + canardBroadcast(&iface->canard, + data_type_signature, + data_type_id, &iface->transfer_id, + priority, payload, payload_len); + chMtxUnlock(&iface->mutex); + chEvtBroadcast(&iface->tx_request); +} diff --git a/sw/airborne/arch/chibios/modules/uavcan/uavcan.h b/sw/airborne/arch/chibios/modules/uavcan/uavcan.h new file mode 100644 index 0000000000..aa844b2f10 --- /dev/null +++ b/sw/airborne/arch/chibios/modules/uavcan/uavcan.h @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2021 Freek van Tienen + * + * 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. + */ +/** + * @file arch/chibios/modules/uavcan/uavcan.h + * Interface with uavcan using the Chibios can interfaces. + * This uses multithreading and starts a transmit and receive thread per interface. + */ +#ifndef MODULES_UAVCAN_ARCH_H +#define MODULES_UAVCAN_ARCH_H + +#include +#include +#include + +/** uavcan interface structure */ +struct uavcan_iface_t { + CANDriver *can_driver; + CANConfig can_cfg; + + event_source_t tx_request; + mutex_t mutex; + void *thread_rx_wa; + void *thread_tx_wa; + void *thread_uavcan_wa; + size_t thread_rx_wa_size; + size_t thread_tx_wa_size; + size_t thread_uavcan_wa_size; + + uint8_t node_id; + CanardInstance canard; + uint8_t canard_memory_pool[1024*2]; + + uint8_t transfer_id; + bool initialized; +}; + +/** Generic uavcan callback definition */ +typedef void (*uavcan_callback)(struct uavcan_iface_t *iface, CanardRxTransfer* transfer); + +/** Main uavcan event structure for registering/calling callbacks */ +struct uavcan_event_t { + uint16_t data_type_id; + uint64_t data_type_signature; + uavcan_callback cb; + struct uavcan_event_t *next; +}; +typedef struct uavcan_event_t uavcan_event; + +/** uavcan interfaces */ +#if UAVCAN_USE_CAN1 +extern struct uavcan_iface_t uavcan1; +#endif +#if UAVCAN_USE_CAN2 +extern struct uavcan_iface_t uavcan2; +#endif + +/** uavcan external functions */ +void uavcan_init(void); +void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb); +void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority, + const void* payload, uint16_t payload_len); + +#endif /* MODULES_UAVCAN_ARCH_H */ \ No newline at end of file diff --git a/sw/airborne/arch/sim/modules/uavcan/uavcan.c b/sw/airborne/arch/sim/modules/uavcan/uavcan.c new file mode 100644 index 0000000000..5462a511d2 --- /dev/null +++ b/sw/airborne/arch/sim/modules/uavcan/uavcan.c @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2020 Freek van Tienen + * + * 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. + */ +/** + * @file arch/chibios/subsystems/actuators/actuators_uavcan_arch.c + * Interface from actuators to ChibiOS CAN driver using UAVCan + * + */ + +#include "uavcan.h" + +/** uavcan interfaces */ +#if UAVCAN_USE_CAN1 +struct uavcan_iface_t uavcan1; +#endif +#if UAVCAN_USE_CAN2 +struct uavcan_iface_t uavcan2; +#endif + +/** uavcan external functions */ +void uavcan_init(void) {} +void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb) {} +void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority, + const void* payload, uint16_t payload_len) {} \ No newline at end of file diff --git a/sw/airborne/arch/sim/modules/uavcan/uavcan.h b/sw/airborne/arch/sim/modules/uavcan/uavcan.h new file mode 100644 index 0000000000..0d719f3644 --- /dev/null +++ b/sw/airborne/arch/sim/modules/uavcan/uavcan.h @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2021 Freek van Tienen + * + * 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. + */ +/** + * @file arch/sim/modules/uavcan/uavcan.h + * Stub interface for simulation + */ +#ifndef MODULES_UAVCAN_ARCH_H +#define MODULES_UAVCAN_ARCH_H + +#include +#include + +/** uavcan interface structure */ +struct uavcan_iface_t {}; + +/** Generic uavcan callback definition */ +struct CanardRxTransfer_t {}; +typedef struct CanardRxTransfer_t CanardRxTransfer; +typedef void (*uavcan_callback)(struct uavcan_iface_t *iface, CanardRxTransfer* transfer); + +/** Main uavcan event structure for registering/calling callbacks */ +struct uavcan_event_t {}; +typedef struct uavcan_event_t uavcan_event; + +/** uavcan interfaces */ +#if UAVCAN_USE_CAN1 +extern struct uavcan_iface_t uavcan1; +#endif +#if UAVCAN_USE_CAN2 +extern struct uavcan_iface_t uavcan2; +#endif + +/** uavcan external functions */ +void uavcan_init(void); +void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb); +void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority, + const void* payload, uint16_t payload_len); + +#endif /* MODULES_UAVCAN_ARCH_H */ \ No newline at end of file diff --git a/sw/airborne/arch/sim/subsystems/actuators/actuators_uavcan_arch.c b/sw/airborne/arch/sim/subsystems/actuators/actuators_uavcan_arch.c new file mode 100644 index 0000000000..286d3faa15 --- /dev/null +++ b/sw/airborne/arch/sim/subsystems/actuators/actuators_uavcan_arch.c @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2020 Freek van Tienen + * + * 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. + */ + +/** @file arch/sim/subsystems/actuators/actuators_uavcan_arch.c + * dummy servos handling for sim + */ + +#include "subsystems/actuators/actuators_uavcan_arch.h" + +void actuators_uavcan_arch_init(void) +{ + +} \ No newline at end of file diff --git a/sw/airborne/arch/sim/subsystems/actuators/actuators_uavcan_arch.h b/sw/airborne/arch/sim/subsystems/actuators/actuators_uavcan_arch.h new file mode 100644 index 0000000000..dd1018aac4 --- /dev/null +++ b/sw/airborne/arch/sim/subsystems/actuators/actuators_uavcan_arch.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2020 Freek van Tienen + * + * 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. + */ + +/** @file arch/sim/subsystems/actuators/actuators_uavcan_arch.h + * dummy servos handling for sim + */ + +#ifndef ACTUATORS_UAVCAN_ARCH_H +#define ACTUATORS_UAVCAN_ARCH_H + +#define SERVOS_TICS_OF_USEC(_v) (_v) + +#define ActuatorUavcanSet(_i, _v) {} +#define ActuatorsUavcanCommit() {} + +extern void actuators_uavcan_arch_init(void); + +#endif /* ACTUATORS_UAVCAN_ARCH_H */ diff --git a/sw/airborne/boards/px4fmu/chibios/v5.0/mcuconf.h b/sw/airborne/boards/px4fmu/chibios/v5.0/mcuconf.h index 0b89117860..e84e3b68f5 100644 --- a/sw/airborne/boards/px4fmu/chibios/v5.0/mcuconf.h +++ b/sw/airborne/boards/px4fmu/chibios/v5.0/mcuconf.h @@ -238,7 +238,7 @@ #endif #define STM32_I2C_BUSY_TIMEOUT 50 #define STM32_I2C_I2C1_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 0) -#define STM32_I2C_I2C1_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 6) +#define STM32_I2C_I2C1_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 7) #define STM32_I2C_I2C2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 2) #define STM32_I2C_I2C2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 7) #define STM32_I2C_I2C3_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 2) diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index c752a88b01..9dcba98470 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -612,6 +612,13 @@ static inline void int32_vect_zero(int32_t *a, const int n) for (i = 0; i < n; i++) { a[i] = 0.; } } +/** a = 0 */ +static inline void int16_vect_zero(int16_t *a, const int n) +{ + int i; + for (i = 0; i < n; i++) { a[i] = 0.; } +} + /** a = v * ones(n,1) */ static inline void int32_vect_set_value(int32_t *a, const int32_t v, const int n) { diff --git a/sw/airborne/subsystems/actuators/actuators_uavcan.c b/sw/airborne/subsystems/actuators/actuators_uavcan.c new file mode 100644 index 0000000000..079a60678a --- /dev/null +++ b/sw/airborne/subsystems/actuators/actuators_uavcan.c @@ -0,0 +1,145 @@ +/* + * Copyright (C) 2021 Freek van Tienen + * + * 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. + */ +/** + * @file subsystems/actuators/actuators_uavcan.c + * UAVCan actuators using RAWCOMMAND message and ESC_STATUS telemetry + * + */ + +#include "actuators_uavcan.h" +#include "subsystems/electrical.h" + +/* uavcan ESC status telemetry structure */ +struct actuators_uavcan_telem_t { + float voltage; + float current; + float temperature; + int32_t rpm; + uint32_t energy; +}; + +#ifdef SERVOS_UAVCAN1_NB +int16_t actuators_uavcan1_values[SERVOS_UAVCAN1_NB]; +static struct actuators_uavcan_telem_t uavcan1_telem[SERVOS_UAVCAN1_NB]; +#endif +#ifdef SERVOS_UAVCAN2_NB +int16_t actuators_uavcan2_values[SERVOS_UAVCAN2_NB]; +static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2_NB]; +#endif + +/* 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) + +static bool actuators_uavcan_initialized = false; +static uavcan_event esc_status_ev; + + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" + +static void actuators_uavcan_send_esc(struct transport_tx *trans, struct link_device *dev) +{ + /*static uint8_t esc_idx = 0; + float power = uavcan_telem[esc_idx].current * uavcan_telem[esc_idx].voltage; + float rpm = uavcan_telem[esc_idx].rpm; + float energy = uavcan_telem[esc_idx].energy; + pprz_msg_send_ESC(trans, dev, AC_ID, &uavcan_telem[esc_idx].current, &electrical.vsupply, &power, + &rpm, &uavcan_telem[esc_idx].voltage, &energy, &esc_idx); + esc_idx++; + + if(esc_idx >= ACTUATORS_UAVCAN_NB) + esc_idx = 0;*/ +} +#endif + +/** + * 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; + + canardDecodeScalar(transfer, 105, 5, false, (void*)&esc_idx); + if(iface == &can2_iface) + esc_idx += 10; + if(esc_idx >= ACTUATORS_UAVCAN_NB) + break; + + canardDecodeScalar(transfer, 0, 32, false, (void*)&uavcan_telem[esc_idx].energy); + canardDecodeScalar(transfer, 32, 16, true, (void*)&tmp_float); + uavcan_telem[esc_idx].voltage = canardConvertFloat16ToNativeFloat(tmp_float); + canardDecodeScalar(transfer, 48, 16, true, (void*)&tmp_float); + uavcan_telem[esc_idx].current = canardConvertFloat16ToNativeFloat(tmp_float); + canardDecodeScalar(transfer, 64, 16, true, (void*)&tmp_float); + uavcan_telem[esc_idx].temperature = canardConvertFloat16ToNativeFloat(tmp_float); + canardDecodeScalar(transfer, 80, 18, true, (void*)&uavcan_telem[esc_idx].rpm); + + // Update total current + electrical.current = 0; + for(uint8_t i = 0; i < ACTUATORS_UAVCAN_NB; ++i) + electrical.current += uavcan_telem[i].current;*/ +} + +/** + * Initialize an uavcan interface + */ +void actuators_uavcan_init(struct uavcan_iface_t* iface __attribute__((unused))) +{ + // Check if not already initialized (for multiple interfaces, needs only 1) + if(actuators_uavcan_initialized) return; + + // Bind uavcan ESC_STATUS message from EQUIPMENT + uavcan_bind(UAVCAN_EQUIPMENT_ESC_STATUS_ID, UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, &esc_status_ev, &actuators_uavcan_esc_status_cb); + + // Configure telemetry +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ESC, actuators_uavcan_send_esc); +#endif + + // Set initialization + actuators_uavcan_initialized = true; +} + +/** + * Commit actuator values to the uavcan interface + */ +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 + for(uint8_t i = 0; i < nb; i++) { + canardEncodeScalar(buffer, offset, 14, (void *)&values[i]); + offset += 14; + } + + // 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); +} diff --git a/sw/airborne/subsystems/actuators/actuators_uavcan.h b/sw/airborne/subsystems/actuators/actuators_uavcan.h new file mode 100644 index 0000000000..91ae8576be --- /dev/null +++ b/sw/airborne/subsystems/actuators/actuators_uavcan.h @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2021 Freek van Tienen + * + * 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. + */ + +#ifndef ACTUATORS_UAVCAN_H +#define ACTUATORS_UAVCAN_H + +#include "modules/uavcan/uavcan.h" +#include BOARD_CONFIG + +/* External functions */ +extern void actuators_uavcan_init(struct uavcan_iface_t* iface); +extern void actuators_uavcan_commit(struct uavcan_iface_t* iface, int16_t *values, uint8_t nb); + +#endif /* ACTUATORS_UAVCAN_H */ \ No newline at end of file diff --git a/sw/airborne/subsystems/actuators/actuators_uavcan1.h b/sw/airborne/subsystems/actuators/actuators_uavcan1.h new file mode 100644 index 0000000000..f087d07944 --- /dev/null +++ b/sw/airborne/subsystems/actuators/actuators_uavcan1.h @@ -0,0 +1,34 @@ +/* + * Copyright (C) 2021 Freek van Tienen + * + * 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. + */ + +#ifndef ACTUATORS_UAVCAN1_H +#define ACTUATORS_UAVCAN1_H + +#include "actuators_uavcan.h" + +/** Stub file needed per uavcan interface because of generator */ +extern int16_t actuators_uavcan1_values[SERVOS_UAVCAN1_NB]; + +#define ActuatorsUavcan1Init() actuators_uavcan_init(&uavcan1) +#define ActuatorUavcan1Set(_i, _v) { actuators_uavcan1_values[_i] = _v; } +#define ActuatorsUavcan1Commit() actuators_uavcan_commit(&uavcan1, actuators_uavcan1_values, SERVOS_UAVCAN1_NB) + +#endif /* ACTUATORS_UAVCAN1_H */ \ No newline at end of file diff --git a/sw/airborne/subsystems/actuators/actuators_uavcan2.h b/sw/airborne/subsystems/actuators/actuators_uavcan2.h new file mode 100644 index 0000000000..301d3bf0fb --- /dev/null +++ b/sw/airborne/subsystems/actuators/actuators_uavcan2.h @@ -0,0 +1,34 @@ +/* + * Copyright (C) 2021 Freek van Tienen + * + * 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. + */ + +#ifndef ACTUATORS_UAVCAN2_H +#define ACTUATORS_UAVCAN2_H + +#include "actuators_uavcan.h" + +/** Stub file needed per interface because of generator */ +extern int16_t actuators_uavcan2_values[SERVOS_UAVCAN2_NB]; + +#define ActuatorsUavcan2Init() actuators_uavcan_init(&uavcan2) +#define ActuatorUavcan2Set(_i, _v) { actuators_uavcan2_values[_i] = _v; } +#define ActuatorsUavcan2Commit() actuators_uavcan_commit(&uavcan2, actuators_uavcan2_values, SERVOS_UAVCAN2_NB) + +#endif /* ACTUATORS_UAVCAN2_H */ \ No newline at end of file diff --git a/sw/ext/libcanard b/sw/ext/libcanard new file mode 160000 index 0000000000..4244b8c0b3 --- /dev/null +++ b/sw/ext/libcanard @@ -0,0 +1 @@ +Subproject commit 4244b8c0b303ac02a20fd603844179809bee59b3