From 26ccd43872744928466d32eec0e6bbadc851f003 Mon Sep 17 00:00:00 2001
From: Rustom Jehangir <rustyj110@gmail.com>
Date: Wed, 10 Feb 2016 19:57:31 -0800
Subject: [PATCH] Sub: Added light control to joystick and docs

---
 ArduSub/README.md | 10 ++++++++--
 ArduSub/radio.cpp | 30 +++++++++++++++++++++++++++++-
 2 files changed, 37 insertions(+), 3 deletions(-)

diff --git a/ArduSub/README.md b/ArduSub/README.md
index 8b92e716af..2e1bb7af95 100644
--- a/ArduSub/README.md
+++ b/ArduSub/README.md
@@ -98,8 +98,12 @@ make px4-v2-vectored-upload
 	- Set all to stabilize
 	- Set last flight mode to altHold (Learning mode)
 9. Set Up Camera Tilt (if desired)
-	- Use camera tab to connect RC8 to Output Channel 8
-	- Connect camera tilt servo or gimbal to Ch. 8
+	- Use camera tab to connect RC8 to Output Channel 7
+	- Connect camera tilt servo or gimbal to Ch. 7
+10. Set up Lights
+	- Use camera tab to connect RC9 to Output Channel 8
+	- Set range to 1100-1900
+	- Connect lights PWM signal to Ch. 8
 
 ### Operation ###
 
@@ -113,6 +117,8 @@ The gamepad controls the ROV during operation. It has been tested with the Micro
 - **B Button:** Switch to stabilize (manual) mode
 - **Buttonpad Up/Down Arrows:** Tilt camera (if applicable and set up in QGC)
 - **Buttonpad Left/Right Arrows:** Increase/decrease roll trim (if IMU is not completely level)
+- **Right Joystick Click:** Cycle through light brightness
+- **Left Joystick Click:** Reset camera tilt angle
 
 #### Modes ####
 
diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp
index 40976ab796..914b5a04dd 100644
--- a/ArduSub/radio.cpp
+++ b/ArduSub/radio.cpp
@@ -135,6 +135,8 @@ void Sub::read_radio()
 void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) {
 	int16_t channels[10];
 
+	uint32_t tnow_ms = millis();
+
 	float rpyScale = 0.5;
 	float throttleScale = 0.8;
 	int16_t rpyCenter = 1500;
@@ -142,31 +144,57 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
 	static int16_t rollTrim = 0;
 	static int16_t mode;
 	static int16_t camTilt = 1500;
+	static int16_t lights = 1100;
+	static uint32_t lastLights;
+	static bool lightsBrighter = true;
 
+	// Button logic to arm/disarm motors (Start and back buttons)
 	if ( buttons & (1 << 4) ) {
 		init_arm_motors(true);
 	} else if ( buttons & (1 << 5) ) {
 		init_disarm_motors();
 	}
 
+	// Button logic to change camera tilt (D-pad up and down + left joystick click to center)
 	if ( buttons & (1 << 0) ) {
 		camTilt = constrain_float(camTilt+20,800,2200);
 	} else if ( buttons & (1 << 1) ) {
 		camTilt = constrain_float(camTilt-20,800,2200);
+	} else if ( buttons & (1 << 6) ) {
+		camTilt = 1500; // Reset camera tilt
 	}
 
+	// Button logic for roll trim (D-pad left and right)
 	if ( (buttons & ( 1 << 2 )) && rollTrim > -200 ) {
 		rollTrim -= 10;
 	} else if ( (buttons & ( 1 << 3 )) && rollTrim < 200 ) {
 		rollTrim += 10;
 	}
 
+	// Button logic for mode changes (B for stabilize, Y for altitude hold)
 	if ( buttons & (1 << 14) ) {
 		mode = 2000;
 	} else if ( buttons & (1 << 12)) {
 		mode = 1000;
 	}
 
+	// Button logic for lights with cyclical dimming (right joystick click)
+	if ( (buttons & (1 << 7)) && (tnow_ms - lastLights > 100) ) {
+		lastLights = tnow_ms;
+
+		if ( lightsBrighter ) {
+			lights += 200;
+		} else {
+			lights -= 200;
+		}
+
+		if ( lights >= 1900 ) {
+			lightsBrighter = false;
+		} else if ( lights <= 1100 ) {
+			lightsBrighter = true;
+		}
+	}
+
 	channels[0] = 1500;                           // pitch
 	channels[1] = 1500 + rollTrim;                // roll
 	channels[2] = z*throttleScale+throttleBase;   // throttle
@@ -175,7 +203,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
 	channels[5] = x*rpyScale+rpyCenter;           // forward for ROV
 	channels[6] = y*rpyScale+rpyCenter;           // strafe for ROV
 	channels[7] = camTilt;                        // camera tilt
-	channels[8] = 0;
+	channels[8] = lights;
 	channels[9] = 0;
 
 	// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation