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

Sub: Rework to support override changes

This commit is contained in:
Michael du Breuil 2018-05-28 23:26:16 -07:00 committed by Andrew Tridgell
parent 6c94811e00
commit c642d1ed74
2 changed files with 31 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduSub/GCS_Mavlink.cpp
View File

@ -713,21 +713,24 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
if (msg->sysid != sub.g.sysid_my_gcs) {
break; // Only accept control from our gcs
}
uint32_t tnow = AP_HAL::millis();
mavlink_rc_channels_override_t packet;
mavlink_msg_rc_channels_override_decode(msg, &packet);
RC_Channels::set_override(0, packet.chan1_raw);
RC_Channels::set_override(1, packet.chan2_raw);
RC_Channels::set_override(2, packet.chan3_raw);
RC_Channels::set_override(3, packet.chan4_raw);
RC_Channels::set_override(4, packet.chan5_raw);
RC_Channels::set_override(5, packet.chan6_raw);
RC_Channels::set_override(6, packet.chan7_raw);
RC_Channels::set_override(7, packet.chan8_raw);
RC_Channels::set_override(0, packet.chan1_raw, tnow);
RC_Channels::set_override(1, packet.chan2_raw, tnow);
RC_Channels::set_override(2, packet.chan3_raw, tnow);
RC_Channels::set_override(3, packet.chan4_raw, tnow);
RC_Channels::set_override(4, packet.chan5_raw, tnow);
RC_Channels::set_override(5, packet.chan6_raw, tnow);
RC_Channels::set_override(6, packet.chan7_raw, tnow);
RC_Channels::set_override(7, packet.chan8_raw, tnow);
sub.failsafe.last_pilot_input_ms = AP_HAL::millis();
sub.failsafe.last_pilot_input_ms = tnow;
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
sub.failsafe.last_heartbeat_ms = AP_HAL::millis();
sub.failsafe.last_heartbeat_ms = tnow;
break;
}

33
ArduSub/joystick.cpp
View File

@ -91,28 +91,30 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
rollTrim = y * rpyScale;
}
RC_Channels::set_override(0, constrain_int16(pitchTrim + rpyCenter,1100,1900)); // pitch
RC_Channels::set_override(1, constrain_int16(rollTrim + rpyCenter,1100,1900)); // roll
uint32_t tnow = AP_HAL::millis();
RC_Channels::set_override(2, constrain_int16((z+zTrim)*throttleScale+throttleBase,1100,1900)); // throttle
RC_Channels::set_override(3, constrain_int16(r*rpyScale+rpyCenter,1100,1900)); // yaw
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)); // forward for ROV
RC_Channels::set_override(5, constrain_int16((y+yTrim)*rpyScale+rpyCenter,1100,1900)); // lateral for ROV
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)); // forward for ROV
RC_Channels::set_override(5, constrain_int16(yTrim*rpyScale+rpyCenter,1100,1900)); // lateral for ROV
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); // camera pan
RC_Channels::set_override(7, cam_tilt); // camera tilt
RC_Channels::set_override(8, lights1); // lights 1
RC_Channels::set_override(9, lights2); // lights 2
RC_Channels::set_override(10, video_switch); // video switch
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;
@ -678,13 +680,14 @@ void Sub::default_js_buttons()
void Sub::set_neutral_controls()
{
uint32_t tnow = AP_HAL::millis();
for (uint8_t i = 0; i < 6; i++) {
RC_Channels::set_override(i, 1500);
RC_Channels::set_override(i, 1500, tnow);
}
for (uint8_t i = 6; i < 11; i++) {
RC_Channels::set_override(i, 0xffff);
RC_Channels::set_override(i, 0xffff, tnow);
}
// Clear pitch/roll trim settings