Merge pull request #1121 from flapper-drones/CPPM_improved_config

Improved support extRx (CPPM)

src/deck/drivers/src/Kconfig

@@ -97,6 +97,59 @@ config DECK_CPPM
     help
       Combined PPM / PPM-Sum driver.
 
+choice
+  prompt "ExtRx CPPM pin"
+  depends on DECK_CPPM
+  default DECK_CPPM_USE_PA7
+
+config DECK_CPPM_USE_PB4
+    bool "PB4/IO_3"
+
+config DECK_CPPM_USE_PB5
+    bool "PB5/IO_2"
+
+config DECK_CPPM_USE_PB8
+    bool "PB8/IO_1"
+
+config DECK_CPPM_USE_PA2
+    bool "PA2/TX2"
+
+config DECK_CPPM_USE_PA3
+    bool "PA3/RX2"
+
+config DECK_CPPM_USE_PA7
+    bool "PA7/MOSI"
+
+endchoice  
+
+config DECK_EXTRX_ALT_HOLD
+    bool "Enable altitude hold via ExtRx"
+    default n
+    depends on DECK_CPPM || DECK_BIGQUAD_ENABLE || DECK_FLAPPER_ENABLE
+    help
+        Switch to altitude hold via ch4 of the external receiver.
+
+config DECK_EXTRX_ARMING
+    bool "Enable arming via ExtRx"
+    default n
+    depends on DECK_CPPM || DECK_BIGQUAD_ENABLE
+    help
+        Arming via ch5 of the external receiver.
+
+config DECK_EXTRX_MODE_RATE
+    bool "Enable rate mode via ExtRx"
+    default n
+    depends on DECK_CPPM || DECK_BIGQUAD_ENABLE
+    help
+        Switch to rate mode via ch6 of the external receiver.
+
+config DECK_EXTRX_TAER
+    bool "Use TAER channel mapping"
+    default n
+    depends on DECK_CPPM || DECK_BIGQUAD_ENABLE
+    help
+        Expecting TAER channel mapping instead of the default AETR.
+
 config DECK_FLOW
     bool "Support the Flow (v1 and v2) deck"
     default y

src/drivers/src/cppm.c

@@ -40,35 +40,35 @@
 #include "debug.h"
 #include "log.h"
 
-#ifdef CPPM_USE_PB4
+#ifdef CONFIG_DECK_CPPM_USE_PB4
   #define CPPM_TIMER_NUMBER            3
   #define CPPM_TIMER_CHANNEL           1
   #define CPPM_GPIO_RCC                RCC_AHB1Periph_GPIOB
   #define CPPM_GPIO_PORT               GPIOB
   #define CPPM_GPIO_PIN                GPIO_Pin_4
   #define CPPM_GPIO_SOURCE             GPIO_PinSource4
-#elif defined(CPPM_USE_PB5) 
+#elif defined(CONFIG_DECK_CPPM_USE_PB5) 
   #define CPPM_TIMER_NUMBER            3
   #define CPPM_TIMER_CHANNEL           2
   #define CPPM_GPIO_RCC                RCC_AHB1Periph_GPIOB
   #define CPPM_GPIO_PORT               GPIOB
   #define CPPM_GPIO_PIN                GPIO_Pin_5
   #define CPPM_GPIO_SOURCE             GPIO_PinSource5
-#elif defined(CPPM_USE_PB8)
+#elif defined(CONFIG_DECK_CPPM_USE_PB8)
   #define CPPM_TIMER_NUMBER            10
   #define CPPM_TIMER_CHANNEL           1
   #define CPPM_GPIO_RCC                RCC_AHB1Periph_GPIOB
   #define CPPM_GPIO_PORT               GPIOB
   #define CPPM_GPIO_PIN                GPIO_Pin_8
   #define CPPM_GPIO_SOURCE             GPIO_PinSource8
-#elif defined(CPPM_USE_PA2)
+#elif defined(CONFIG_DECK_CPPM_USE_PA2)
   #define CPPM_TIMER_NUMBER            9
   #define CPPM_TIMER_CHANNEL           1
   #define CPPM_GPIO_RCC                RCC_AHB1Periph_GPIOA
   #define CPPM_GPIO_PORT               GPIOA
   #define CPPM_GPIO_PIN                GPIO_Pin_2
   #define CPPM_GPIO_SOURCE             GPIO_PinSource2
