Line data    Source code

       1             : #include "Sub.h"
       2             : 
       3             : /*****************************************************************************
       4             : *   The init_ardupilot function processes everything we need for an in - air restart
       5             : *        We will determine later if we are actually on the ground and process a
       6             : *        ground start in that case.
       7             : *
       8             : *****************************************************************************/
       9             : 
      10     6750292 : static void failsafe_check_static()
      11             : {
      12     6750292 :     sub.mainloop_failsafe_check();
      13     6750292 : }
      14             : 
      15          83 : void Sub::init_ardupilot()
      16             : {
      17             :     // initialise notify system
      18          83 :     notify.init();
      19             : 
      20             :     // initialise battery monitor
      21          83 :     battery.init();
      22             : 
      23          83 :     barometer.init();
      24             : 
      25             : #if AP_FEATURE_BOARD_DETECT
      26             :     // Detection won't work until after BoardConfig.init()
      27             :     switch (AP_BoardConfig::get_board_type()) {
      28             :     case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
      29             :         AP_Param::set_default_by_name("BARO_EXT_BUS", 0);
      30             :         break;
      31             :     case AP_BoardConfig::PX4_BOARD_PIXHAWK:
      32             :         AP_Param::set_by_name("BARO_EXT_BUS", 1);
      33             :         break;
      34             :     default:
      35             :         AP_Param::set_default_by_name("BARO_EXT_BUS", 1);
      36             :         break;
      37             :     }
      38             : #elif CONFIG_HAL_BOARD != HAL_BOARD_LINUX
      39          83 :     AP_Param::set_default_by_name("BARO_EXT_BUS", 1);
      40             : #endif
      41             : 
      42             : #if AP_TEMPERATURE_SENSOR_ENABLED
      43             :     // In order to preserve Sub's previous AP_TemperatureSensor Behavior we set the Default I2C Bus Here
      44          83 :     AP_Param::set_default_by_name("TEMP1_BUS", barometer.external_bus());
      45             : #endif
      46             : 
      47             :     // setup telem slots with serial ports
      48          83 :     gcs().setup_uarts();
      49             : 
      50             :     // initialise rc channels including setting mode
      51          83 :     rc().init();
      52             : 
      53             : 
      54          83 :     init_rc_in();               // sets up rc channels from radio
      55          83 :     init_rc_out();              // sets up motors and output to escs
      56          83 :     init_joystick();            // joystick initialization
      57             : 
      58             : #if AP_RELAY_ENABLED
      59          83 :     relay.init();
      60             : #endif
      61             : 
      62             : #if OSD_ENABLED
      63          83 :     osd.init();
      64             : #endif
      65             : 
      66             :     /*
      67             :      *  setup the 'main loop is dead' check. Note that this relies on
      68             :      *  the RC library being initialised.
      69             :      */
      70          83 :     hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
      71             : 
      72             :     // Do GPS init
      73          83 :     gps.set_log_gps_bit(MASK_LOG_GPS);
      74          83 :     gps.init();
      75             : 
      76          83 :     AP::compass().set_log_bit(MASK_LOG_COMPASS);
      77          83 :     AP::compass().init();
      78             : 
      79             : #if AP_AIRSPEED_ENABLED
      80          83 :     airspeed.set_log_bit(MASK_LOG_IMU);
      81             : #endif
      82             : 
      83             : #if AP_OPTICALFLOW_ENABLED
      84             :     // initialise optical flow sensor
      85          83 :     optflow.init(MASK_LOG_OPTFLOW);
      86             : #endif
      87             : 
      88             : #if HAL_MOUNT_ENABLED
      89             :     // initialise camera mount
      90          83 :     camera_mount.init();
      91             :     // This step is necessary so that the servo is properly initialized
      92          83 :     camera_mount.set_angle_target(0, 0, 0, false);
      93             :     // for some reason the call to set_angle_targets changes the mode to mavlink targeting!
      94          83 :     camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING);
      95             : #endif
      96             : 
      97             : #if AP_CAMERA_ENABLED
      98             :     // initialise camera
      99          83 :     camera.init();
     100             : #endif
     101             : 
     102             : #ifdef USERHOOK_INIT
     103             :     USERHOOK_INIT
     104             : #endif
     105             : 
     106             :     // Init baro and determine if we have external (depth) pressure sensor
     107          83 :     barometer.set_log_baro_bit(MASK_LOG_IMU);
     108          83 :     barometer.calibrate(false);
     109          82 :     barometer.update();
     110             : 
     111          82 :     for (uint8_t i = 0; i < barometer.num_instances(); i++) {
     112          82 :         if (barometer.get_type(i) == AP_Baro::BARO_TYPE_WATER) {
     113          82 :             barometer.set_primary_baro(i);
     114          82 :             depth_sensor_idx = i;
     115          82 :             ap.depth_sensor_present = true;
     116          82 :             sensor_health.depth = barometer.healthy(depth_sensor_idx); // initialize health flag
     117          82 :             break; // Go with the first one we find
     118             :         }
     119             :     }
     120             : 
     121          82 :     if (!ap.depth_sensor_present) {
     122             :         // We only have onboard baro
     123             :         // No external underwater depth sensor detected
     124           0 :         barometer.set_primary_baro(0);
     125           0 :         ahrs.set_alt_measurement_noise(10.0f);  // Readings won't correspond with rest of INS
     126             :     } else {
     127          82 :         ahrs.set_alt_measurement_noise(0.1f);
     128             :     }
     129             : 
     130          82 :     leak_detector.init();
     131             : 
     132          82 :     last_pilot_heading_rad = ahrs.get_yaw_rad();
     133             : 
     134             :     // initialise rangefinder
     135             : #if AP_RANGEFINDER_ENABLED
     136          82 :     init_rangefinder();
     137             : #endif
     138             : 
     139             :     // initialise mission library
     140          82 :     mission.init();
     141             : #if HAL_LOGGING_ENABLED
     142          82 :     mission.set_log_start_mission_item_bit(MASK_LOG_CMD);
     143             : #endif
     144             : 
     145             :     // initialise AP_Logger library
     146             : #if HAL_LOGGING_ENABLED
     147          82 :     logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&sub, &Sub::Log_Write_Vehicle_Startup_Messages, void));
     148             : #endif
     149             : 
     150          82 :     startup_INS_ground();
     151             : 
     152             :     // enable CPU failsafe
     153          82 :     mainloop_failsafe_enable();
     154             : 
     155          82 :     ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
     156             : 
     157             :     // PARAMETER_CONVERSION - Added: Mar-2026
     158          82 :     if (g2.param_conversion_increment < 1) {
     159           9 :         update_actuators_from_jsbuttons();
     160           9 :         update_lights_from_rcin();
     161           9 :         g2.param_conversion_increment.set_and_save(1);
     162             :     }
     163             : 
     164          82 :     g2.actuators.initialize_actuators();
     165             : 
     166             : #if LEAKDETECTOR_MAX_INSTANCES > 0
     167          82 :     update_leak_pins();
     168             : #endif
     169             : #if AP_RELAY_ENABLED
     170          82 :     update_relay_pins();
     171             : #endif
     172             :     // flag that initialisation has completed
     173          82 :     ap.initialised = true;
     174          82 : }
     175             : 
     176             : 
     177             : //******************************************************************************
     178             : //This function does all the calibrations, etc. that we need during a ground start
     179             : //******************************************************************************
     180          82 : void Sub::startup_INS_ground()
     181             : {
     182             :     // initialise ahrs (may push imu calibration into the mpu6000 if using that device).
     183          82 :     ahrs.init();
     184          82 :     ahrs.set_vehicle_class(AP_AHRS::VehicleClass::SUBMARINE);
     185          82 :     ahrs.set_fly_forward(false);
     186             : 
     187             :     // Warm up and calibrate gyro offsets
     188          82 :     ins.init(scheduler.get_loop_rate_hz());
     189             : 
     190             :     // reset ahrs including gyro bias
     191          82 :     ahrs.reset();
     192          82 : }
     193             : 
     194             : // calibrate gyros - returns true if successfully calibrated
     195             : // position_ok - returns true if the horizontal absolute position is ok and home position is set
     196       85627 : bool Sub::position_ok()
     197             : {
     198             :     // return false if ekf failsafe has triggered
     199       85627 :     if (failsafe.ekf) {
     200           0 :         return false;
     201             :     }
     202             : 
     203             :     // check ekf position estimate
     204       85627 :     return (ekf_position_ok() || optflow_position_ok());
     205             : }
     206             : 
     207             : // ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set
     208       85627 : bool Sub::ekf_position_ok()
     209             : {
     210       85627 :     if (!ahrs.have_inertial_nav()) {
     211             :         // do not allow navigation with dcm position
     212         143 :         return false;
     213             :     }
     214             : 
     215             :     // if disarmed we accept a predicted horizontal position
     216       85484 :     if (!motors.armed()) {
     217        2293 :         if (ahrs.has_status(AP_AHRS::Status::HORIZ_POS_ABS)) {
     218         739 :             return true;
     219             :         }
     220        1554 :         if (ahrs.has_status(AP_AHRS::Status::PRED_HORIZ_POS_ABS)) {
     221           0 :             return true;
     222             :         }
     223        1554 :         return false;
     224             :     }
     225             : 
     226             :     // once armed we require a good absolute position and EKF must not be in const_pos_mode
     227       83191 :     if (ahrs.has_status(AP_AHRS::Status::CONST_POS_MODE)) {
     228        5662 :         return false;
     229             :     }
     230       77529 :     return ahrs.has_status(AP_AHRS::Status::HORIZ_POS_ABS);
     231             : }
     232             : 
     233             : // optflow_position_ok - returns true if optical flow based position estimate is ok
     234       21490 : bool Sub::optflow_position_ok()
     235             : {
     236             :     // return immediately if EKF not used
     237       21490 :     if (!ahrs.have_inertial_nav()) {
     238         143 :         return false;
     239             :     }
     240             : 
     241             :     // return immediately if neither optflow nor visual odometry is enabled
     242       21347 :     bool enabled = false;
     243             : #if AP_OPTICALFLOW_ENABLED
     244       21347 :     if (optflow.enabled()) {
     245           0 :         enabled = true;
     246             :     }
     247             : #endif
     248             : #if HAL_VISUALODOM_ENABLED
     249       21347 :     if (visual_odom.enabled()) {
     250       19785 :         enabled = true;
     251             :     }
     252             : #endif
     253       21347 :     if (!enabled) {
     254        1562 :         return false;
     255             :     }
     256             : 
     257             :     // if disarmed we accept a predicted horizontal relative position
     258       19785 :     if (!motors.armed()) {
     259          53 :         return ahrs.has_status(AP_AHRS::Status::PRED_HORIZ_POS_REL);
     260             :     }
     261             : 
     262       19732 :     if (ahrs.has_status(AP_AHRS::Status::CONST_POS_MODE)) {
     263        5601 :         return false;
     264             :     }
     265             : 
     266       14131 :     return ahrs.has_status(AP_AHRS::Status::HORIZ_POS_REL);
     267             : }
     268             : 
     269             : #if HAL_LOGGING_ENABLED
     270             : /*
     271             :   should we log a message type now?
     272             :  */
     273     3144745 : bool Sub::should_log(uint32_t mask)
     274             : {
     275     3144745 :     ap.logging_started = logger.logging_started();
     276     3144745 :     return logger.should_log(mask);
     277             : }
     278             : #endif
     279             : 
     280             : #include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
     281             : #include <AP_Avoidance/AP_Avoidance.h>
     282             : #include <AP_ADSB/AP_ADSB.h>
     283             : 
     284             : // dummy method to avoid linking AFS
     285             : #if AP_ADVANCEDFAILSAFE_ENABLED
     286           0 : bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) { return false; }
     287           3 : AP_AdvancedFailsafe *AP::advancedfailsafe() { return nullptr; }
     288             : #endif
     289             : 
     290             : #if AP_ADSB_AVOIDANCE_ENABLED
     291             : // dummy method to avoid linking AP_Avoidance
     292           0 : AP_Avoidance *AP::ap_avoidance() { return nullptr; }
     293             : #endif  // AP_ADSB_AVOIDANCE_ENABLED