Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Added function to transform MANUAL_CONTROL messages to RC override. This allows MANUAL_CONTROL to work properly without changing much else.

Browse Source
This commit is contained in:
Rustom Jehangir 2016-01-10 16:23:03 -08:00 committed by Andrew Tridgell
parent 57b6303b07
commit 08a56ee6d2
3 changed files with 31 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/Copter.h
View File

@ -914,6 +914,7 @@ private:
void init_rc_out(); void init_rc_out();
void enable_motor_output(); void enable_motor_output();
void read_radio(); void read_radio();
void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons);
void set_throttle_and_failsafe(uint16_t throttle_pwm); void set_throttle_and_failsafe(uint16_t throttle_pwm);
void set_throttle_zero_flag(int16_t throttle_control); void set_throttle_zero_flag(int16_t throttle_control);
void init_barometer(bool full_calibration); void init_barometer(bool full_calibration);

37
ArduSub/GCS_Mavlink.cpp
View File

@ -1107,36 +1107,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} }
case MAVLINK_MSG_ID_MANUAL_CONTROL: // MAV ID: 69 case MAVLINK_MSG_ID_MANUAL_CONTROL: // MAV ID: 69
{ {
if(msg->sysid != copter.g.sysid_my_gcs) break; // Only accept control from our gcs if(msg->sysid != copter.g.sysid_my_gcs) break; // Only accept control from our gcs
mavlink_manual_control_t packet; mavlink_manual_control_t packet;
int16_t v[8]; mavlink_msg_manual_control_decode(msg, &packet);
uint16_t b;
mavlink_msg_manual_control_decode(msg, &packet);
static uint16_t skip = 0; copter.transform_manual_control_to_rc_override(packet.x,packet.y,packet.z,packet.r,packet.buttons);
if ( skip++ % 25 == 0 ) { // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
copter.gcs_send_text_fmt(MAV_SEVERITY_INFO,"%4.0f,%4.0f,%4.0f",packet.x,packet.y,packet.z); copter.failsafe.last_heartbeat_ms = AP_HAL::millis();
} break;
}
v[0] = packet.x/2+1500;
v[1] = packet.y/2+1500;
v[2] = packet.z+1000;
v[3] = packet.r/2+1500;
v[4] = packet.buttons;
v[5] = 0;
v[6] = 0;
v[7] = 0;
b = packet.buttons;
// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
copter.failsafe.rc_override_active = hal.rcin->set_overrides(v, 8);
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
copter.failsafe.last_heartbeat_ms = AP_HAL::millis();
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: // MAV ID: 70 case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: // MAV ID: 70
{ {

21
ArduSub/radio.cpp
View File

@ -141,6 +141,27 @@ void Copter::read_radio()
} }
} }
void Copter::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) {
int16_t channels[8];
float rpyScale = 0.4;
float throttleScale = 0.8;
int16_t rpyCenter = 1500;
int16_t throttleBase = 1100;
channels[0] = x*rpyScale+rpyCenter; // pitch (forward for ROV)
channels[1] = y*rpyScale+rpyCenter; // roll (strafe for ROV)
channels[2] = z*throttleScale+throttleBase; // throttle
channels[3] = r*rpyScale+rpyCenter; // yaw
channels[4] = buttons; // for testing only
channels[5] = 1100;
channels[6] = 1100;
channels[7] = 1500; // camera tilt
// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
failsafe.rc_override_active = hal.rcin->set_overrides(channels, 8);
}
#define FS_COUNTER 3 // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value #define FS_COUNTER 3 // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value
void Copter::set_throttle_and_failsafe(uint16_t throttle_pwm) void Copter::set_throttle_and_failsafe(uint16_t throttle_pwm)
{ {