-#elif defined(CPPM_USE_PA3)
+#elif defined(CONFIG_DECK_CPPM_USE_PA3)
   #define CPPM_TIMER_NUMBER            9
   #define CPPM_TIMER_CHANNEL           2
   #define CPPM_GPIO_RCC                RCC_AHB1Periph_GPIOA

src/modules/interface/crtp_commander.h

@@ -34,4 +34,10 @@ void crtpCommanderInit(void);
 void crtpCommanderRpytDecodeSetpoint(setpoint_t *setpoint, CRTPPacket *pk);
 void crtpCommanderGenericDecodeSetpoint(setpoint_t *setpoint, CRTPPacket *pk);
 
+float getCPPMRollScale();
+float getCPPMRollRateScale();
+float getCPPMPitchScale();
+float getCPPMPitchRateScale();
+float getCPPMYawRateScale();
+
 #endif /* CRTP_COMMANDER_H_ */

src/modules/interface/extrx.h

@@ -28,4 +28,5 @@
 #include <stdint.h>
 
 void extRxInit(void);
+float extRxGetNormalizedChannelValue(uint8_t channel);
 

src/modules/src/crtp_commander_generic.c

@@ -147,15 +147,21 @@ static void zDistanceDecoder(setpoint_t *setpoint, uint8_t type, const void *dat
   setpoint->attitude.pitch = values->pitch;
 }
 
-/* cppmEmuDecoder
- * CRTP packet containing an emulation of CPPM channels
+/**
+ * The CPPM (Combined Pulse Position Modulation) commander packet contains
+ * an emulation of CPPM channels transmitted in a CRTP packet that can be sent
+ * from e.g. a RC Transmitter. Often running custom firmware such as Deviation.
+ *
  * Channels have a range of 1000-2000 with a midpoint of 1500
  * Supports the ordinary RPYT channels plus up to MAX_AUX_RC_CHANNELS auxiliary channels.
  * Auxiliary channels are optional and transmitters do not have to transmit all the data
  * unless a given channel is actually in use (numAuxChannels must be set accordingly)
  *
  * Current aux channel assignments:
- * - AuxChannel0: set high to enable self-leveling, low to disable
+ *  AuxChannel0: set high to enable self-leveling, low to disable
+ *
+ * The scaling can be configured using s_CppmEmuRollMax... parameters, setting the maximum
+ * angle/rate output given a maximum stick input (1000 or 2000).
  */
 #define MAX_AUX_RC_CHANNELS 10
 
@@ -183,6 +189,31 @@ static inline float getChannelUnitMultiplier(uint16_t channelValue, uint16_t cha
   return ((float)channelValue - (float)channelMidpoint) / (float)channelRange;
 }
 
+float getCPPMRollScale()
+{
+  return s_CppmEmuRollMaxAngleDeg;
+}
+
+float getCPPMRollRateScale()
+{
+  return s_CppmEmuRollMaxRateDps;
+}
+
+float getCPPMPitchScale()
+{
+  return s_CppmEmuPitchMaxAngleDeg;
+}
+
+float getCPPMPitchRateScale()
+{
+  return s_CppmEmuPitchMaxRateDps;
+}
+
+float getCPPMYawRateScale()
+{
+  return s_CppmEmuYawMaxRateDps;
+}
+
 static void cppmEmuDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
 {
   bool isSelfLevelEnabled = true;
@@ -400,22 +431,13 @@ void crtpCommanderGenericDecodeSetpoint(setpoint_t *setpoint, CRTPPacket *pk)
 }
 
 /**
- * The CPPM (Combined Pulse Position Modulation) commander packet contains
- * an emulation of CPPM channels transmitted in a CRTP packet that can be sent
- * from e.g. a RC Transmitter. Often running custom firmware such as Deviation.
- *
- * Channels have a range of 1000-2000 with a midpoint of 1500
- * Supports the ordinary RPYT channels plus up to MAX_AUX_RC_CHANNELS auxiliary channels.
- * Auxiliary channels are optional and transmitters do not have to transmit all the data
- * unless a given channel is actually in use (numAuxChannels must be set accordingly)
- *
- * Current aux channel assignments:
- *  AuxChannel0: set high to enable self-leveling, low to disable
- *
- * The scaling can be configured using the parameters, setting the maximum
- * angle/rate output given a maximum stick input (1000 or 2000).
+ * The CPPM (Combined Pulse Position Modulation) parameters
+ * configure the maximum angle/rate output given a maximum stick input
+ * for CRTP packets with emulated CPPM channels (e.g. RC transmitters connecting
+ * directly to the NRF radio, often with a 4-in-1 Multimodule), or for CPPM channels
+ * from an external receiver.  
  */
