mirror of https://github.com/ArduPilot/ardupilot
Plane: move data stream send up
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
a692acad81
commit
0fec702ba6
|
|
@ -554,8 +554,10 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
|
|||
break;
|
||||
|
||||
case MSG_PID_TUNING:
|
||||
CHECK_PAYLOAD_SIZE(PID_TUNING);
|
||||
plane.send_pid_tuning(chan);
|
||||
if (plane.control_mode != MANUAL) {
|
||||
CHECK_PAYLOAD_SIZE(PID_TUNING);
|
||||
plane.send_pid_tuning(chan);
|
||||
}
|
||||
break;
|
||||
|
||||
case MSG_RPM:
|
||||
|
|
@ -680,127 +682,89 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
|
|||
AP_GROUPEND
|
||||
};
|
||||
|
||||
void
|
||||
GCS_MAVLINK_Plane::data_stream_send(void)
|
||||
{
|
||||
gcs().set_out_of_time(false);
|
||||
|
||||
send_queued_parameters();
|
||||
|
||||
if (gcs().out_of_time()) return;
|
||||
|
||||
if (hal.scheduler->in_delay_callback()) {
|
||||
#if HIL_SUPPORT
|
||||
if (plane.g.hil_mode == 1) {
|
||||
// in HIL we need to keep sending servo values to ensure
|
||||
// the simulator doesn't pause, otherwise our sensor
|
||||
// calibration could stall
|
||||
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
|
||||
send_message(MSG_SERVO_OUT);
|
||||
}
|
||||
if (stream_trigger(STREAM_RC_CHANNELS)) {
|
||||
send_message(MSG_SERVO_OUTPUT_RAW);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
// don't send any other stream types while in the delay callback
|
||||
return;
|
||||
}
|
||||
|
||||
if (gcs().out_of_time()) return;
|
||||
|
||||
if (stream_trigger(STREAM_RAW_SENSORS)) {
|
||||
send_message(MSG_RAW_IMU1);
|
||||
send_message(MSG_RAW_IMU2);
|
||||
send_message(MSG_RAW_IMU3);
|
||||
}
|
||||
|
||||
if (gcs().out_of_time()) return;
|
||||
|
||||
if (stream_trigger(STREAM_EXTENDED_STATUS)) {
|
||||
send_message(MSG_EXTENDED_STATUS1);
|
||||
send_message(MSG_EXTENDED_STATUS2);
|
||||
send_message(MSG_CURRENT_WAYPOINT);
|
||||
send_message(MSG_GPS_RAW);
|
||||
send_message(MSG_GPS_RTK);
|
||||
send_message(MSG_GPS2_RAW);
|
||||
send_message(MSG_GPS2_RTK);
|
||||
send_message(MSG_NAV_CONTROLLER_OUTPUT);
|
||||
send_message(MSG_FENCE_STATUS);
|
||||
send_message(MSG_POSITION_TARGET_GLOBAL_INT);
|
||||
}
|
||||
|
||||
if (gcs().out_of_time()) return;
|
||||
|
||||
if (stream_trigger(STREAM_POSITION)) {
|
||||
// sent with GPS read
|
||||
send_message(MSG_LOCATION);
|
||||
send_message(MSG_LOCAL_POSITION);
|
||||
}
|
||||
|
||||
if (gcs().out_of_time()) return;
|
||||
|
||||
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
|
||||
send_message(MSG_SERVO_OUT);
|
||||
}
|
||||
|
||||
if (gcs().out_of_time()) return;
|
||||
|
||||
if (stream_trigger(STREAM_RC_CHANNELS)) {
|
||||
send_message(MSG_SERVO_OUTPUT_RAW);
|
||||
send_message(MSG_RADIO_IN);
|
||||
}
|
||||
|
||||
if (gcs().out_of_time()) return;
|
||||
|
||||
if (stream_trigger(STREAM_EXTRA1)) {
|
||||
send_message(MSG_ATTITUDE);
|
||||
send_message(MSG_SIMSTATE);
|
||||
send_message(MSG_RPM);
|
||||
send_message(MSG_AOA_SSA);
|
||||
|
||||
if (plane.control_mode != MANUAL) {
|
||||
send_message(MSG_PID_TUNING);
|
||||
}
|
||||
send_message(MSG_LANDING);
|
||||
}
|
||||
|
||||
if (gcs().out_of_time()) return;
|
||||
|
||||
if (stream_trigger(STREAM_EXTRA2)) {
|
||||
send_message(MSG_VFR_HUD);
|
||||
}
|
||||
|
||||
if (gcs().out_of_time()) return;
|
||||
|
||||
if (stream_trigger(STREAM_EXTRA3)) {
|
||||
send_message(MSG_AHRS);
|
||||
send_message(MSG_HWSTATUS);
|
||||
send_message(MSG_WIND);
|
||||
send_message(MSG_RANGEFINDER);
|
||||
send_message(MSG_SYSTEM_TIME);
|
||||
static const uint8_t STREAM_RAW_SENSORS_msgs[] = {
|
||||
MSG_RAW_IMU1, // RAW_IMU, SCALED_IMU2, SCALED_IMU3
|
||||
MSG_RAW_IMU2, // SCALED_PRESSURE, SCALED_PRESSURE2, SCALED_PRESSURE3
|
||||
MSG_RAW_IMU3 // SENSOR_OFFSETS
|
||||
};
|
||||
static const uint8_t STREAM_EXTENDED_STATUS_msgs[] = {
|
||||
MSG_EXTENDED_STATUS1, // SYS_STATUS, POWER_STATUS
|
||||
MSG_EXTENDED_STATUS2, // MEMINFO
|
||||
MSG_CURRENT_WAYPOINT,
|
||||
MSG_GPS_RAW,
|
||||
MSG_GPS_RTK,
|
||||
MSG_GPS2_RAW,
|
||||
MSG_GPS2_RTK,
|
||||
MSG_NAV_CONTROLLER_OUTPUT,
|
||||
MSG_FENCE_STATUS,
|
||||
MSG_POSITION_TARGET_GLOBAL_INT,
|
||||
};
|
||||
static const uint8_t STREAM_POSITION_msgs[] = {
|
||||
MSG_LOCATION,
|
||||
MSG_LOCAL_POSITION
|
||||
};
|
||||
static const uint8_t STREAM_RAW_CONTROLLER_msgs[] = {
|
||||
MSG_SERVO_OUT,
|
||||
};
|
||||
static const uint8_t STREAM_RC_CHANNELS_msgs[] = {
|
||||
MSG_SERVO_OUTPUT_RAW,
|
||||
MSG_RADIO_IN
|
||||
};
|
||||
static const uint8_t STREAM_EXTRA1_msgs[] = {
|
||||
MSG_ATTITUDE,
|
||||
MSG_SIMSTATE, // SIMSTATE, AHRS2
|
||||
MSG_RPM,
|
||||
MSG_AOA_SSA,
|
||||
MSG_PID_TUNING,
|
||||
MSG_LANDING
|
||||
};
|
||||
static const uint8_t STREAM_EXTRA2_msgs[] = {
|
||||
MSG_VFR_HUD
|
||||
};
|
||||
static const uint8_t STREAM_EXTRA3_msgs[] = {
|
||||
MSG_AHRS,
|
||||
MSG_HWSTATUS,
|
||||
MSG_WIND,
|
||||
MSG_RANGEFINDER,
|
||||
MSG_SYSTEM_TIME,
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
send_message(MSG_TERRAIN);
|
||||
MSG_TERRAIN,
|
||||
#endif
|
||||
send_message(MSG_MAG_CAL_REPORT);
|
||||
send_message(MSG_MAG_CAL_PROGRESS);
|
||||
send_message(MSG_BATTERY2);
|
||||
send_message(MSG_BATTERY_STATUS);
|
||||
send_message(MSG_MOUNT_STATUS);
|
||||
send_message(MSG_OPTICAL_FLOW);
|
||||
send_message(MSG_EKF_STATUS_REPORT);
|
||||
send_message(MSG_GIMBAL_REPORT);
|
||||
send_message(MSG_VIBRATION);
|
||||
}
|
||||
MSG_BATTERY2,
|
||||
MSG_BATTERY_STATUS,
|
||||
MSG_MOUNT_STATUS,
|
||||
MSG_OPTICAL_FLOW,
|
||||
MSG_GIMBAL_REPORT,
|
||||
MSG_MAG_CAL_REPORT,
|
||||
MSG_MAG_CAL_PROGRESS,
|
||||
MSG_EKF_STATUS_REPORT,
|
||||
MSG_VIBRATION,
|
||||
};
|
||||
static const uint8_t STREAM_ADSB_msgs[] = {
|
||||
MSG_ADSB_VEHICLE
|
||||
};
|
||||
|
||||
if (gcs().out_of_time()) return;
|
||||
const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
|
||||
MAV_STREAM_ENTRY(STREAM_RAW_SENSORS),
|
||||
MAV_STREAM_ENTRY(STREAM_EXTENDED_STATUS),
|
||||
MAV_STREAM_ENTRY(STREAM_POSITION),
|
||||
MAV_STREAM_ENTRY(STREAM_RAW_CONTROLLER),
|
||||
MAV_STREAM_ENTRY(STREAM_RC_CHANNELS),
|
||||
MAV_STREAM_ENTRY(STREAM_EXTRA1),
|
||||
MAV_STREAM_ENTRY(STREAM_EXTRA2),
|
||||
MAV_STREAM_ENTRY(STREAM_EXTRA3),
|
||||
MAV_STREAM_ENTRY(STREAM_ADSB),
|
||||
MAV_STREAM_TERMINATOR // must have this at end of stream_entries
|
||||
};
|
||||
|
||||
if (stream_trigger(STREAM_ADSB)) {
|
||||
send_message(MSG_ADSB_VEHICLE);
|
||||
}
|
||||
bool GCS_MAVLINK_Plane::in_hil_mode() const
|
||||
{
|
||||
#if HIL_SUPPORT
|
||||
return plane.g.hil_mode == 1;
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
handle a request to switch to guided mode. This happens via a
|
||||
callback from handle_mission_item()
|
||||
|
|
|
|||
|
|
@ -10,8 +10,6 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK
|
|||
|
||||
public:
|
||||
|
||||
void data_stream_send(void) override;
|
||||
|
||||
protected:
|
||||
|
||||
uint32_t telem_delay() const override;
|
||||
|
|
@ -35,6 +33,8 @@ protected:
|
|||
MAV_RESULT handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
|
||||
MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
|
||||
|
||||
virtual bool in_hil_mode() const override;
|
||||
|
||||
private:
|
||||
|
||||
void handleMessage(mavlink_message_t * msg) override;
|
||||
|
|
|
|||
Loading…
Reference in New Issue