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[11];
float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain
float throttleScale = 0.8*gain*g.throttle_gain; // Scale 0-1000 to 0-800 times gain
int16_t rpyCenter = 1500;
int16_t throttleBase = 1500-500*throttleScale;
bool shift = false;
// Neutralize camera tilt speed setpoint
cam_tilt = 1500;
// Detect if any shift button is pressed
for (uint8_t i = 0 ; i < 16 ; i++) {
if ((buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) {
shift = true;
}
}
// Act if button is pressed
// Only act upon pressing button and ignore holding. This provides compatibility with Taranis as joystick.
for (uint8_t i = 0 ; i < 16 ; i++) {
if ((buttons & (1 << i))) {
handle_jsbutton_press(i,shift,(buttons_prev & (1 << i)));
}
}
buttons_prev = buttons;
// Set channels to override
if (!roll_pitch_flag) {
channels[0] = 1500 + pitchTrim; // pitch
channels[1] = 1500 + rollTrim; // roll
} else {
// adjust roll and pitch with joystick input instead of forward and lateral
channels[0] = constrain_int16((x+pitchTrim)*rpyScale+rpyCenter,1100,1900);
channels[1] = constrain_int16((y+rollTrim)*rpyScale+rpyCenter,1100,1900);
}
channels[2] = constrain_int16((z+zTrim)*throttleScale+throttleBase,1100,1900); // throttle
channels[3] = constrain_int16(r*rpyScale+rpyCenter,1100,1900); // yaw
if (!roll_pitch_flag) {
// adjust forward and lateral with joystick input instead of roll and pitch
channels[4] = constrain_int16((x+xTrim)*rpyScale+rpyCenter,1100,1900); // forward for ROV
channels[5] = constrain_int16((y+yTrim)*rpyScale+rpyCenter,1100,1900); // lateral for ROV
} else {
// neutralize forward and lateral input while we are adjusting roll and pitch
channels[4] = constrain_int16(xTrim*rpyScale+rpyCenter,1100,1900); // forward for ROV
channels[5] = constrain_int16(yTrim*rpyScale+rpyCenter,1100,1900); // lateral for ROV
}
channels[6] = 0; // Unused
channels[7] = cam_tilt; // camera tilt
channels[8] = lights1; // lights 1
channels[9] = lights2; // lights 2
channels[10] = video_switch; // video switch
// Store old x, y, z values for use in input hold logic
x_last = x;
y_last = y;
z_last = z;
hal.rcin->set_overrides(channels, 10);
}
void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons)
{
float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain
float throttleScale = 0.8*gain*g.throttle_gain; // Scale 0-1000 to 0-800 times gain
int16_t rpyCenter = 1500;
int16_t throttleBase = 1500-500*throttleScale;
bool shift = false;
// Neutralize camera tilt and pan speed setpoint
cam_tilt = 1500;
cam_pan = 1500;
// Detect if any shift button is pressed
for (uint8_t i = 0 ; i < 16 ; i++) {
if ((buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) {
shift = true;
}
}
// Act if button is pressed
// Only act upon pressing button and ignore holding. This provides compatibility with Taranis as joystick.
for (uint8_t i = 0 ; i < 16 ; i++) {
if ((buttons & (1 << i))) {
handle_jsbutton_press(i,shift,(buttons_prev & (1 << i)));
// buttonDebounce = tnow_ms;
} else if (buttons_prev & (1 << i)) {
handle_jsbutton_release(i, shift);
}
}
buttons_prev = buttons;
// attitude mode:
if (roll_pitch_flag == 1) {
// adjust roll/pitch trim with joystick input instead of forward/lateral
pitchTrim = -x * rpyScale;
rollTrim = y * rpyScale;
}
uint32_t tnow = AP_HAL::millis();
RC_Channels::set_override(0, constrain_int16(pitchTrim + rpyCenter,1100,1900), tnow); // pitch
RC_Channels::set_override(1, constrain_int16(rollTrim + rpyCenter,1100,1900), tnow); // roll
RC_Channels::set_override(2, constrain_int16((z+zTrim)*throttleScale+throttleBase,1100,1900), tnow); // throttle
RC_Channels::set_override(3, constrain_int16(r*rpyScale+rpyCenter,1100,1900), tnow); // yaw
// maneuver mode:
if (roll_pitch_flag == 0) {
// adjust forward and lateral with joystick input instead of roll and pitch
RC_Channels::set_override(4, constrain_int16((x+xTrim)*rpyScale+rpyCenter,1100,1900), tnow); // forward for ROV
RC_Channels::set_override(5, constrain_int16((y+yTrim)*rpyScale+rpyCenter,1100,1900), tnow); // lateral for ROV
} else {
// neutralize forward and lateral input while we are adjusting roll and pitch
RC_Channels::set_override(4, constrain_int16(xTrim*rpyScale+rpyCenter,1100,1900), tnow); // forward for ROV
RC_Channels::set_override(5, constrain_int16(yTrim*rpyScale+rpyCenter,1100,1900), tnow); // lateral for ROV
}
RC_Channels::set_override(6, cam_pan, tnow); // camera pan
RC_Channels::set_override(7, cam_tilt, tnow); // camera tilt
RC_Channels::set_override(8, lights1, tnow); // lights 1
RC_Channels::set_override(9, lights2, tnow); // lights 2
RC_Channels::set_override(10, video_switch, tnow); // video switch
// Store old x, y, z values for use in input hold logic
x_last = x;
y_last = y;
z_last = z;
}