-PARAM_GROUP_START(cmdrCPPM)
+PARAM_GROUP_START(cppm)
 
 /**
  * @brief Config of max roll rate at max stick input [DPS] (default: 720)
@@ -438,4 +460,4 @@ PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, angRoll, &s_CppmEmuRollMaxAngleDeg)
  */
 PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, rateYaw, &s_CppmEmuYawMaxRateDps)
 
-PARAM_GROUP_STOP(cmdrCPPM)
+PARAM_GROUP_STOP(cppm)

src/modules/src/extrx.c

@@ -37,6 +37,7 @@
 #include "commander.h"
 #include "uart1.h"
 #include "cppm.h"
+#include "crtp_commander.h"
 
 #define DEBUG_MODULE  "EXTRX"
 #include "debug.h"
@@ -71,13 +72,11 @@
 
 #define EXTRX_CH_ALTHOLD   4
 #define EXTRX_CH_ARM       5
+#define EXTRX_CH_MODE     6
 
 #define EXTRX_SIGN_ALTHOLD   (-1)
 #define EXTRX_SIGN_ARM       (-1)
-
-#define EXTRX_SCALE_ROLL   (40.0f)
-#define EXTRX_SCALE_PITCH  (40.0f)
-#define EXTRX_SCALE_YAW    (200.0f)
+#define EXTRX_SIGN_MODE       (-1)
 
 #define EXTRX_DEADBAND_ROLL (0.05f)
 #define EXTRX_DEADBAND_PITCH (0.05f)
@@ -87,24 +86,23 @@
 
 bool extRxArm = false;
 bool extRxAltHold = false;
+bool extRxModeRate = false;
 
-#ifndef EXTRX_ARMING
-  #define EXTRX_ARMING    false
-#endif
-#if EXTRX_ARMING
+#if CONFIG_DECK_EXTRX_ARMING
   bool extRxArmPrev = false;
   int8_t arm_cnt = 0;
 #endif
 
-#ifndef EXTRX_ALT_HOLD
-  #define EXTRX_ALT_HOLD    false
-#endif
-#if EXTRX_ALT_HOLD
+#if CONFIG_DECK_EXTRX_ALT_HOLD
   #define EXTRX_DEADBAND_ZVEL  (0.25f)
   bool extRxAltHoldPrev = false;
   int8_t altHold_cnt = 0;
 #endif
 
+#if CONFIG_DECK_EXTRX_MODE_RATE
+  bool extRxModeRatePrev = false;
+  int8_t modeRate_cnt = 0;
+#endif
 
 static setpoint_t extrxSetpoint;
 static uint16_t ch[EXTRX_NR_CHANNELS] = {0};
@@ -125,7 +123,7 @@ void extRxInit(void)
 
 #ifdef ENABLE_CPPM
   cppmInit();
-  #ifdef EXTRX_TAER
+  #ifdef CONFIG_DECK_EXTRX_TAER
     DEBUG_PRINT("CPPM initialized, expecting TAER channel mapping\n");
   #else
     DEBUG_PRINT("CPPM initialized, expecting AETR channel mapping\n");
