AC fix attitude level hil

This commit is contained in:
Michael Oborne 2012-01-21 07:52:52 +08:00
parent b94c80059b
commit c2f3cf0cf5
1 changed files with 10 additions and 26 deletions

View File

@ -1520,6 +1520,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// set dcm hil sensor // set dcm hil sensor
dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
packet.pitchspeed,packet.yawspeed); packet.pitchspeed,packet.yawspeed);
// rad/sec
Vector3f gyros;
gyros.x = (float)packet.rollspeed;
gyros.y = (float)packet.pitchspeed;
gyros.z = (float)packet.yawspeed;
imu.set_gyro(gyros);
//imu.set_accel(accels);
break; break;
} }
#endif #endif
@ -1547,32 +1557,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
packet.v,packet.hdg,0,0); packet.v,packet.hdg,0,0);
break; break;
} }
// Is this resolved? - MAVLink protocol change.....
case MAVLINK_MSG_ID_VFR_HUD:
{
// decode
mavlink_vfr_hud_t packet;
mavlink_msg_vfr_hud_decode(msg, &packet);
// set airspeed
airspeed = 100*packet.airspeed;
break;
}
#endif
#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
*/ */
#if HIL_MODE == HIL_MODE_SENSORS #if HIL_MODE == HIL_MODE_SENSORS