Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Add light control to joystick

This commit is contained in:
Jacob Walser 2017-02-09 15:20:36 -05:00 committed by Andrew Tridgell
parent fb64195cc7
commit eb76fc7a3c
1 changed files with 29 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ArduSub/radio.cpp
View File

@ -144,6 +144,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) { 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]; int16_t channels[10];
uint32_t tnow_ms = millis();
float rpyScale = 0.5; float rpyScale = 0.5;
float throttleScale = 0.8; float throttleScale = 0.8;
int16_t rpyCenter = 1500; int16_t rpyCenter = 1500;
@ -151,31 +153,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 rollTrim = 0;
static int16_t mode; static int16_t mode;
static int16_t camTilt = 1500; 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) ) { if ( buttons & (1 << 4) ) {
init_arm_motors(true); init_arm_motors(true);
} else if ( buttons & (1 << 5) ) { } else if ( buttons & (1 << 5) ) {
init_disarm_motors(); init_disarm_motors();
} }
// Button logic to change camera tilt (D-pad up and down + left joystick click to center)
if ( buttons & (1 << 0) ) { if ( buttons & (1 << 0) ) {
camTilt = constrain_float(camTilt+20,800,2200); camTilt = constrain_float(camTilt+20,800,2200);
} else if ( buttons & (1 << 1) ) { } else if ( buttons & (1 << 1) ) {
camTilt = constrain_float(camTilt-20,800,2200); 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 ) { if ( (buttons & ( 1 << 2 )) && rollTrim > -200 ) {
rollTrim -= 10; rollTrim -= 10;
} else if ( (buttons & ( 1 << 3 )) && rollTrim < 200 ) { } else if ( (buttons & ( 1 << 3 )) && rollTrim < 200 ) {
rollTrim += 10; rollTrim += 10;
} }
// Button logic for mode changes (B for stabilize, Y for altitude hold)
if ( buttons & (1 << 14) ) { if ( buttons & (1 << 14) ) {
mode = 2000; mode = 2000;
} else if ( buttons & (1 << 12)) { } else if ( buttons & (1 << 12)) {
mode = 1000; 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[0] = 1500; // pitch
channels[1] = 1500 + rollTrim; // roll channels[1] = 1500 + rollTrim; // roll
channels[2] = z*throttleScale+throttleBase; // throttle channels[2] = z*throttleScale+throttleBase; // throttle
@ -184,7 +212,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[5] = x*rpyScale+rpyCenter; // forward for ROV
channels[6] = y*rpyScale+rpyCenter; // strafe for ROV channels[6] = y*rpyScale+rpyCenter; // strafe for ROV
channels[7] = camTilt; // camera tilt channels[7] = camTilt; // camera tilt
channels[8] = 0; channels[8] = lights;
channels[9] = 0; channels[9] = 0;
// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation // record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation