Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

ACM: added SIMSTATE, DCM and HWSTATUS messages to ACM

Browse Source
This commit is contained in:
Andrew Tridgell 2012-03-01 23:36:35 +11:00
parent 55413bfcc1
commit b07a3eb966
2 changed files with 63 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
ArduCopter/GCS_Mavlink.pde
View File

@ -124,6 +124,42 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
crosstrack_error); // was 0
}
#if HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_dcm(mavlink_channel_t chan)
{
Vector3f omega_I = dcm.get_integrator();
mavlink_msg_dcm_send(
chan,
omega_I.x,
omega_I.y,
omega_I.z,
dcm.get_accel_weight(),
dcm.get_renorm_val(),
dcm.get_error_rp(),
dcm.get_error_yaw());
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
#ifdef DESKTOP_BUILD
// report simulator state
static void NOINLINE send_simstate(mavlink_channel_t chan)
{
extern void sitl_simstate_send(uint8_t chan);
sitl_simstate_send((uint8_t)chan);
}
#endif
#ifndef DESKTOP_BUILD
static void NOINLINE send_hwstatus(mavlink_channel_t chan)
{
mavlink_msg_hwstatus_send(
chan,
board_voltage(),
I2c.lockup_count());
}
#endif
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{
mavlink_msg_gps_raw_send(
@ -435,6 +471,27 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
send_statustext(chan);
break;
case MSG_DCM:
#if HIL_MODE != HIL_MODE_ATTITUDE
CHECK_PAYLOAD_SIZE(DCM);
send_dcm(chan);
#endif
break;
case MSG_SIMSTATE:
#ifdef DESKTOP_BUILD
CHECK_PAYLOAD_SIZE(DCM);
send_simstate(chan);
#endif
break;
case MSG_HWSTATUS:
#ifndef DESKTOP_BUILD
CHECK_PAYLOAD_SIZE(HWSTATUS);
send_hwstatus(chan);
#endif
break;
case MSG_RETRY_DEFERRED:
break; // just here to prevent a warning
}
@ -669,6 +726,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
send_message(MSG_ATTITUDE);
send_message(MSG_SIMSTATE);
//Serial.printf("mav6 %d\n", (int)streamRateExtra1.get());
}
@ -678,8 +736,8 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
}
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
// Available datastream
//Serial.printf("mav8 %d\n", (int)streamRateExtra3.get());
send_message(MSG_DCM);
send_message(MSG_HWSTATUS);
}
}
}

3
ArduCopter/defines.h
View File

@ -234,6 +234,9 @@ enum ap_message {
MSG_NEXT_WAYPOINT,
MSG_NEXT_PARAM,
MSG_STATUSTEXT,
MSG_DCM,
MSG_SIMSTATE,
MSG_HWSTATUS,
MSG_RETRY_DEFERRED // this must be last
};