make it possible to enable RC override on non-HIL setups

this makes it possible to allow a GCS to override RC input values for
non-HIL setups. This gives a way for a GCS to set an arbitrary switch
value, or set a precise PWM value for any control.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1789 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
tridge60@gmail.com 2011-03-19 10:18:21 +00:00
parent f0458631a3
commit f9469507c0
3 changed files with 45 additions and 17 deletions

View File

@ -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.

View File

@ -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:

View File

@ -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