aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC: add hilstate message

Browse Source
This commit is contained in:
Michael Oborne 2012-06-04 16:24:08 +08:00
parent 83c1b7fe3b
commit 1423810247
1 changed files with 56 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
ArduCopter/GCS_Mavlink.pde
View File

@ -1846,7 +1846,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
APM_RC.setHIL(v); APM_RC.setHIL(v);
break; break;
} }
#if HIL_MODE != HIL_MODE_DISABLED && MAVLINK10 != ENABLED #if HIL_MODE != HIL_MODE_DISABLED && MAVLINK10 != ENABLED
// This is used both as a sensor and to pass the location // This is used both as a sensor and to pass the location
// in HIL_ATTITUDE mode. Note that MAVLINK10 uses HIL_STATE // in HIL_ATTITUDE mode. Note that MAVLINK10 uses HIL_STATE
@ -1871,6 +1871,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} }
break; break;
} }
#if HIL_MODE == HIL_MODE_ATTITUDE #if HIL_MODE == HIL_MODE_ATTITUDE
case MAVLINK_MSG_ID_ATTITUDE: //30 case MAVLINK_MSG_ID_ATTITUDE: //30
{ {
@ -1913,6 +1914,60 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} }
#endif // HIL_MODE_ATTITUDE #endif // HIL_MODE_ATTITUDE
#endif // HIL_MODE != HIL_MODE_DISABLED && MAVLINK10 != ENABLED #endif // HIL_MODE != HIL_MODE_DISABLED && MAVLINK10 != ENABLED
#if MAVLINK10 == ENABLED && HIL_MODE != HIL_MODE_DISABLED
case MAVLINK_MSG_ID_HIL_STATE:
{
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
float vel = sqrt((packet.vx * (float)packet.vx) + (packet.vy * (float)packet.vy));
float cog = wrap_360(ToDeg(atan2(packet.vx, packet.vy)) * 100);
// set gps hil sensor
g_gps->setHIL(packet.time_usec/1000.0,
packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3,
vel*1.0e-2, cog*1.0e-2, 0, 10);
if (gps_base_alt == 0) {
gps_base_alt = g_gps->altitude;
}
current_loc.lng = g_gps->longitude;
current_loc.lat = g_gps->latitude;
current_loc.alt = g_gps->altitude - gps_base_alt;
if (!home_is_set) {
init_home();
}
#if HIL_MODE == HIL_MODE_SENSORS
// rad/sec
Vector3f gyros;
gyros.x = (float)packet.xgyro / 1000.0;
gyros.y = (float)packet.ygyro / 1000.0;
gyros.z = (float)packet.zgyro / 1000.0;
// m/s/s
Vector3f accels;
accels.x = (float)packet.xacc / 1000.0;
accels.y = (float)packet.yacc / 1000.0;
accels.z = (float)packet.zacc / 1000.0;
imu.set_gyro(gyros);
imu.set_accel(accels);
#else
// set AHRS hil sensor
ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
packet.pitchspeed,packet.yawspeed);
#endif
break;
}
#endif // MAVLINK10 == ENABLED && HIL_MODE != HIL_MODE_DISABLED
/* /*
case MAVLINK_MSG_ID_HEARTBEAT: case MAVLINK_MSG_ID_HEARTBEAT:
{ {