From 0fec702ba67f68419d52c5944659d971c6457571 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 21 Aug 2017 15:53:45 +1000 Subject: [PATCH] Plane: move data stream send up --- ArduPlane/GCS_Mavlink.cpp | 198 ++++++++++++++++---------------------- ArduPlane/GCS_Mavlink.h | 4 +- 2 files changed, 83 insertions(+), 119 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 66df990766..125270086e 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -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() diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index e4797c37ee..a2cd94d127 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -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;