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

Plane: move data stream send up

Browse Source
This commit is contained in:
Peter Barker 2017-08-21 15:53:45 +10:00 committed by Andrew Tridgell
parent a692acad81
commit 0fec702ba6
2 changed files with 83 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

198
ArduPlane/GCS_Mavlink.cpp
View File

@ -554,8 +554,10 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
break; break;
case MSG_PID_TUNING: case MSG_PID_TUNING:
CHECK_PAYLOAD_SIZE(PID_TUNING); if (plane.control_mode != MANUAL) {
plane.send_pid_tuning(chan); CHECK_PAYLOAD_SIZE(PID_TUNING);
plane.send_pid_tuning(chan);
}
break; break;
case MSG_RPM: case MSG_RPM:
@ -680,127 +682,89 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
AP_GROUPEND AP_GROUPEND
}; };
void static const uint8_t STREAM_RAW_SENSORS_msgs[] = {
GCS_MAVLINK_Plane::data_stream_send(void) MSG_RAW_IMU1, // RAW_IMU, SCALED_IMU2, SCALED_IMU3
{ MSG_RAW_IMU2, // SCALED_PRESSURE, SCALED_PRESSURE2, SCALED_PRESSURE3
gcs().set_out_of_time(false); MSG_RAW_IMU3 // SENSOR_OFFSETS
};
send_queued_parameters(); static const uint8_t STREAM_EXTENDED_STATUS_msgs[] = {
MSG_EXTENDED_STATUS1, // SYS_STATUS, POWER_STATUS
if (gcs().out_of_time()) return; MSG_EXTENDED_STATUS2, // MEMINFO
MSG_CURRENT_WAYPOINT,
if (hal.scheduler->in_delay_callback()) { MSG_GPS_RAW,
#if HIL_SUPPORT MSG_GPS_RTK,
if (plane.g.hil_mode == 1) { MSG_GPS2_RAW,
// in HIL we need to keep sending servo values to ensure MSG_GPS2_RTK,
// the simulator doesn't pause, otherwise our sensor MSG_NAV_CONTROLLER_OUTPUT,
// calibration could stall MSG_FENCE_STATUS,
if (stream_trigger(STREAM_RAW_CONTROLLER)) { MSG_POSITION_TARGET_GLOBAL_INT,
send_message(MSG_SERVO_OUT); };
} static const uint8_t STREAM_POSITION_msgs[] = {
if (stream_trigger(STREAM_RC_CHANNELS)) { MSG_LOCATION,
send_message(MSG_SERVO_OUTPUT_RAW); MSG_LOCAL_POSITION
} };
} static const uint8_t STREAM_RAW_CONTROLLER_msgs[] = {
#endif MSG_SERVO_OUT,
// don't send any other stream types while in the delay callback };
return; static const uint8_t STREAM_RC_CHANNELS_msgs[] = {
} MSG_SERVO_OUTPUT_RAW,
MSG_RADIO_IN
if (gcs().out_of_time()) return; };
static const uint8_t STREAM_EXTRA1_msgs[] = {
if (stream_trigger(STREAM_RAW_SENSORS)) { MSG_ATTITUDE,
send_message(MSG_RAW_IMU1); MSG_SIMSTATE, // SIMSTATE, AHRS2
send_message(MSG_RAW_IMU2); MSG_RPM,
send_message(MSG_RAW_IMU3); MSG_AOA_SSA,
} MSG_PID_TUNING,
MSG_LANDING
if (gcs().out_of_time()) return; };
static const uint8_t STREAM_EXTRA2_msgs[] = {
if (stream_trigger(STREAM_EXTENDED_STATUS)) { MSG_VFR_HUD
send_message(MSG_EXTENDED_STATUS1); };
send_message(MSG_EXTENDED_STATUS2); static const uint8_t STREAM_EXTRA3_msgs[] = {
send_message(MSG_CURRENT_WAYPOINT); MSG_AHRS,
send_message(MSG_GPS_RAW); MSG_HWSTATUS,
send_message(MSG_GPS_RTK); MSG_WIND,
send_message(MSG_GPS2_RAW); MSG_RANGEFINDER,
send_message(MSG_GPS2_RTK); MSG_SYSTEM_TIME,
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);
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
send_message(MSG_TERRAIN); MSG_TERRAIN,
#endif #endif
send_message(MSG_MAG_CAL_REPORT); MSG_BATTERY2,
send_message(MSG_MAG_CAL_PROGRESS); MSG_BATTERY_STATUS,
send_message(MSG_BATTERY2); MSG_MOUNT_STATUS,
send_message(MSG_BATTERY_STATUS); MSG_OPTICAL_FLOW,
send_message(MSG_MOUNT_STATUS); MSG_GIMBAL_REPORT,
send_message(MSG_OPTICAL_FLOW); MSG_MAG_CAL_REPORT,
send_message(MSG_EKF_STATUS_REPORT); MSG_MAG_CAL_PROGRESS,
send_message(MSG_GIMBAL_REPORT); MSG_EKF_STATUS_REPORT,
send_message(MSG_VIBRATION); 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)) { bool GCS_MAVLINK_Plane::in_hil_mode() const
send_message(MSG_ADSB_VEHICLE); {
} #if HIL_SUPPORT
return plane.g.hil_mode == 1;
#endif
return false;
} }
/* /*
handle a request to switch to guided mode. This happens via a handle a request to switch to guided mode. This happens via a
callback from handle_mission_item() callback from handle_mission_item()

4
ArduPlane/GCS_Mavlink.h
View File

@ -10,8 +10,6 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK
public: public:
void data_stream_send(void) override;
protected: protected:
uint32_t telem_delay() const override; 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;
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: private:
void handleMessage(mavlink_message_t * msg) override; void handleMessage(mavlink_message_t * msg) override;