Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
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.

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 enable_motor_output();
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_zero_flag(int16_t throttle_control);
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
{
if(msg->sysid != copter.g.sysid_my_gcs) break; // Only accept control from our gcs
mavlink_manual_control_t packet;
int16_t v[8];
uint16_t b;
mavlink_msg_manual_control_decode(msg, &packet);
{
if(msg->sysid != copter.g.sysid_my_gcs) break; // Only accept control from our gcs
mavlink_manual_control_t packet;
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 ) {
copter.gcs_send_text_fmt(MAV_SEVERITY_INFO,"%4.0f,%4.0f,%4.0f",packet.x,packet.y,packet.z);
}
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;
}
// 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
{

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
void Copter::set_throttle_and_failsafe(uint16_t throttle_pwm)
{