Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HIL: enable reception of GPS and ATTITUDE via MAVLink 

this allows us to get GPS position and ATTITUDE in HIL_MODE_ATTITUDE

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2893 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
tridge60@gmail.com 2011-07-17 10:33:42 +00:00
parent e6a46d888f
commit 1a35209e5d
1 changed files with 52 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
ArduCopterMega/GCS_Mavlink.pde
View File

@ -861,33 +861,64 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
} // end case } // end case
/*
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
{ {
// allow override of RC channel values for HIL // allow override of RC channel values for HIL
// or for complete GCS control of switch position // or for complete GCS control of switch position
// and RC PWM values. // and RC PWM values.
if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs
mavlink_rc_channels_override_t packet; mavlink_rc_channels_override_t packet;
int16_t v[8]; int16_t v[8];
mavlink_msg_rc_channels_override_decode(msg, &packet); mavlink_msg_rc_channels_override_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) if (mavlink_check_target(packet.target_system,packet.target_component))
break; break;
v[0] = packet.chan1_raw; v[0] = packet.chan1_raw;
v[1] = packet.chan2_raw; v[1] = packet.chan2_raw;
v[2] = packet.chan3_raw; v[2] = packet.chan3_raw;
v[3] = packet.chan4_raw; v[3] = packet.chan4_raw;
v[4] = packet.chan5_raw; v[4] = packet.chan5_raw;
v[5] = packet.chan6_raw; v[5] = packet.chan6_raw;
v[6] = packet.chan7_raw; v[6] = packet.chan7_raw;
v[7] = packet.chan8_raw; v[7] = packet.chan8_raw;
rc_override_active = APM_RC.setHIL(v); APM_RC.setHIL(v);
rc_override_fs_timer = millis(); break;
break; }
}
#if HIL_MODE != HIL_MODE_DISABLED
// This is used both as a sensor and to pass the location
// in HIL_ATTITUDE mode.
case MAVLINK_MSG_ID_GPS_RAW:
{
// decode
mavlink_gps_raw_t packet;
mavlink_msg_gps_raw_decode(msg, &packet);
// set gps hil sensor
g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt,
packet.v,packet.hdg,0,0);
current_loc.lng = packet.lon * T7;
current_loc.lat = packet.lat * T7;
current_loc.alt = packet.alt * 10;
break;
}
#if HIL_MODE == HIL_MODE_ATTITUDE
case MAVLINK_MSG_ID_ATTITUDE:
{
// decode
mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet);
// set dcm hil sensor
dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
packet.pitchspeed,packet.yawspeed);
break;
}
#endif
#endif
/*
case MAVLINK_MSG_ID_HEARTBEAT: case MAVLINK_MSG_ID_HEARTBEAT:
{ {
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes