diff --git a/APMrover2/GCS_Rover.cpp b/APMrover2/GCS_Rover.cpp index c6c5337f718e0..7b7a92bfa91bb 100644 --- a/APMrover2/GCS_Rover.cpp +++ b/APMrover2/GCS_Rover.cpp @@ -4,6 +4,11 @@ #include +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 index 8ac6481a46dc7..0ee3615af12e9 100644 --- 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 ¶ms, AP_HAL::UARTDriver &uart) override { return new GCS_MAVLINK_Rover(params, uart); diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 9ab6212981801..9a951aed446c5 100644 --- 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 index 3df6e10848d7c..6fb9ce0ac8750 100644 --- a/APMrover2/failsafe.cpp +++ b/APMrover2/failsafe.cpp @@ -8,8 +8,7 @@ #include /* - 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 index 21bec82ea344f..5cd07b6cdf846 100644 --- 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 index 55f93ff468585..4c93f533a4db2 100644 --- 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 index 64f80159d9453..e7494f7f0c5ec 100644 --- 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 ¶ms, AP_HAL::UARTDriver &uart) override { return new GCS_MAVLINK_Tracker(params, uart); diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 7cbf79bbf0ac4..9e650a749857e 100644 --- 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 index 75434dd052d6a..88fa1fc6471d0 100644 --- 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 index d394df616e626..85ce8336d213b 100644 --- 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 index fdbf16f86eaf8..709549e68478b 100644 --- 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 index b513b1d087c62..e2ec3149c826e 100644 --- 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 index 64e87c9f0d94f..33d6becddbbd5 100644 --- 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 index a102f216a724b..aa4620d67c9e3 100644 --- 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 index 184a121c2a156..def42612d79b9 100644 --- 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 index 4eab4350952a2..c40196c9ae565 100644 --- 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 index b2c7bfc5ea6bd..7a8248573b2f6 100644 --- 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 index aa390403ceafa..17991ea15f475 100644 --- 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 index 8bbbd33b82ab7..3fa4001fcf7c2 100644 --- 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 index eab2e0962a69c..10eac69b95b99 100644 --- 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 index c9627e2f6e26f..cb807c1ba56b3 100644 --- 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 index d88765421e31e..4aed6d3d72418 100644 --- 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 ¶ms, AP_HAL::UARTDriver &uart) = 0; diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index 057c3f6b79643..9ed189c245f2d 100644 --- 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 ¶ms, 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 index 51f3b499ca715..8997aa2410549 100644 --- 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(); }