GCS_MAVLink: add support for mavlink out on AP_Periph

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;

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)
 

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