From a42aed332bbb33e6de3adefebc469bbdae29057c Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Sun, 10 Jan 2016 20:51:58 +0100
Subject: [PATCH] Rccommand scaled to FPV cam (cli command:
 rc_fpv_cam_correct_degrees)

---
 src/main/config/config.c |  3 ++-
 src/main/io/serial_cli.c |  1 +
 src/main/mw.c            | 12 ++++++++++++
 src/main/rx/rx.h         |  1 +
 4 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index 8c5637079..4d5fa343b 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 116;
+static const uint8_t EEPROM_CONF_VERSION = 117;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -442,6 +442,7 @@ static void resetConf(void)
     masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
     masterConfig.rxConfig.rssi_ppm_invert = 0;
     masterConfig.rxConfig.rcSmoothing = 0;
+    masterConfig.rxConfig.fpvCamAngleDegrees = 0;
 
     resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);
 
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 93ed6bf30..64674357d 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -460,6 +460,7 @@ const clivalue_t valueTable[] = {
     { "rssi_ppm_invert",            VAR_INT8   | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.rxConfig.rssi_ppm_invert, .config.lookup = { TABLE_OFF_ON } },
     { "input_filtering_mode",       VAR_INT8   | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON } },
     { "rc_smoothing",               VAR_INT8   | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_OFF_ON } },
+    { "rc_fpv_cam_correct_degrees", VAR_UINT8  | MASTER_VALUE,  &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0,  50 } },
 
     { "min_throttle",               VAR_UINT16 | MASTER_VALUE,  &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO,  PWM_RANGE_MAX } },
     { "max_throttle",               VAR_UINT16 | MASTER_VALUE,  &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO,  PWM_RANGE_MAX } },
diff --git a/src/main/mw.c b/src/main/mw.c
index db9eb66cd..408fb117a 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -207,6 +207,13 @@ void filterRc(void){
     }
 }
 
+void scaleRcCommandToFpvCamAngle(void) {
+    int16_t roll = rcCommand[ROLL];
+    int16_t yaw = rcCommand[YAW];
+    rcCommand[ROLL] = constrain(cos(masterConfig.rxConfig.fpvCamAngleDegrees) * roll + sin(masterConfig.rxConfig.fpvCamAngleDegrees) * yaw, -500, 500);
+    rcCommand[YAW] = constrain(cos(masterConfig.rxConfig.fpvCamAngleDegrees) * yaw + sin(masterConfig.rxConfig.fpvCamAngleDegrees) * roll, -500, 500);
+}
+
 void annexCode(void)
 {
     int32_t tmp, tmp2;
@@ -285,6 +292,11 @@ void annexCode(void)
         filterRc();
     }
 
+    // experimental scaling of RC command to FPV cam angle
+    if (masterConfig.rxConfig.fpvCamAngleDegrees && !FLIGHT_MODE(HEADFREE_MODE)) {
+        scaleRcCommandToFpvCamAngle();
+    }
+
     if (ARMING_FLAG(ARMED)) {
         LED0_ON;
     } else {
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 2ab071959..19ae78eae 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -118,6 +118,7 @@ typedef struct rxConfig_s {
     uint16_t mincheck;                      // minimum rc end
     uint16_t maxcheck;                      // maximum rc end
     uint8_t rcSmoothing;
+    uint8_t fpvCamAngleDegrees;             // Camera angle to be scaled into rc commands
 
     uint16_t rx_min_usec;
     uint16_t rx_max_usec;