diff --git a/ArduCopterMega/APM_Config.h.reference b/ArduCopterMega/APM_Config.h.reference index 68b433b5a2..cecd7722bd 100644 --- a/ArduCopterMega/APM_Config.h.reference +++ b/ArduCopterMega/APM_Config.h.reference @@ -712,6 +712,21 @@ //#define LOG_CMD ENABLED // +////////////////////////////////////////////////////////////////////////////// +// RC override +// +// ALLOW_RC_OVERRIDE OPTIONAL +// +// This is for advanced used only! +// +// If you enable ALLOW_RC_OVERRIDE then a GCS will be able to +// override RC input values using a MAVLINK_MSG_ID_RC_CHANNELS_RAW +// message sent to APM. This allows the GCS to precisely control +// all PWM values as seen by APM, which means it can set any switch +// position, or set a precise control value. Do not enable unless you +// really know what you are doing! +// + // // Do not remove - this is to discourage users from copying this file // and using it as-is rather than editing their own. diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index 0c9ac3ac54..9dc8b3cbf4 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -673,6 +673,28 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } // end case +#if ALLOW_RC_OVERRIDE == ENABLED + case MAVLINK_MSG_ID_RC_CHANNELS_RAW: + { + // allow override of RC channel values for HIL + // or for complete GCS control of switch position + // and RC PWM values. + mavlink_rc_channels_raw_t packet; + int16_t v[8]; + mavlink_msg_rc_channels_raw_decode(msg, &packet); + v[0] = packet.chan1_raw; + v[1] = packet.chan2_raw; + v[2] = packet.chan3_raw; + v[3] = packet.chan4_raw; + v[4] = packet.chan5_raw; + v[5] = packet.chan6_raw; + v[6] = packet.chan7_raw; + v[7] = packet.chan8_raw; + APM_RC.setHIL(v); + break; + } +#endif + #if HIL_MODE != HIL_MODE_DISABLED // This is used both as a sensor and to pass the location // in HIL_ATTITUDE mode. @@ -700,23 +722,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_RC_CHANNELS_RAW: - { - // allow override of RC channel values for HIL - mavlink_rc_channels_raw_t packet; - int16_t v[8]; - mavlink_msg_rc_channels_raw_decode(msg, &packet); - v[0] = packet.chan1_raw; - v[1] = packet.chan2_raw; - v[2] = packet.chan3_raw; - v[3] = packet.chan4_raw; - v[4] = packet.chan5_raw; - v[5] = packet.chan6_raw; - v[6] = packet.chan7_raw; - v[7] = packet.chan8_raw; - APM_RC.setHIL(v); - break; - } #endif #if HIL_MODE == HIL_MODE_ATTITUDE case MAVLINK_MSG_ID_ATTITUDE: diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 0342f8052e..d78d1bb412 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -550,3 +550,11 @@ #define STANDARD_SPEED_SQUARED (STANDARD_SPEED * STANDARD_SPEED) #endif #define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE) + + +////////////////////////////////////////////////////////////////////////////// +// RC override +// +#ifndef ALLOW_RC_OVERRIDE +# define ALLOW_RC_OVERRIDE DISABLED +#endif