ArduPilot · tridge · Feb 25, 2020 · Feb 12, 2020 · Feb 12, 2020 · Feb 12, 2020
diff --git a/APMrover2/GCS_Rover.cpp b/APMrover2/GCS_Rover.cpp
@@ -4,6 +4,11 @@
 
 #include <AP_RangeFinder/AP_RangeFinder_Backend.h>
 
+uint8_t GCS_Rover::sysid_this_mav() const
+{
+    return rover.g.sysid_this_mav;
+}
+
 bool GCS_Rover::simple_input_active() const
 {
     if (rover.control_mode != &rover.mode_simple) {

diff --git a/APMrover2/GCS_Rover.h b/APMrover2/GCS_Rover.h
@@ -38,6 +38,8 @@ class GCS_Rover : public GCS
 
 protected:
 
+    uint8_t sysid_this_mav() const override;
+
     GCS_MAVLINK_Rover *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
                                                AP_HAL::UARTDriver &uart) override {
         return new GCS_MAVLINK_Rover(params, uart);

diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
@@ -350,7 +350,7 @@ class Rover : public AP_Vehicle {
     void load_parameters(void) override;
 
     // radio.cpp
-    void set_control_channels(void);
+    void set_control_channels(void) override;
     void init_rc_in();
     void rudder_arm_disarm_check();
     void read_radio();

diff --git a/APMrover2/failsafe.cpp b/APMrover2/failsafe.cpp
@@ -8,8 +8,7 @@
 #include <stdio.h>
 
 /*
-  our failsafe strategy is to detect main loop lockup and switch to
-  passing inputs straight from the RC inputs to RC outputs.
+  our failsafe strategy is to detect main loop lockup and disarm.
  */
 
 /*

diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
@@ -19,16 +19,6 @@ void Rover::init_ardupilot()
     g2.stats.init();
 #endif
 
-    mavlink_system.sysid = g.sysid_this_mav;
-
-    // initialise serial ports
-    serial_manager.init();
-
-    // setup first port early to allow BoardConfig to report errors
-    gcs().setup_console();
-
-    register_scheduler_delay_callback();
-
     BoardConfig.init();
 #if HAL_WITH_UAVCAN
     BoardConfig_CAN.init();
@@ -96,7 +86,6 @@ void Rover::init_ardupilot()
 
     ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
 
-    set_control_channels();  // setup radio channels and outputs ranges
     init_rc_in();            // sets up rc channels deadzone
     g2.motors.init();        // init motors including setting servo out channels ranges
     SRV_Channels::enable_aux_servos();

diff --git a/AntennaTracker/GCS_Tracker.cpp b/AntennaTracker/GCS_Tracker.cpp
@@ -1,6 +1,11 @@
 #include "GCS_Tracker.h"
 #include "Tracker.h"
 
+uint8_t GCS_Tracker::sysid_this_mav() const
+{
+    return tracker.g.sysid_this_mav;
+}
+
 void GCS_Tracker::request_datastream_position(const uint8_t sysid, const uint8_t compid)
 {
     for (uint8_t i=0; i < num_gcs(); i++) {

diff --git a/AntennaTracker/GCS_Tracker.h b/AntennaTracker/GCS_Tracker.h
@@ -33,6 +33,8 @@ class GCS_Tracker : public GCS
 
 protected:
 
+    uint8_t sysid_this_mav() const override;
+
     GCS_MAVLINK_Tracker *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
                                                  AP_HAL::UARTDriver &uart) override {
         return new GCS_MAVLINK_Tracker(params, uart);

diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp
@@ -8,16 +8,6 @@ void Tracker::init_ardupilot()
     // initialise stats module
     stats.init();
 
-    mavlink_system.sysid = g.sysid_this_mav;
-
-    // initialise serial ports
-    serial_manager.init();
-
-    // setup first port early to allow BoardConfig to report errors
-    gcs().setup_console();
-
-    register_scheduler_delay_callback();
-
     BoardConfig.init();
 #if HAL_WITH_UAVCAN
     BoardConfig_CAN.init();

diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp
@@ -2,6 +2,11 @@
 
 #include "Copter.h"
 
+uint8_t GCS_Copter::sysid_this_mav() const
+{
+    return copter.g.sysid_this_mav;
+}
+
 const char* GCS_Copter::frame_string() const
 {
     return copter.get_frame_string();

diff --git a/ArduCopter/GCS_Copter.h b/ArduCopter/GCS_Copter.h
@@ -39,6 +39,8 @@ class GCS_Copter : public GCS
 
 protected:
 
+    uint8_t sysid_this_mav() const override;
+
     // minimum amount of time (in microseconds) that must remain in
     // the main scheduler loop before we are allowed to send any
     // mavlink messages.  We want to prioritise the main flight

diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp
@@ -21,17 +21,6 @@ void Copter::init_ardupilot()
     g2.stats.init();
 #endif
 
-    // identify ourselves correctly with the ground station
-    mavlink_system.sysid = g.sysid_this_mav;
-
-    // initialise serial ports
-    serial_manager.init();
-
-    // setup first port early to allow BoardConfig to report errors
-    gcs().setup_console();
-
-    register_scheduler_delay_callback();
-
     BoardConfig.init();
 #if HAL_WITH_UAVCAN
     BoardConfig_CAN.init();

diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp
@@ -1,6 +1,11 @@
 #include "GCS_Plane.h"
 #include "Plane.h"
 
+uint8_t GCS_Plane::sysid_this_mav() const
+{
+    return plane.g.sysid_this_mav;
+}
+
 void GCS_Plane::update_vehicle_sensor_status_flags(void)
 {
     // first what sensors/controllers we have

diff --git a/ArduPlane/GCS_Plane.h b/ArduPlane/GCS_Plane.h
@@ -27,6 +27,7 @@ class GCS_Plane : public GCS
 
 protected:
 
+    uint8_t sysid_this_mav() const override;
     void update_vehicle_sensor_status_flags(void) override;
     uint32_t custom_mode() const override;
     MAV_TYPE frame_type() const override;

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -869,7 +869,7 @@ class Plane : public AP_Vehicle {
     void update_fbwb_speed_height(void);
     void setup_turn_angle(void);
     bool reached_loiter_target(void);
-    void set_control_channels(void);
+    void set_control_channels(void) override;
     void init_rc_in();
     void init_rc_out_main();
     void init_rc_out_aux();

diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
@@ -31,16 +31,6 @@ void Plane::init_ardupilot()
 
     ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
 
-    set_control_channels();
-
-    mavlink_system.sysid = g.sysid_this_mav;
-
-    // initialise serial ports
-    serial_manager.init();
-    gcs().setup_console();
-
-    register_scheduler_delay_callback();
-
     // setup any board specific drivers
     BoardConfig.init();
 #if HAL_WITH_UAVCAN

diff --git a/ArduSub/GCS_Sub.cpp b/ArduSub/GCS_Sub.cpp
@@ -2,6 +2,11 @@
 
 #include "Sub.h"
 
+uint8_t GCS_Sub::sysid_this_mav() const
+{
+    return sub.g.sysid_this_mav;
+}
+
 void GCS_Sub::update_vehicle_sensor_status_flags()
 {
     control_sensors_present |=

diff --git a/ArduSub/GCS_Sub.h b/ArduSub/GCS_Sub.h
@@ -34,6 +34,8 @@ class GCS_Sub : public GCS
 
 protected:
 
+    uint8_t sysid_this_mav() const override;
+
     // minimum amount of time (in microseconds) that must remain in
     // the main scheduler loop before we are allowed to send any
     // mavlink messages.  We want to prioritise the main flight

diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp
@@ -36,15 +36,6 @@ void Sub::init_ardupilot()
     celsius.init(1);
 #endif
 
-    // identify ourselves correctly with the ground station
-    mavlink_system.sysid = g.sysid_this_mav;
-
-    // initialise serial port
-    serial_manager.init();
-
-    // setup first port early to allow BoardConfig to report errors
-    gcs().setup_console();
-
     // init cargo gripper
 #if GRIPPER_ENABLED == ENABLED
     g2.gripper.init();
@@ -62,8 +53,6 @@ void Sub::init_ardupilot()
 
     barometer.init();
 
-    register_scheduler_delay_callback();
-
     // setup telem slots with serial ports
     gcs().setup_uarts();
 

diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -53,6 +53,25 @@ void AP_Vehicle::setup()
     // actual loop rate
     G_Dt = scheduler.get_loop_period_s();
 
+    // this is here for Plane; its failsafe_check method requires the
+    // RC channels to be set as early as possible for maximum
+    // survivability.
+    set_control_channels();
+
+    // initialise serial manager as early as sensible to get
+    // diagnostic output during boot process.  We have to initialise
+    // the GCS singleton first as it sets the global mavlink system ID
+    // which may get used very early on.
+    gcs().init();
+
+    // initialise serial ports
+    serial_manager.init();
+    gcs().setup_console();
+
+    // Register scheduler_delay_cb, which will run anytime you have
+    // more than 5ms remaining in your call to hal.scheduler->delay
+    hal.scheduler->register_delay_callback(scheduler_delay_callback, 5);
+
     // init_ardupilot is where the vehicle does most of its initialisation.
     init_ardupilot();
 }
@@ -131,13 +150,6 @@ void AP_Vehicle::scheduler_delay_callback()
     logger.EnableWrites(true);
 }
 
-void AP_Vehicle::register_scheduler_delay_callback()
-{
-    // Register scheduler_delay_cb, which will run anytime you have
-    // more than 5ms remaining in your call to hal.scheduler->delay
-    hal.scheduler->register_delay_callback(scheduler_delay_callback, 5);
-}
-
 AP_Vehicle *AP_Vehicle::_singleton = nullptr;
 
 AP_Vehicle *AP_Vehicle::get_singleton()

diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h
@@ -168,6 +168,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
 
     virtual void init_ardupilot() = 0;
     virtual void load_parameters() = 0;
+    virtual void set_control_channels() {}
 
     // board specific config
     AP_BoardConfig BoardConfig;
@@ -223,8 +224,6 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
     static const struct AP_Param::GroupInfo var_info[];
     static const struct AP_Scheduler::Task scheduler_tasks[];
 
-    void register_scheduler_delay_callback();
-
 private:
 
     static AP_Vehicle *_singleton;

diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp
@@ -33,6 +33,11 @@ const MAV_MISSION_TYPE GCS_MAVLINK::supported_mission_types[] = {
     MAV_MISSION_TYPE_FENCE,
 };
 
+void GCS::init()
+{
+    mavlink_system.sysid = sysid_this_mav();
+}
+
 /*
   send a text message to all GCS
  */

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
@@ -870,6 +870,7 @@ class GCS
         return 200;
     }
 
+    void init();
     void setup_console();
     void setup_uarts();
 
@@ -908,6 +909,8 @@ class GCS
 
 protected:
 
+    virtual uint8_t sysid_this_mav() const = 0;
+
     virtual GCS_MAVLINK *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
                                                  AP_HAL::UARTDriver &uart) = 0;
 

diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h
@@ -64,6 +64,8 @@ class GCS_Dummy : public GCS
 
 protected:
 
+    uint8_t sysid_this_mav() const override { return 1; }
+
     GCS_MAVLINK_Dummy *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
                                                AP_HAL::UARTDriver &uart) override {
         return new GCS_MAVLINK_Dummy(params, uart);

diff --git a/libraries/GCS_MAVLink/examples/routing/routing.cpp b/libraries/GCS_MAVLink/examples/routing/routing.cpp
@@ -28,6 +28,7 @@ static MAVLink_routing routing;
 void setup(void)
 {
     hal.console->printf("routing test startup...");
+    gcs().init();
     gcs().setup_console();
 }