From eb409919fe4336a6c0764edfe95521fc5a627851 Mon Sep 17 00:00:00 2001 From: AndreWongZH Date: Thu, 7 Jan 2021 17:43:46 +0800 Subject: [PATCH 1/8] Updated board configs for dev c to enable can1 port --- boards/robomaster/dev-c/nuttx-config/include/board.h | 4 ++-- boards/robomaster/dev-c/nuttx-config/nsh/defconfig | 11 +++++++++++ boards/robomaster/dev-c/src/board_config.h | 7 ++++++- boards/robomaster/dev-c/src/can.c | 2 ++ boards/robomaster/dev-c/src/init.c | 4 ++++ platforms/nuttx/src/px4/common/px4_init.cpp | 1 - 6 files changed, 25 insertions(+), 4 deletions(-) diff --git a/boards/robomaster/dev-c/nuttx-config/include/board.h b/boards/robomaster/dev-c/nuttx-config/include/board.h index d64428e1106e..1a26cac9176a 100644 --- a/boards/robomaster/dev-c/nuttx-config/include/board.h +++ b/boards/robomaster/dev-c/nuttx-config/include/board.h @@ -408,8 +408,8 @@ #define GPIO_CAN1_RX GPIO_CAN1_RX_3 #define GPIO_CAN1_TX GPIO_CAN1_TX_3 -#define GPIO_CAN2_RX GPIO_CAN2_RX_2 -#define GPIO_CAN2_TX GPIO_CAN2_TX_2 +// #define GPIO_CAN2_RX GPIO_CAN2_RX_2 +// #define GPIO_CAN2_TX GPIO_CAN2_TX_2 /* I2C. Only I2C1 is available on the STM3240G-EVAL. I2C1_SCL and I2C1_SDA are * available on the following pins: diff --git a/boards/robomaster/dev-c/nuttx-config/nsh/defconfig b/boards/robomaster/dev-c/nuttx-config/nsh/defconfig index 7ad98d15fb94..035e8dfa3c8b 100644 --- a/boards/robomaster/dev-c/nuttx-config/nsh/defconfig +++ b/boards/robomaster/dev-c/nuttx-config/nsh/defconfig @@ -46,14 +46,23 @@ CONFIG_CDCACM_TXBUFSIZE=2000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_VENDORSTR="NUS RoboMaster" CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_CAN=y +CONFIG_DEBUG_CAN_ERROR=y +CONFIG_DEBUG_CAN_WARN=y +CONFIG_DEBUG_ERROR=y +CONFIG_DEBUG_FEATURES=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_INFO=y CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_WARN=y CONFIG_DEFAULT_SMALL=y CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 CONFIG_DISABLE_MQUEUE=y +CONFIG_EXAMPLES_CAN=y +CONFIG_EXAMPLES_CAN_NMSGS=30 CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y @@ -129,6 +138,8 @@ CONFIG_STDIO_BUFFER_SIZE=32 CONFIG_STM32_BBSRAM=y CONFIG_STM32_BBSRAM_FILES=5 CONFIG_STM32_BKPSRAM=y +CONFIG_STM32_CAN1=y +CONFIG_STM32_CAN1_BAUD=1000000 CONFIG_STM32_CCMDATARAM=y CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y CONFIG_STM32_FLASH_CONFIG_G=y diff --git a/boards/robomaster/dev-c/src/board_config.h b/boards/robomaster/dev-c/src/board_config.h index af3b008a033f..736ad6c52647 100644 --- a/boards/robomaster/dev-c/src/board_config.h +++ b/boards/robomaster/dev-c/src/board_config.h @@ -76,7 +76,7 @@ // #define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 // #define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX // #define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX -// #define PX4IO_SERIAL_RCC_REG STM32_RCC_APB2ENR +// #define PX4IO_SERIAL_RCC_REG STM32_RCC_APB1ENR // #define PX4IO_SERIAL_RCC_EN RCC_APB2ENR_USART6EN // #define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY // #define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ @@ -84,6 +84,11 @@ #define FLASH_BASED_PARAMS // #define FLASH_BASED_DATAMAN +// setting uavcan stuff +// #define UAVCAN_STM32_NUTTX +// #define RCC_APB1ENR_CAN1EN +// #define UAVCAN_NUM_IFACES_RUNTIME 1 + /* PX4FMU GPIOs ***********************************************************************************/ /** diff --git a/boards/robomaster/dev-c/src/can.c b/boards/robomaster/dev-c/src/can.c index 8a2bcbf4061b..41780e2b7b0d 100644 --- a/boards/robomaster/dev-c/src/can.c +++ b/boards/robomaster/dev-c/src/can.c @@ -94,6 +94,7 @@ int can_devinit(void); int can_devinit(void) { + syslog(LOG_INFO, "can dev init\n"); static bool initialized = false; struct can_dev_s *can; int ret; @@ -113,6 +114,7 @@ int can_devinit(void) /* Register the CAN driver at "/dev/can0" */ ret = can_register("/dev/can0", can); + syslog(LOG_INFO, "can dev registered at /dev/can0\n"); if (ret < 0) { canerr("ERROR: can_register failed: %d\n", ret); diff --git a/boards/robomaster/dev-c/src/init.c b/boards/robomaster/dev-c/src/init.c index 070cf99c58c2..9cb77794e35d 100644 --- a/boards/robomaster/dev-c/src/init.c +++ b/boards/robomaster/dev-c/src/init.c @@ -76,6 +76,8 @@ #include #include +#include "can.c" + #include #if defined(FLASH_BASED_PARAMS) @@ -378,6 +380,8 @@ __EXPORT int board_app_initialize(uintptr_t arg) /* configure SPI interfaces (after the hw is determined) */ stm32_spiinitialize(); + can_devinit(); + px4_platform_init(); /* configure the DMA allocator */ diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index 7159ff72386f..ad52a5400ac3 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -47,7 +47,6 @@ int px4_platform_init(void) { - syslog(LOG_INFO, "px4_platform_init\n"); syslog(LOG_INFO, "px4_platform_init\n"); From 1baf0ac5e0cbd896f34a0741ac18e41ef34a499c Mon Sep 17 00:00:00 2001 From: AndreWongZH Date: Tue, 12 Jan 2021 19:26:47 +0800 Subject: [PATCH 2/8] add new uorb topic for can motor --- msg/CMakeLists.txt | 1 + msg/can_motor.msg | 17 +++++++++++++++++ 2 files changed, 18 insertions(+) create mode 100644 msg/can_motor.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 52cb19beafa7..68bc7f71b9f1 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -49,6 +49,7 @@ set(msg_files collision_report.msg commander_state.msg cpuload.msg + can_motor.msg debug_array.msg debug_key_value.msg debug_value.msg diff --git a/msg/can_motor.msg b/msg/can_motor.msg new file mode 100644 index 000000000000..ce752ba56897 --- /dev/null +++ b/msg/can_motor.msg @@ -0,0 +1,17 @@ +uint64 timestamp # time since system start (microseconds) + +# uint8 data0 +# uint8 data1 +# uint8 data2 +# uint8 data3 +# uint8 data4 +# uint8 data5 +# uint8 data6 +# uint8 data7 + +# values read from the dji motor + +uint16 mech_angle +uint16 rot_speed +uint16 torque +uint8 temp From f6238ca4eae41ef0936768cf6874fc59ec3da33f Mon Sep 17 00:00:00 2001 From: AndreWongZH Date: Tue, 12 Jan 2021 19:27:21 +0800 Subject: [PATCH 3/8] create driver for can motor --- src/drivers/djican/CMakeLists.txt | 8 ++ src/drivers/djican/djican.cpp | 223 ++++++++++++++++++++++++++++++ src/drivers/djican/djican.hpp | 45 ++++++ 3 files changed, 276 insertions(+) create mode 100644 src/drivers/djican/CMakeLists.txt create mode 100644 src/drivers/djican/djican.cpp create mode 100644 src/drivers/djican/djican.hpp diff --git a/src/drivers/djican/CMakeLists.txt b/src/drivers/djican/CMakeLists.txt new file mode 100644 index 000000000000..4a8bd66d1817 --- /dev/null +++ b/src/drivers/djican/CMakeLists.txt @@ -0,0 +1,8 @@ +px4_add_module( + MODULE drivers__djican + MAIN djican + SRCS + djican.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/djican/djican.cpp b/src/drivers/djican/djican.cpp new file mode 100644 index 000000000000..fc288b1ce261 --- /dev/null +++ b/src/drivers/djican/djican.cpp @@ -0,0 +1,223 @@ + +#define CAN_DEVPATH "/dev/can0" +#define CAN_OFLAGS O_RDWR + +#include "djican.hpp" + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +using namespace time_literals; + +DjiCan::DjiCan() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1) +{ +} + +DjiCan::~DjiCan() +{ + perf_free(_loop_perf); + perf_free(_loop_interval_perf); +} + +bool DjiCan::init() +{ + ScheduleOnInterval(100_ms); // 10Hz rate + + return true; +} + +int DjiCan::start_can() +{ + PX4_INFO("CAN driver start reading"); + struct canioc_bittiming_s bt; + + struct can_msg_s rxmsg; + + size_t msgsize; + size_t nbytes; + long nmsgs = CONFIG_EXAMPLES_CAN_NMSGS; + long msgno; + int fd; + int errval = 0; + int ret; + + /* Initialization of the CAN hardware is performed by board-specific, + * logic external prior to running this test. + */ + + /* Open the CAN device for reading */ + + fd = open(CAN_DEVPATH, CAN_OFLAGS); + if (fd < 0) + { + printf("ERROR: open %s failed: %d\n", + CAN_DEVPATH, errno); + errval = 2; + goto errout_with_dev; + } + + /* Show bit timing information if provided by the driver. Not all CAN + * drivers will support this IOCTL. + */ + + ret = ioctl(fd, CANIOC_GET_BITTIMING, (unsigned long)((uintptr_t)&bt)); + + if (ret < 0) + { + printf("Bit timing not available: %d\n", errno); + } + else + { + printf("Bit timing:\n"); + printf(" Baud: %lu\n", (unsigned long)bt.bt_baud); + printf(" TSEG1: %u\n", bt.bt_tseg1); + printf(" TSEG2: %u\n", bt.bt_tseg2); + printf(" SJW: %u\n", bt.bt_sjw); + } + + for (msgno = 0; !nmsgs || msgno < nmsgs; msgno++) + { + msgsize = sizeof(struct can_msg_s); + nbytes = read(fd, &rxmsg, msgsize); + if (nbytes < CAN_MSGLEN(0) || nbytes > msgsize) + { + printf("ERROR: read(%ld) returned %ld\n", + (long)msgsize, (long)nbytes); + errval = 4; + goto errout_with_dev; + } + + // show raw data as debug message + printf(" ID: %4u DLC: %u\n", + rxmsg.cm_hdr.ch_id, rxmsg.cm_hdr.ch_dlc); + printf("Data0: %d\n", rxmsg.cm_data[0]); + printf("Data1: %d\n", rxmsg.cm_data[1]); + printf("Data2: %d\n", rxmsg.cm_data[2]); + printf("Data3: %d\n", rxmsg.cm_data[3]); + printf("Data4: %d\n", rxmsg.cm_data[4]); + printf("Data5: %d\n", rxmsg.cm_data[5]); + printf("Data6: %d\n", rxmsg.cm_data[6]); + printf("Data7: %d\n", rxmsg.cm_data[7]); + + // publish can motor data + can_motor_s data{}; + data.timestamp = hrt_absolute_time(); + data.mech_angle = (rxmsg.cm_data[0] << 8 | rxmsg.cm_data[1]); + data.rot_speed = (rxmsg.cm_data[2] << 8 | rxmsg.cm_data[3]); + data.torque = (rxmsg.cm_data[4] << 8 | rxmsg.cm_data[5]); + data.temp = rxmsg.cm_data[6]; + + _orb_can_pub.publish(data); + } + +errout_with_dev: + close(fd); + + printf("Terminating!\n"); + fflush(stdout); + + return errval; +} + +void DjiCan::Run() +{ + PX4_INFO("CAN driver running routine"); + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + perf_count(_loop_interval_perf); + + // code to start can communication + this->start_can(); + + uint64_t time_now = hrt_absolute_time(); + PX4_INFO("%llu\n", time_now); // this should print out from syslog + + + perf_end(_loop_perf); +} + +int DjiCan::task_spawn(int argc, char *argv[]) +{ + PX4_INFO("djican spawn task"); + DjiCan *instance = new DjiCan(); + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int DjiCan::print_status() +{ + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); + return 0; +} + +int DjiCan::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int DjiCan::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Start can driver communication. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("djican", "template"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + + +extern "C" int djican_main(int argc, char *argv[]) +{ + PX4_INFO("CAN driver start"); + return DjiCan::main(argc, argv); +} diff --git a/src/drivers/djican/djican.hpp b/src/drivers/djican/djican.hpp new file mode 100644 index 000000000000..f9e10e3079b7 --- /dev/null +++ b/src/drivers/djican/djican.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class DjiCan : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + DjiCan(); + ~DjiCan() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + + int print_status() override; + + int start_can(); + +private: + void Run() override; + + uORB::Publication _orb_can_pub{ORB_ID(can_motor)}; + + // uORB::SubscriptionData _sensor_accel_sub{ORB_ID(sensor_accel)}; + + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; +}; From f9d5c76bc5045366bf54f8fec1c527e6dcbb06ee Mon Sep 17 00:00:00 2001 From: AndreWongZH Date: Tue, 12 Jan 2021 19:27:51 +0800 Subject: [PATCH 4/8] add djican to driver --- boards/robomaster/dev-c/default.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/robomaster/dev-c/default.cmake b/boards/robomaster/dev-c/default.cmake index af5aa4040120..ad5ba78475e3 100644 --- a/boards/robomaster/dev-c/default.cmake +++ b/boards/robomaster/dev-c/default.cmake @@ -47,6 +47,7 @@ px4_add_board( # test_ppm tone_alarm # uavcan + djican MODULES commander attitude_estimator_q From 6b92aa7ba2363fc0a8c63d0d5c808a0cabcb290a Mon Sep 17 00:00:00 2001 From: AndreWongZH Date: Thu, 14 Jan 2021 20:29:45 +0800 Subject: [PATCH 5/8] added functionallity for can module to send data to motor --- src/drivers/djican/djican.cpp | 110 +++++++++++++++++++++++++--------- src/drivers/djican/djican.hpp | 1 - 2 files changed, 83 insertions(+), 28 deletions(-) diff --git a/src/drivers/djican/djican.cpp b/src/drivers/djican/djican.cpp index fc288b1ce261..48a1301a9872 100644 --- a/src/drivers/djican/djican.cpp +++ b/src/drivers/djican/djican.cpp @@ -26,6 +26,7 @@ using namespace time_literals; +// for now uses work queue configuration: test1 DjiCan::DjiCan() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1) @@ -53,21 +54,23 @@ int DjiCan::start_can() struct can_msg_s rxmsg; size_t msgsize; - size_t nbytes; - long nmsgs = CONFIG_EXAMPLES_CAN_NMSGS; - long msgno; - int fd; - int errval = 0; - int ret; + size_t nbytes; + long nmsgs = 20; + long msgno; + int fd; + int errval = 0; + int ret; /* Initialization of the CAN hardware is performed by board-specific, * logic external prior to running this test. */ - /* Open the CAN device for reading */ + /* Open the CAN device for reading + * Will hang if not connected to anything: to fix soon + */ - fd = open(CAN_DEVPATH, CAN_OFLAGS); - if (fd < 0) + fd = open(CAN_DEVPATH, CAN_OFLAGS); + if (fd < 0) { printf("ERROR: open %s failed: %d\n", CAN_DEVPATH, errno); @@ -79,13 +82,13 @@ int DjiCan::start_can() * drivers will support this IOCTL. */ - ret = ioctl(fd, CANIOC_GET_BITTIMING, (unsigned long)((uintptr_t)&bt)); + ret = ioctl(fd, CANIOC_GET_BITTIMING, (unsigned long)((uintptr_t)&bt)); if (ret < 0) { printf("Bit timing not available: %d\n", errno); } - else + else { printf("Bit timing:\n"); printf(" Baud: %lu\n", (unsigned long)bt.bt_baud); @@ -106,17 +109,19 @@ int DjiCan::start_can() goto errout_with_dev; } + PX4_INFO("Motor number: %d", rxmsg.cm_hdr.ch_id); + // show raw data as debug message - printf(" ID: %4u DLC: %u\n", - rxmsg.cm_hdr.ch_id, rxmsg.cm_hdr.ch_dlc); - printf("Data0: %d\n", rxmsg.cm_data[0]); - printf("Data1: %d\n", rxmsg.cm_data[1]); - printf("Data2: %d\n", rxmsg.cm_data[2]); - printf("Data3: %d\n", rxmsg.cm_data[3]); - printf("Data4: %d\n", rxmsg.cm_data[4]); - printf("Data5: %d\n", rxmsg.cm_data[5]); - printf("Data6: %d\n", rxmsg.cm_data[6]); - printf("Data7: %d\n", rxmsg.cm_data[7]); + // printf(" ID: %4u DLC: %u\n", + // rxmsg.cm_hdr.ch_id, rxmsg.cm_hdr.ch_dlc); + // printf("Data0: %d\n", rxmsg.cm_data[0]); + // printf("Data1: %d\n", rxmsg.cm_data[1]); + // printf("Data2: %d\n", rxmsg.cm_data[2]); + // printf("Data3: %d\n", rxmsg.cm_data[3]); + // printf("Data4: %d\n", rxmsg.cm_data[4]); + // printf("Data5: %d\n", rxmsg.cm_data[5]); + // printf("Data6: %d\n", rxmsg.cm_data[6]); + // printf("Data7: %d\n", rxmsg.cm_data[7]); // publish can motor data can_motor_s data{}; @@ -130,10 +135,10 @@ int DjiCan::start_can() } errout_with_dev: - close(fd); + close(fd); - printf("Terminating!\n"); - fflush(stdout); + printf("Terminating!\n"); + fflush(stdout); return errval; } @@ -162,7 +167,6 @@ void DjiCan::Run() int DjiCan::task_spawn(int argc, char *argv[]) { - PX4_INFO("djican spawn task"); DjiCan *instance = new DjiCan(); if (instance) { _object.store(instance); @@ -209,15 +213,67 @@ Start can driver communication. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("djican", "template"); - PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "listen to can motor signals"); + PRINT_MODULE_USAGE_COMMAND_DESCR("send", "send data to can motor"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } +static void send_can(char* string_value) +{ + PX4_INFO("Starting to send data: %s", string_value); + struct can_msg_s txmsg; + int msgdlc = 8; + size_t msgsize; + int fd; + size_t nbytes; + + fd = open(CAN_DEVPATH, CAN_OFLAGS); + if (fd < 0) + { + printf("ERROR: open %s failed: %d\n", + CAN_DEVPATH, errno); + } + + /* Construct the next TX message */ + txmsg.cm_hdr.ch_id = 0x200; + txmsg.cm_hdr.ch_rtr = true; + txmsg.cm_hdr.ch_dlc = msgdlc; + + for (int i = 0; i < msgdlc; i++) + { + txmsg.cm_data[i] = 0; + } + + int16_t actual_value = atoi(string_value); + + // hard wired to only write to motor device id 2 + txmsg.cm_data[2] = (actual_value >> 8); + txmsg.cm_data[3] = (actual_value & 0xff); + + /* Send the TX message */ + + msgsize = CAN_MSGLEN(msgdlc); + nbytes = write(fd, &txmsg, msgsize); + if (nbytes != msgsize) + { + PX4_INFO("ERROR: write(%ld) returned %ld\n", + (long)msgsize, (long)nbytes); + // return 3 + } + PX4_INFO("Value wrote to ESC"); + // return 0; + + close(fd); +} extern "C" int djican_main(int argc, char *argv[]) { - PX4_INFO("CAN driver start"); + PX4_INFO("%d", strcmp(argv[1], "send")); + if (strcmp(argv[1], "send") == 0) { + send_can(argv[2]); + return 0; + } return DjiCan::main(argc, argv); } diff --git a/src/drivers/djican/djican.hpp b/src/drivers/djican/djican.hpp index f9e10e3079b7..99eff6102d15 100644 --- a/src/drivers/djican/djican.hpp +++ b/src/drivers/djican/djican.hpp @@ -7,7 +7,6 @@ #include #include #include -#include #include #include #include From 43b6d015806c8528d94405742d13d0e489d17084 Mon Sep 17 00:00:00 2001 From: AndreWongZH Date: Thu, 28 Jan 2021 14:47:54 +0800 Subject: [PATCH 6/8] update device file to support can port mixing --- ROMFS/robomaster_common/init.d/rc.interface | 415 ++++++++++++++++++++ ROMFS/robomaster_common/init.d/rcS | 13 +- 2 files changed, 423 insertions(+), 5 deletions(-) create mode 100644 ROMFS/robomaster_common/init.d/rc.interface diff --git a/ROMFS/robomaster_common/init.d/rc.interface b/ROMFS/robomaster_common/init.d/rc.interface new file mode 100644 index 000000000000..f64602bd55f4 --- /dev/null +++ b/ROMFS/robomaster_common/init.d/rc.interface @@ -0,0 +1,415 @@ +#!/bin/sh +# +# Script to configure control interfaces. +# +# +# NOTE: environment variable references: +# If the dollar sign ('$') is followed by a left bracket ('{') then the +# variable name is terminated with the right bracket character ('}'). +# Otherwise, the variable name goes to the end of the argument. +# + +set OUTPUT_CMD pwm_out +set MIXER_AUX_FILE none +set OUTPUT_AUX_DEV /dev/pwm_output1 +set OUTPUT_DEV none + +# +# If mount (gimbal) control is enabled and output mode is AUX, set the aux +# mixer to mount (override the airframe-specific MIXER_AUX setting). +# +if ! param compare -s MNT_MODE_IN -1 +then + if param compare -s MNT_MODE_OUT 0 + then + set MIXER_AUX mount + fi +fi + +# USE_IO is set to 'no' for all boards w/o px4io driver or SYS_USE_IO disabled +if [ $USE_IO = no ] +then + set MIXER_AUX none +fi + +# +# Set the default output mode if none was set. +# +if [ $OUTPUT_MODE = none ] +then + if [ $USE_IO = yes ] + then + # Enable IO output only if IO is present. + if [ $IO_PRESENT = yes ] + then + set OUTPUT_MODE io + if param greater DSHOT_CONFIG 0 + then + set OUTPUT_CMD dshot + fi + fi + else + if param greater DSHOT_CONFIG 0 + then + set OUTPUT_MODE dshot + set OUTPUT_CMD dshot + else + set OUTPUT_MODE can + fi + fi +fi + +# +# If OUTPUT_MODE = none then something is wrong with setup and we shouldn't try to enable output. +# +if [ $OUTPUT_MODE != none ] +then + if [ $OUTPUT_MODE = mkblctrl ] + then + if [ $MKBLCTRL_MODE = x ] + then + set MKBLCTRL_ARG "-mkmode x" + fi + if [ $MKBLCTRL_MODE = + ] + then + set MKBLCTRL_ARG "-mkmode +" + fi + + if ! mkblctrl $MKBLCTRL_ARG + then + # Error tune. + tune_control play -t 2 + fi + fi + + if [ $OUTPUT_MODE = hil -o $OUTPUT_MODE = sim ] + then + if ! pwm_out_sim start -m $OUTPUT_MODE + then + # Error tune. + tune_control play -t 2 + fi + fi + + if [ $OUTPUT_MODE = $OUTPUT_CMD ] + then + if ! $OUTPUT_CMD mode_$FMU_MODE + then + echo "$OUTPUT_CMD start failed" >> $LOG_FILE + # Error tune. + tune_control play -t 2 + fi + fi + + if [ $OUTPUT_MODE = uavcan_esc ] + then + if param compare UAVCAN_ENABLE 0 + then + param set UAVCAN_ENABLE 3 + fi + fi + + if [ $OUTPUT_MODE = io ] + then + sh /etc/init.d/rc.io + fi + + # + # Start IO for RC input if needed. + # + if [ $IO_PRESENT = yes ] + then + if [ $OUTPUT_MODE != io ] + then + sh /etc/init.d/rc.io + fi + fi +fi + +if [ $MIXER != none -a $MIXER != skip ] +then + # + # Load main mixer. + # + if [ $MIXER_AUX = none -a $USE_IO = yes ] + then + set MIXER_AUX ${MIXER} + fi + + if [ "$MIXER_FILE" = none ] + then + echo "path is: ${MIXER}" + if [ -f ${SDCARD_MIXERS_PATH}/${MIXER}.main.mix ] + then + # Use the mixer file from the SD-card if it exists. + set MIXER_FILE ${SDCARD_MIXERS_PATH}/${MIXER}.main.mix + else + # Try out the old convention, for backward compatibility. + if [ -f ${SDCARD_MIXERS_PATH}/${MIXER}.mix ] + then + set MIXER_FILE ${SDCARD_MIXERS_PATH}/${MIXER}.mix + else + echo "setting mixer file" + set MIXER_FILE /etc/mixers/${MIXER}.main.mix + fi + fi + fi + + if [ $OUTPUT_MODE = mkblctrl ] + then + set OUTPUT_DEV /dev/mkblctrl0 + else + set OUTPUT_DEV /dev/pwm_output0 + fi + + if [ $OUTPUT_MODE = uavcan_esc ] + then + set OUTPUT_DEV /dev/uavcan/esc + fi + + if [ $OUTPUT_MODE = tap_esc ] + then + set OUTPUT_DEV /dev/tap_esc + fi + + if [ $OUTPUT_MODE = can ] + then + set OUTPUT_DEV /dev/can_output0 + echo "set output dev to can_output0" + fi + + if mixer load ${OUTPUT_DEV} ${MIXER_FILE} + then + echo "INFO [init] Mixer: ${MIXER_FILE} on ${OUTPUT_DEV}" + else + echo "ERROR [init] Failed loading mixer: ${MIXER_FILE}" + echo "ERROR [init] Failed loading mixer: ${MIXER_FILE}" >> $LOG_FILE + tune_control play -t 20 + fi + +else + if [ $MIXER != skip ] + then + echo "ERROR [init] Mixer undefined" + echo "ERROR [init] Mixer undefined" >> $LOG_FILE + tune_control play -t 20 + fi +fi + +if [ $MIXER_AUX != none -a $AUX_MODE != none ] +then + # + # Load aux mixer. + # + if [ -f ${SDCARD_MIXERS_PATH}/${MIXER_AUX}.aux.mix ] + then + set MIXER_AUX_FILE ${SDCARD_MIXERS_PATH}/${MIXER_AUX}.aux.mix + else + + if [ -f /etc/mixers/${MIXER_AUX}.aux.mix ] + then + set MIXER_AUX_FILE /etc/mixers/${MIXER_AUX}.aux.mix + fi + fi + + if [ $MIXER_AUX_FILE != none ] + then + if $OUTPUT_CMD mode_${AUX_MODE} + then + # Append aux mixer to main device. + if param greater SYS_HITL 0 + then + if mixer append ${OUTPUT_DEV} ${MIXER_AUX_FILE} + then + echo "INFO [init] Mixer: ${MIXER_AUX_FILE} appended to ${OUTPUT_DEV}" + else + echo "ERROR [init] Failed appending mixer: ${MIXER_AUX_FILE}" + echo "ERROR [init] Failed appending mixer: ${MIXER_AUX_FILE}" >> $LOG_FILE + fi + fi + if [ -e $OUTPUT_AUX_DEV -a $OUTPUT_MODE != hil ] + then + if mixer load ${OUTPUT_AUX_DEV} ${MIXER_AUX_FILE} + then + echo "INFO [init] Mixer: ${MIXER_AUX_FILE} on ${OUTPUT_AUX_DEV}" + else + echo "ERROR [init] Failed loading mixer: ${MIXER_AUX_FILE}" + echo "ERROR [init] Failed loading mixer: ${MIXER_AUX_FILE}" >> $LOG_FILE + fi + else + set PWM_AUX_OUT none + set FAILSAFE_AUX none + fi + else + echo "ERROR: Could not start: pwm_out mode_pwm" >> $LOG_FILE + tune_control play -t 20 + set PWM_AUX_OUT none + set FAILSAFE_AUX none + fi + + # for DShot do not configure pwm values + if [ $OUTPUT_CMD != dshot ] + then + # Set min / max for aux out and rates. + if [ $PWM_AUX_OUT != none ] + then + # Set PWM_AUX output frequency. + if [ $PWM_AUX_RATE != none ] + then + pwm rate -c ${PWM_AUX_OUT} -r ${PWM_AUX_RATE} -d ${OUTPUT_AUX_DEV} + fi + + # Set disarmed, min and max PWM_AUX values. + if [ $PWM_AUX_DISARMED != none ] + then + pwm disarmed -c ${PWM_AUX_OUT} -p ${PWM_AUX_DISARMED} -d ${OUTPUT_AUX_DEV} + fi + if [ $PWM_AUX_MIN != none ] + then + pwm min -c ${PWM_AUX_OUT} -p ${PWM_AUX_MIN} -d ${OUTPUT_AUX_DEV} + fi + if [ $PWM_AUX_MAX != none ] + then + pwm max -c ${PWM_AUX_OUT} -p ${PWM_AUX_MAX} -d ${OUTPUT_AUX_DEV} + fi + fi + + # + # Per channel disarmed settings. + # + pwm disarmed -c 1 -p p:PWM_AUX_DIS1 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 2 -p p:PWM_AUX_DIS2 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 3 -p p:PWM_AUX_DIS3 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 4 -p p:PWM_AUX_DIS4 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 5 -p p:PWM_AUX_DIS5 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 6 -p p:PWM_AUX_DIS6 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 7 -p p:PWM_AUX_DIS7 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 8 -p p:PWM_AUX_DIS8 -d ${OUTPUT_AUX_DEV} + + # + # Per channel min settings. + # + pwm min -c 1 -p p:PWM_AUX_MIN1 -d ${OUTPUT_AUX_DEV} + pwm min -c 2 -p p:PWM_AUX_MIN2 -d ${OUTPUT_AUX_DEV} + pwm min -c 3 -p p:PWM_AUX_MIN3 -d ${OUTPUT_AUX_DEV} + pwm min -c 4 -p p:PWM_AUX_MIN4 -d ${OUTPUT_AUX_DEV} + pwm min -c 5 -p p:PWM_AUX_MIN5 -d ${OUTPUT_AUX_DEV} + pwm min -c 6 -p p:PWM_AUX_MIN6 -d ${OUTPUT_AUX_DEV} + pwm min -c 7 -p p:PWM_AUX_MIN7 -d ${OUTPUT_AUX_DEV} + pwm min -c 8 -p p:PWM_AUX_MIN8 -d ${OUTPUT_AUX_DEV} + + # + # Per channel max settings. + # + pwm max -c 1 -p p:PWM_AUX_MAX1 -d ${OUTPUT_AUX_DEV} + pwm max -c 2 -p p:PWM_AUX_MAX2 -d ${OUTPUT_AUX_DEV} + pwm max -c 3 -p p:PWM_AUX_MAX3 -d ${OUTPUT_AUX_DEV} + pwm max -c 4 -p p:PWM_AUX_MAX4 -d ${OUTPUT_AUX_DEV} + pwm max -c 5 -p p:PWM_AUX_MAX5 -d ${OUTPUT_AUX_DEV} + pwm max -c 6 -p p:PWM_AUX_MAX6 -d ${OUTPUT_AUX_DEV} + pwm max -c 7 -p p:PWM_AUX_MAX7 -d ${OUTPUT_AUX_DEV} + pwm max -c 8 -p p:PWM_AUX_MAX8 -d ${OUTPUT_AUX_DEV} + + if [ $FAILSAFE_AUX != none ] + then + pwm failsafe -c ${PWM_AUX_OUT} -p ${FAILSAFE} -d ${OUTPUT_AUX_DEV} + fi + + # + # Per channel failsafe settings. + # + pwm failsafe -c 1 -p p:PWM_AUX_FAIL1 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 2 -p p:PWM_AUX_FAIL2 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 3 -p p:PWM_AUX_FAIL3 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 4 -p p:PWM_AUX_FAIL4 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 5 -p p:PWM_AUX_FAIL5 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 6 -p p:PWM_AUX_FAIL6 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 7 -p p:PWM_AUX_FAIL7 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 8 -p p:PWM_AUX_FAIL8 -d ${OUTPUT_AUX_DEV} + fi + fi +fi + +if [ $OUTPUT_MODE = pwm_out -o $OUTPUT_MODE = io ] +then + if [ $PWM_OUT != none ] + then + # Set PWM output frequency. + if [ $PWM_RATE != none ] + then + pwm rate -c ${PWM_OUT} -r ${PWM_RATE} + fi + + # Set disarmed, min and max PWM values. + if [ $PWM_DISARMED != none ] + then + pwm disarmed -c ${PWM_OUT} -p ${PWM_DISARMED} + fi + if [ $PWM_MIN != none ] + then + pwm min -c ${PWM_OUT} -p ${PWM_MIN} + fi + if [ $PWM_MAX != none ] + then + pwm max -c ${PWM_OUT} -p ${PWM_MAX} + fi + fi + + # + # Per channel disarmed settings. + # + pwm disarmed -c 1 -p p:PWM_MAIN_DIS1 + pwm disarmed -c 2 -p p:PWM_MAIN_DIS2 + pwm disarmed -c 3 -p p:PWM_MAIN_DIS3 + pwm disarmed -c 4 -p p:PWM_MAIN_DIS4 + pwm disarmed -c 5 -p p:PWM_MAIN_DIS5 + pwm disarmed -c 6 -p p:PWM_MAIN_DIS6 + pwm disarmed -c 7 -p p:PWM_MAIN_DIS7 + pwm disarmed -c 8 -p p:PWM_MAIN_DIS8 + + # + # Per channel min settings. + # + pwm min -c 1 -p p:PWM_MAIN_MIN1 + pwm min -c 2 -p p:PWM_MAIN_MIN2 + pwm min -c 3 -p p:PWM_MAIN_MIN3 + pwm min -c 4 -p p:PWM_MAIN_MIN4 + pwm min -c 5 -p p:PWM_MAIN_MIN5 + pwm min -c 6 -p p:PWM_MAIN_MIN6 + pwm min -c 7 -p p:PWM_MAIN_MIN7 + pwm min -c 8 -p p:PWM_MAIN_MIN8 + + # + # Per channel max settings. + # + pwm max -c 1 -p p:PWM_MAIN_MAX1 + pwm max -c 2 -p p:PWM_MAIN_MAX2 + pwm max -c 3 -p p:PWM_MAIN_MAX3 + pwm max -c 4 -p p:PWM_MAIN_MAX4 + pwm max -c 5 -p p:PWM_MAIN_MAX5 + pwm max -c 6 -p p:PWM_MAIN_MAX6 + pwm max -c 7 -p p:PWM_MAIN_MAX7 + pwm max -c 8 -p p:PWM_MAIN_MAX8 + + if [ $FAILSAFE != none ] + then + pwm failsafe -c ${PWM_OUT} -p ${FAILSAFE} -d ${OUTPUT_DEV} + fi + + # + # Per channel failsafe settings. + # + pwm failsafe -c 1 -p p:PWM_MAIN_FAIL1 + pwm failsafe -c 2 -p p:PWM_MAIN_FAIL2 + pwm failsafe -c 3 -p p:PWM_MAIN_FAIL3 + pwm failsafe -c 4 -p p:PWM_MAIN_FAIL4 + pwm failsafe -c 5 -p p:PWM_MAIN_FAIL5 + pwm failsafe -c 6 -p p:PWM_MAIN_FAIL6 + pwm failsafe -c 7 -p p:PWM_MAIN_FAIL7 + pwm failsafe -c 8 -p p:PWM_MAIN_FAIL8 +fi + +unset OUTPUT_CMD +unset MIXER_AUX_FILE +unset OUTPUT_AUX_DEV +unset OUTPUT_DEV diff --git a/ROMFS/robomaster_common/init.d/rcS b/ROMFS/robomaster_common/init.d/rcS index b3fe16b33603..ec17d2b00637 100644 --- a/ROMFS/robomaster_common/init.d/rcS +++ b/ROMFS/robomaster_common/init.d/rcS @@ -26,10 +26,10 @@ set AUTOCNF no # set MAV_TYPE none # set MIXER none # set MIXER_AUX none -# set MIXER_FILE none +set MIXER_FILE none # set MK_MODE none # set MKBLCTRL_ARG "" -# set OUTPUT_MODE none +set OUTPUT_MODE none # set PARAM_FILE /fs/microsd/params # set PWM_AUX_DISARMED p:PWM_AUX_DISARMED # set PWM_AUX_MAX p:PWM_AUX_MAX @@ -189,7 +189,10 @@ then fi unset BOARD_RC_MAVLINK - +# +# Start djican to create can_output0 +# +djican start # # Configure vehicle type specific parameters. @@ -239,10 +242,10 @@ unset AUTOCNF # unset MAV_TYPE # unset MIXER # unset MIXER_AUX -# unset MIXER_FILE +unset MIXER_FILE # unset MK_MODE # unset MKBLCTRL_ARG -# unset OUTPUT_MODE +unset OUTPUT_MODE unset PARAM_DEFAULTS_VER # unset PARAM_FILE # unset PWM_AUX_DISARMED From a9fc8cc09c4953525faf4e9b015391b8f6b716ce Mon Sep 17 00:00:00 2001 From: AndreWongZH Date: Thu, 28 Jan 2021 14:53:21 +0800 Subject: [PATCH 7/8] fix bug in can dji code --- src/drivers/djican/djican.cpp | 134 ++++++++++++++++++++++++++++------ src/drivers/djican/djican.hpp | 33 +++++++-- 2 files changed, 140 insertions(+), 27 deletions(-) diff --git a/src/drivers/djican/djican.cpp b/src/drivers/djican/djican.cpp index 48a1301a9872..f2ac60edc213 100644 --- a/src/drivers/djican/djican.cpp +++ b/src/drivers/djican/djican.cpp @@ -1,5 +1,6 @@ -#define CAN_DEVPATH "/dev/can0" +#define CAN_DEVPATH "/dev/can_output" +#define PX4FMU_DEVICE_PATH "/dev/px4fmu" #define CAN_OFLAGS O_RDWR #include "djican.hpp" @@ -26,24 +27,51 @@ using namespace time_literals; +#define CAN_MOTOR_CURRENT_MIN 0 + // for now uses work queue configuration: test1 DjiCan::DjiCan() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1) + CDev(PX4FMU_DEVICE_PATH), + OutputModuleInterface(MODULE_NAME, px4::wq_configurations::test1), + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), + _interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval")) { + _mixing_output.setAllMinValues(CAN_MOTOR_CURRENT_MIN); + _mixing_output.setAllMaxValues(0); } DjiCan::~DjiCan() { - perf_free(_loop_perf); - perf_free(_loop_interval_perf); + /* clean up the alternate device node */ + unregister_class_devname(CAN_DEVPATH, _class_instance); + + perf_free(_cycle_perf); + perf_free(_interval_perf); } -bool DjiCan::init() +int DjiCan::init() { - ScheduleOnInterval(100_ms); // 10Hz rate + /* do regular cdev init */ + int ret = CDev::init(); - return true; + if (ret != OK) { + return ret; + } + + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + _class_instance = register_class_devname(CAN_DEVPATH); + + if (_class_instance == 0) { + /* lets not be too verbose */ + } else if (_class_instance < 0) { + PX4_ERR("FAILED registering class device"); + } + + _mixing_output.setDriverInstance(_class_instance); + + ScheduleNow(); + + return 0; } int DjiCan::start_can() @@ -69,7 +97,7 @@ int DjiCan::start_can() * Will hang if not connected to anything: to fix soon */ - fd = open(CAN_DEVPATH, CAN_OFLAGS); + fd = ::open("/dev/can0", CAN_OFLAGS); if (fd < 0) { printf("ERROR: open %s failed: %d\n", @@ -82,7 +110,7 @@ int DjiCan::start_can() * drivers will support this IOCTL. */ - ret = ioctl(fd, CANIOC_GET_BITTIMING, (unsigned long)((uintptr_t)&bt)); + ret = ::ioctl(fd, CANIOC_GET_BITTIMING, (unsigned long)((uintptr_t)&bt)); if (ret < 0) { @@ -100,7 +128,7 @@ int DjiCan::start_can() for (msgno = 0; !nmsgs || msgno < nmsgs; msgno++) { msgsize = sizeof(struct can_msg_s); - nbytes = read(fd, &rxmsg, msgsize); + nbytes = ::read(fd, &rxmsg, msgsize); if (nbytes < CAN_MSGLEN(0) || nbytes > msgsize) { printf("ERROR: read(%ld) returned %ld\n", @@ -135,7 +163,7 @@ int DjiCan::start_can() } errout_with_dev: - close(fd); + ::close(fd); printf("Terminating!\n"); fflush(stdout); @@ -145,24 +173,30 @@ int DjiCan::start_can() void DjiCan::Run() { - PX4_INFO("CAN driver running routine"); if (should_exit()) { ScheduleClear(); exit_and_cleanup(); return; } - perf_begin(_loop_perf); - perf_count(_loop_interval_perf); + perf_begin(_cycle_perf); + perf_count(_interval_perf); + + // push backup schedule + ScheduleDelayed(_backup_schedule_interval_us); + + _mixing_output.update(); // code to start can communication - this->start_can(); + //this->start_can(); - uint64_t time_now = hrt_absolute_time(); - PX4_INFO("%llu\n", time_now); // this should print out from syslog + // uint64_t time_now = hrt_absolute_time(); + // PX4_INFO("%llu\n", time_now); // this should print out from syslog + // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) + _mixing_output.updateSubscriptions(true, true); - perf_end(_loop_perf); + perf_end(_cycle_perf); } int DjiCan::task_spawn(int argc, char *argv[]) @@ -172,7 +206,7 @@ int DjiCan::task_spawn(int argc, char *argv[]) _object.store(instance); _task_id = task_id_is_work_queue; - if (instance->init()) { + if (instance->init() == PX4_OK) { return PX4_OK; } @@ -189,8 +223,8 @@ int DjiCan::task_spawn(int argc, char *argv[]) int DjiCan::print_status() { - perf_print_counter(_loop_perf); - perf_print_counter(_loop_interval_perf); + perf_print_counter(_cycle_perf); + perf_print_counter(_interval_perf); return 0; } @@ -220,6 +254,26 @@ Start can driver communication. return 0; } +bool DjiCan::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) +{ + /* output to the motors */ + // must set value for each individual motors + for (size_t i = 0; i < num_outputs; i++) { + // send message to can motor + send_motor_data(i, outputs[i]); + } + // PX4_INFO("subscribed actuator control0: %d", _mixing_output._control_subs[0].control); + _mixing_output.printStatus(); + return true; +} + +void DjiCan::send_motor_data(size_t channel, uint16_t output_data) { + if (output_data != 0) { + PX4_INFO("Data for channel %d is: %d", channel, output_data); + } +} + static void send_can(char* string_value) { PX4_INFO("Starting to send data: %s", string_value); @@ -268,6 +322,42 @@ static void send_can(char* string_value) close(fd); } +int DjiCan::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret; + ret = can_ioctl(filp, cmd, arg); + return ret; +} + +int DjiCan::can_ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + PX4_INFO("ioctl cmd: %d, arg: %ld", cmd, arg); + + lock(); + + switch (cmd) { + case MIXERIOCRESET: + _mixing_output.resetMixerThreadSafe(); + break; + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strlen(buf); + ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + + break; + } + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; +} + extern "C" int djican_main(int argc, char *argv[]) { PX4_INFO("%d", strcmp(argv[1], "send")); diff --git a/src/drivers/djican/djican.hpp b/src/drivers/djican/djican.hpp index 99eff6102d15..5bebd738d6ef 100644 --- a/src/drivers/djican/djican.hpp +++ b/src/drivers/djican/djican.hpp @@ -1,21 +1,26 @@ #pragma once #include +#include +#include #include #include #include #include #include +#include #include #include #include #include -class DjiCan : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +using namespace time_literals; + +class DjiCan : public cdev::CDev, public ModuleBase, public OutputModuleInterface { public: DjiCan(); - ~DjiCan() override; + virtual ~DjiCan(); /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -26,19 +31,37 @@ class DjiCan : public ModuleBase, public ModuleParams, public px4::Sched /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); - bool init(); + virtual int init(); int print_status() override; + virtual int ioctl(file *filp, int cmd, unsigned long arg); + int start_can(); + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + private: void Run() override; + uint32_t _backup_schedule_interval_us{1_s}; + uORB::Publication _orb_can_pub{ORB_ID(can_motor)}; + MixingOutput _mixing_output{4, *this, MixingOutput::SchedulingPolicy::Auto, true}; + + int _class_instance{-1}; + + int can_ioctl(file *filp, int cmd, unsigned long arg); + + void send_motor_data(size_t channel, uint16_t output_data); + // uORB::SubscriptionData _sensor_accel_sub{ORB_ID(sensor_accel)}; - perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; - perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + // perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + // perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + + perf_counter_t _cycle_perf; + perf_counter_t _interval_perf; }; From fa8ed0b74bdeae4d43d8c9ca1c26294b07732115 Mon Sep 17 00:00:00 2001 From: AndreWongZH Date: Sun, 31 Jan 2021 14:23:23 +0800 Subject: [PATCH 8/8] updated rc input pins --- ROMFS/robomaster_common/init.d/rcS | 11 ++++++----- boards/robomaster/dev-c/default.cmake | 6 ++++-- boards/robomaster/dev-c/nuttx-config/include/board.h | 11 ++++++++++- boards/robomaster/dev-c/nuttx-config/nsh/defconfig | 7 +++++-- src/drivers/djican/djican.cpp | 4 ++-- 5 files changed, 27 insertions(+), 12 deletions(-) diff --git a/ROMFS/robomaster_common/init.d/rcS b/ROMFS/robomaster_common/init.d/rcS index 196f7f8a9721..d37535c8dc6a 100644 --- a/ROMFS/robomaster_common/init.d/rcS +++ b/ROMFS/robomaster_common/init.d/rcS @@ -41,7 +41,7 @@ set OUTPUT_MODE none # set PWM_MIN p:PWM_MIN # set PWM_OUT none # set PWM_RATE p:PWM_RATE -# set RC_INPUT_ARGS "" +set RC_INPUT_ARGS "" # set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers set STARTUP_TUNE 1 # set USE_IO no @@ -93,7 +93,7 @@ fi # Needs to be started after the parameters are loaded (for CBRK_BUZZER). # Note that this will still play the already published startup tone. # -tone_alarm start +# tone_alarm start # # Play the startup tune (if not disabled or there is an error) @@ -203,8 +203,9 @@ sh /etc/init.d/rc.serial # Must be started after the serial config is read -rc_input start $RC_INPUT_ARGS -# rc_input start /dev/ttyS0 +echo $RC_INPUT_ARGS +# rc_input start $RC_INPUT_ARGS +# rc_input start -d /dev/ttyS0 # # Configure vehicle type specific parameters. @@ -270,7 +271,7 @@ unset PARAM_DEFAULTS_VER # unset PWM_MIN # unset PWM_OUT # unset PWM_RATE -# unset RC_INPUT_ARGS +unset RC_INPUT_ARGS # unset SDCARD_MIXERS_PATH unset STARTUP_TUNE # unset USE_IO diff --git a/boards/robomaster/dev-c/default.cmake b/boards/robomaster/dev-c/default.cmake index ad5ba78475e3..5e4e24860ee5 100644 --- a/boards/robomaster/dev-c/default.cmake +++ b/boards/robomaster/dev-c/default.cmake @@ -12,7 +12,8 @@ px4_add_board( TESTING # UAVCAN_INTERFACES 2 SERIAL_PORTS - URT6:/dev/ttyS0 + RC:/dev/ttyS0 + # URT6:/dev/ttyS0 DRIVERS # adc # barometer # all available barometer drivers @@ -48,6 +49,7 @@ px4_add_board( tone_alarm # uavcan djican + rc_input MODULES commander attitude_estimator_q @@ -84,7 +86,7 @@ px4_add_board( # hardfault_log i2cdetect # led_control - # mixer + mixer # motor_ramp # motor_test # mtd diff --git a/boards/robomaster/dev-c/nuttx-config/include/board.h b/boards/robomaster/dev-c/nuttx-config/include/board.h index 1a26cac9176a..1e8bb4bcfeb6 100644 --- a/boards/robomaster/dev-c/nuttx-config/include/board.h +++ b/boards/robomaster/dev-c/nuttx-config/include/board.h @@ -295,6 +295,15 @@ #define GPIO_USART6_RX GPIO_USART6_RX_2 #define GPIO_USART6_TX GPIO_USART6_TX_2 + +/* DBUS + * + * USART3 RX: PC11 + */ +#define GPIO_USART3_RX GPIO_USART3_RX_2 +#define GPIO_USART3_TX 0 + + /* PWM * * The STM3240G-Eval has no real on-board PWM devices, but the board can be @@ -468,7 +477,7 @@ #define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 // dma stuff -#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +// #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 // #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 /* DMA Channel/Stream Selections *****************************************************/ diff --git a/boards/robomaster/dev-c/nuttx-config/nsh/defconfig b/boards/robomaster/dev-c/nuttx-config/nsh/defconfig index 035e8dfa3c8b..310aebcc82ae 100644 --- a/boards/robomaster/dev-c/nuttx-config/nsh/defconfig +++ b/boards/robomaster/dev-c/nuttx-config/nsh/defconfig @@ -61,8 +61,6 @@ CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 CONFIG_DISABLE_MQUEUE=y -CONFIG_EXAMPLES_CAN=y -CONFIG_EXAMPLES_CAN_NMSGS=30 CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y @@ -109,6 +107,7 @@ CONFIG_NSH_NESTDEPTH=8 CONFIG_NSH_QUOTE=y CONFIG_NSH_ROMFSETC=y CONFIG_NSH_STRERROR=y +CONFIG_NSH_USBCONSOLE=y CONFIG_NSH_VARS=y CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=4 @@ -157,6 +156,7 @@ CONFIG_STM32_SERIALBRK_BSDCOMPAT=y CONFIG_STM32_SERIAL_DISABLE_REORDERING=y CONFIG_STM32_SPI1=y CONFIG_STM32_USART1=y +CONFIG_STM32_USART3=y CONFIG_STM32_USART6=y CONFIG_STM32_USART_BREAKS=y CONFIG_STM32_USART_SINGLEWIRE=y @@ -165,6 +165,9 @@ CONFIG_SYSTEM_NSH=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TIME_EXTENDED=y CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART3_BAUD=100000 +CONFIG_USART3_PARITY=2 +CONFIG_USART3_RXDMA=y CONFIG_USBDEV=y CONFIG_USBDEV_BUSPOWERED=y CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/src/drivers/djican/djican.cpp b/src/drivers/djican/djican.cpp index f2ac60edc213..f399e6ba3b03 100644 --- a/src/drivers/djican/djican.cpp +++ b/src/drivers/djican/djican.cpp @@ -264,7 +264,7 @@ bool DjiCan::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], send_motor_data(i, outputs[i]); } // PX4_INFO("subscribed actuator control0: %d", _mixing_output._control_subs[0].control); - _mixing_output.printStatus(); + // _mixing_output.printStatus(); return true; } @@ -283,7 +283,7 @@ static void send_can(char* string_value) int fd; size_t nbytes; - fd = open(CAN_DEVPATH, CAN_OFLAGS); + fd = open("/dev/can0", CAN_OFLAGS); if (fd < 0) { printf("ERROR: open %s failed: %d\n",