added radio override via MAVLink

This allows MAVLink commands to override the RC input values. See the
discussion on the mavlink list.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1738 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
tridge60@gmail.com 2011-03-03 11:40:06 +00:00
parent 7691133230
commit b7e70bb1e6
2 changed files with 25 additions and 0 deletions

View File

@ -618,6 +618,24 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
airspeed = 100*packet.airspeed;
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

@ -112,6 +112,13 @@ void init_ardupilot()
Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use());
}
#ifdef RADIO_OVERRIDE_DEFAULTS
{
int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS;
APM_RC.setHIL(rc_override);
}
#endif
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs
init_camera();