From 3d03979b1601c820af66c9a707579efbfae96c28 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 11 Jul 2021 12:10:22 +0530 Subject: [PATCH] GCS_MAVLink: add support for mavlink out on AP_Periph --- libraries/GCS_MAVLink/GCS.cpp | 25 +++++++++++++++++++++++++ libraries/GCS_MAVLink/GCS.h | 7 +++++++ libraries/GCS_MAVLink/GCS_Common.cpp | 5 ++++- 3 files changed, 36 insertions(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 71ef48b456..808be939c8 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -138,6 +138,7 @@ void GCS::update_sensor_status_flags() control_sensors_enabled = 0; control_sensors_health = 0; +#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_AHRS) AP_AHRS &ahrs = AP::ahrs(); const AP_InertialSensor &ins = AP::ins(); @@ -150,7 +151,9 @@ void GCS::update_sensor_status_flags() } } } +#endif +#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_MAG) const Compass &compass = AP::compass(); if (AP::compass().enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; @@ -159,14 +162,18 @@ void GCS::update_sensor_status_flags() if (compass.enabled() && compass.healthy()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; } +#endif +#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BARO) const AP_Baro &barometer = AP::baro(); control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; if (barometer.all_healthy()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; } +#endif +#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_GPS) const AP_GPS &gps = AP::gps(); if (gps.status() > AP_GPS::NO_GPS) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; @@ -175,7 +182,9 @@ void GCS::update_sensor_status_flags() if (gps.is_healthy() && gps.status() >= min_status_for_gps_healthy()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; } +#endif +#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) const AP_BattMonitor &battery = AP::battery(); control_sensors_present |= MAV_SYS_STATUS_SENSOR_BATTERY; if (battery.num_instances() > 0) { @@ -184,7 +193,9 @@ void GCS::update_sensor_status_flags() if (battery.healthy() && !battery.has_failsafed()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_BATTERY; } +#endif +#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_AHRS) control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_GYRO; control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; if (!ins.calibrating()) { @@ -197,7 +208,9 @@ void GCS::update_sensor_status_flags() control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO; } } +#endif +#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_LOGGING_ENABLED) const AP_Logger &logger = AP::logger(); if (logger.logging_present() || gps.logging_present()) { // primary logging only (usually File) control_sensors_present |= MAV_SYS_STATUS_LOGGING; @@ -208,13 +221,16 @@ void GCS::update_sensor_status_flags() if (!logger.logging_failed() && !gps.logging_failed()) { control_sensors_health |= MAV_SYS_STATUS_LOGGING; } +#endif // set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED) +#if !defined(HAL_BUILD_AP_PERIPH) control_sensors_present |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS; if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS; } control_sensors_health |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (ahrs.get_ekf_type() == 10) { @@ -224,6 +240,7 @@ void GCS::update_sensor_status_flags() } #endif +#if !defined(HAL_BUILD_AP_PERIPH) const AC_Fence *fence = AP::fence(); if (fence != nullptr) { if (fence->sys_status_enabled()) { @@ -236,6 +253,7 @@ void GCS::update_sensor_status_flags() control_sensors_health |= MAV_SYS_STATUS_GEOFENCE; } } +#endif #if HAL_VISUALODOM_ENABLED const AP_VisualOdom *visual_odom = AP::visualodom(); @@ -250,6 +268,7 @@ void GCS::update_sensor_status_flags() // give GCS status of prearm checks. This is enabled if any arming checks are enabled. // it is healthy if armed or checks are passing +#if !defined(HAL_BUILD_AP_PERIPH) control_sensors_present |= MAV_SYS_STATUS_PREARM_CHECK; if (AP::arming().get_enabled_checks()) { control_sensors_enabled |= MAV_SYS_STATUS_PREARM_CHECK; @@ -257,12 +276,18 @@ void GCS::update_sensor_status_flags() control_sensors_health |= MAV_SYS_STATUS_PREARM_CHECK; } } +#endif update_vehicle_sensor_status_flags(); } bool GCS::out_of_time() const { +#if defined(HAL_BUILD_AP_PERIPH) + // we are never out of time for AP_Periph + // as we don't have concept of AP_Scheduler in AP_Periph + return false; +#endif // while we are in the delay callback we are never out of time: if (hal.scheduler->in_delay_callback()) { return false; diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 8dee40f65b..1160d15a9d 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -1095,7 +1095,14 @@ private: GCS &gcs(); // send text when we do have a GCS +#if !defined(HAL_BUILD_AP_PERIPH) #define GCS_SEND_TEXT(severity, format, args...) gcs().send_text(severity, format, ##args) +#else +extern "C" { +void can_printf(const char *fmt, ...); +} +#define GCS_SEND_TEXT(severity, format, args...) (void)severity; can_printf(format, ##args) +#endif #elif defined(HAL_BUILD_AP_PERIPH) && !defined(STM32F1) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 62c4d98175..93514ea05f 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1075,11 +1075,12 @@ int8_t GCS_MAVLINK::deferred_message_to_send_index(uint16_t now16_ms) void GCS_MAVLINK::update_send() { +#if !defined(HAL_BUILD_AP_PERIPH) || HAL_LOGGING_ENABLED if (!hal.scheduler->in_delay_callback()) { // AP_Logger will not send log data if we are armed. AP::logger().handle_log_send(); } - +#endif send_ftp_replies(); if (!deferred_messages_initialised) { @@ -2080,6 +2081,7 @@ void GCS::send_message(enum ap_message id) void GCS::update_send() { update_send_has_been_called = true; +#ifndef HAL_BUILD_AP_PERIPH if (!initialised_missionitemprotocol_objects) { initialised_missionitemprotocol_objects = true; // once-only initialisation of MissionItemProtocol objects: @@ -2105,6 +2107,7 @@ void GCS::update_send() if (_missionitemprotocol_fence != nullptr) { _missionitemprotocol_fence->update(); } +#endif // HAL_BUILD_AP_PERIPH // round-robin the GCS_MAVLINK backend that gets to go first so // one backend doesn't monopolise all of the time allowed for sending // messages