@@ -156,7 +154,7 @@ static void extRxTask(void *param)
 static void extRxDecodeChannels(void)
 {
 
-  #if EXTRX_ARMING
+  #if CONFIG_DECK_EXTRX_ARMING
   if (EXTRX_SIGN_ARM * cppmConvert2Float(ch[EXTRX_CH_ARM], -1, 1, 0.0) > 0.5f) // channel needs to be 75% or more to work correctly with 2/3 way switches
   {
     if (arm_cnt < EXTRX_SWITCH_MIN_CNT) arm_cnt++;
@@ -183,7 +181,7 @@ static void extRxDecodeChannels(void)
   extRxArmPrev = extRxArm;
   #endif
 
-  #if EXTRX_ALT_HOLD
+  #if CONFIG_DECK_EXTRX_ALT_HOLD
   if (EXTRX_SIGN_ALTHOLD * cppmConvert2Float(ch[EXTRX_CH_ALTHOLD], -1, 1, 0.0) > 0.5f)
   {
     if (altHold_cnt < EXTRX_SWITCH_MIN_CNT) altHold_cnt++;
@@ -214,13 +212,51 @@ static void extRxDecodeChannels(void)
 
   extRxAltHoldPrev = extRxAltHold;
   #else
+
   extrxSetpoint.mode.z = modeDisable;
   extrxSetpoint.thrust = cppmConvert2uint16(ch[EXTRX_CH_THRUST]);
   #endif
+
+  #if CONFIG_DECK_EXTRX_MODE_RATE
+  if (EXTRX_SIGN_MODE * cppmConvert2Float(ch[EXTRX_CH_MODE], -1, 1, 0.0) > 0.5f) // channel needs to be 75% or more to work correctly with 2/3 way switches
+  {
+    if (modeRate_cnt < EXTRX_SWITCH_MIN_CNT) modeRate_cnt++;
+    else extRxModeRate = true;
+
+    if (extRxModeRatePrev != extRxModeRate)
+    {
+      DEBUG_PRINT("Switched to rate mode\n");
+      extrxSetpoint.mode.roll = modeVelocity;
+      extrxSetpoint.mode.pitch = modeVelocity;
+    }
+  }
+  else
+  {
+    if (modeRate_cnt > 0) modeRate_cnt--;
+    else extRxModeRate = false;
+
+    if (extRxModeRatePrev != extRxModeRate)
+    {
+      DEBUG_PRINT("Switched to level mode\n");
+      extrxSetpoint.mode.roll = modeAbs;
+      extrxSetpoint.mode.pitch = modeAbs;
+    }
+  }
+
+  extRxModeRatePrev = extRxModeRate;
+  #endif
 
-  extrxSetpoint.attitude.roll = EXTRX_SIGN_ROLL * EXTRX_SCALE_ROLL * cppmConvert2Float(ch[EXTRX_CH_ROLL], -1, 1, EXTRX_DEADBAND_ROLL);
-  extrxSetpoint.attitude.pitch = EXTRX_SIGN_PITCH * EXTRX_SCALE_PITCH * cppmConvert2Float(ch[EXTRX_CH_PITCH], -1, 1, EXTRX_DEADBAND_PITCH);
-  extrxSetpoint.attitudeRate.yaw = EXTRX_SIGN_YAW * EXTRX_SCALE_YAW *cppmConvert2Float(ch[EXTRX_CH_YAW], -1, 1, EXTRX_DEADBAND_YAW);
+  if (extRxModeRate)
+  {  
+    extrxSetpoint.attitudeRate.roll = EXTRX_SIGN_ROLL * getCPPMRollRateScale() * cppmConvert2Float(ch[EXTRX_CH_ROLL], -1, 1, EXTRX_DEADBAND_ROLL);
+    extrxSetpoint.attitudeRate.pitch = EXTRX_SIGN_PITCH * getCPPMPitchRateScale() * cppmConvert2Float(ch[EXTRX_CH_PITCH], -1, 1, EXTRX_DEADBAND_PITCH);
+  }
+  else
+  {  
+    extrxSetpoint.attitude.roll = EXTRX_SIGN_ROLL * getCPPMRollScale() * cppmConvert2Float(ch[EXTRX_CH_ROLL], -1, 1, EXTRX_DEADBAND_ROLL);
+    extrxSetpoint.attitude.pitch = EXTRX_SIGN_PITCH * getCPPMPitchScale() * cppmConvert2Float(ch[EXTRX_CH_PITCH], -1, 1, EXTRX_DEADBAND_PITCH);
+  }
+  extrxSetpoint.attitudeRate.yaw = EXTRX_SIGN_YAW * getCPPMYawRateScale() * cppmConvert2Float(ch[EXTRX_CH_YAW], -1, 1, EXTRX_DEADBAND_YAW);
 
   commanderSetSetpoint(&extrxSetpoint, COMMANDER_PRIORITY_EXTRX);
 }
@@ -349,6 +385,14 @@ LOG_ADD(LOG_FLOAT, roll, &extrxSetpoint.attitude.roll)
  * @brief External RX pitch setpoint
  */
 LOG_ADD(LOG_FLOAT, pitch, &extrxSetpoint.attitude.pitch)
+/**
+ * @brief External RX roll rate setpoint
+ */
+LOG_ADD(LOG_FLOAT, rollRate, &extrxSetpoint.attitudeRate.roll)
+/**
+ * @brief External RX pitch rate setpoint
+ */
+LOG_ADD(LOG_FLOAT, pitchRate, &extrxSetpoint.attitudeRate.pitch)
 /**
  * @brief External RX yaw rate setpoint
  */
@@ -366,4 +410,4 @@ LOG_ADD(LOG_UINT8, AltHold, &extRxAltHold)
  */
 LOG_ADD(LOG_UINT8, Arm, &extRxArm)
 LOG_GROUP_STOP(extrx)
-#endif
+#endif