From c43d91b92e89e37fdd60bccfabcc1c6bedae6efa Mon Sep 17 00:00:00 2001 From: Eugene Shamaev Date: Sun, 2 Apr 2017 17:55:50 +0300 Subject: [PATCH] SRV_Channel: support for servo and ESC commands to be send via UAVCAN --- libraries/SRV_Channel/SRV_Channel.cpp | 1 + libraries/SRV_Channel/SRV_Channel.h | 18 ++++++++++++++++++ libraries/SRV_Channel/SRV_Channels.cpp | 19 +++++++++++++++++++ 3 files changed, 38 insertions(+) diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp index bc4f8d9344..efc75e16d5 100644 --- a/libraries/SRV_Channel/SRV_Channel.cpp +++ b/libraries/SRV_Channel/SRV_Channel.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "SRV_Channel.h" extern const AP_HAL::HAL& hal; diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index 37d567ef13..91fb7f3c70 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -352,6 +352,19 @@ public: static bool upgrade_parameters(const uint8_t old_keys[14], uint16_t aux_channel_mask, RCMapper *rcmap); static void upgrade_motors_servo(uint8_t ap_motors_key, uint8_t ap_motors_idx, uint8_t new_channel); + static uint32_t get_can_servo_bm(void) { + if(p_can_servo_bm != nullptr) + return *p_can_servo_bm; + else + return 0; + } + static uint32_t get_can_esc_bm(void) { + if(p_can_esc_bm != nullptr) + return *p_can_esc_bm; + else + return 0; + } + private: struct { bool k_throttle_reversible:1; @@ -382,4 +395,9 @@ private: static bool passthrough_disabled(void) { return disabled_passthrough; } + + AP_Int32 can_servo_bm; + AP_Int32 can_esc_bm; + static AP_Int32 *p_can_servo_bm; + static AP_Int32 *p_can_esc_bm; }; diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index 22a9053bd6..0d6fa42dee 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -30,6 +30,9 @@ bool SRV_Channels::initialised; Bitmask SRV_Channels::function_mask{SRV_Channel::k_nr_aux_servo_functions}; SRV_Channels::srv_function SRV_Channels::functions[SRV_Channel::k_nr_aux_servo_functions]; +AP_Int32* SRV_Channels::p_can_servo_bm = nullptr; +AP_Int32* SRV_Channels::p_can_esc_bm = nullptr; + const AP_Param::GroupInfo SRV_Channels::var_info[] = { // @Group: 1_ // @Path: SRV_Channel.cpp @@ -102,6 +105,20 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = { // @User: Advanced AP_GROUPINFO_FRAME("_AUTO_TRIM", 17, SRV_Channels, auto_trim, 0, AP_PARAM_FRAME_PLANE), + // @Param: _CANSRV_BM + // @DisplayName: RC Out channels to be transmitted as servo over CAN bus + // @Description: Bitmask with one set for channel to be transmitted as a servo command over CAN bus + // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15 + // @User: Standard + AP_GROUPINFO("_CANSRV_BM", 18, SRV_Channels, can_servo_bm, 0), + + // @Param: _CANESC_BM + // @DisplayName: RC Out channels to be transmitted as ESC over CAN bus + // @Description: Bitmask with one set for channel to be transmitted as a ESC command over CAN bus + // @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16 + // @User: Standard + AP_GROUPINFO("_CANESC_BM", 19, SRV_Channels, can_esc_bm, 0), + AP_GROUPEND }; @@ -111,6 +128,8 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = { SRV_Channels::SRV_Channels(void) { channels = obj_channels; + p_can_servo_bm = &can_servo_bm; + p_can_esc_bm = &can_esc_bm; // set defaults from the parameter table AP_Param::setup_object_defaults(this, var_info);