ardupilot/ArduSub/system.cpp

292 lines
8.2 KiB
C++

#include "Sub.h"
/*****************************************************************************
* The init_ardupilot function processes everything we need for an in - air restart
* We will determine later if we are actually on the ground and process a
* ground start in that case.
*
*****************************************************************************/
static void failsafe_check_static()
{
sub.mainloop_failsafe_check();
}
void Sub::init_ardupilot()
{
BoardConfig.init();
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
can_mgr.init();
#endif
// init cargo gripper
#if AP_GRIPPER_ENABLED
g2.gripper.init();
#endif
// initialise notify system
notify.init();
// initialise battery monitor
battery.init();
barometer.init();
#if AP_FEATURE_BOARD_DETECT
// Detection won't work until after BoardConfig.init()
switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
AP_Param::set_default_by_name("BARO_EXT_BUS", 0);
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK:
AP_Param::set_by_name("BARO_EXT_BUS", 1);
break;
default:
AP_Param::set_default_by_name("BARO_EXT_BUS", 1);
break;
}
#elif CONFIG_HAL_BOARD != HAL_BOARD_LINUX
AP_Param::set_default_by_name("BARO_EXT_BUS", 1);
#endif
#if AP_TEMPERATURE_SENSOR_ENABLED
// In order to preserve Sub's previous AP_TemperatureSensor Behavior we set the Default I2C Bus Here
AP_Param::set_default_by_name("TEMP1_BUS", barometer.external_bus());
#endif
// setup telem slots with serial ports
gcs().setup_uarts();
#if LOGGING_ENABLED == ENABLED
log_init();
#endif
// initialise rc channels including setting mode
rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM);
rc().init();
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up motors and output to escs
init_joystick(); // joystick initialization
relay.init();
/*
* setup the 'main loop is dead' check. Note that this relies on
* the RC library being initialised.
*/
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
// Do GPS init
gps.set_log_gps_bit(MASK_LOG_GPS);
gps.init(serial_manager);
AP::compass().set_log_bit(MASK_LOG_COMPASS);
AP::compass().init();
#if AP_AIRSPEED_ENABLED
airspeed.set_log_bit(MASK_LOG_IMU);
#endif
#if AP_OPTICALFLOW_ENABLED
// initialise optical flow sensor
optflow.init(MASK_LOG_OPTFLOW);
#endif
#if HAL_MOUNT_ENABLED
// initialise camera mount
camera_mount.init();
// This step ncessary so the servo is properly initialized
camera_mount.set_angle_target(0, 0, 0, false);
// for some reason the call to set_angle_targets changes the mode to mavlink targeting!
camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING);
#endif
#if AP_CAMERA_ENABLED
// initialise camera
camera.init();
#endif
#ifdef USERHOOK_INIT
USERHOOK_INIT
#endif
// Init baro and determine if we have external (depth) pressure sensor
barometer.set_log_baro_bit(MASK_LOG_IMU);
barometer.calibrate(false);
barometer.update();
for (uint8_t i = 0; i < barometer.num_instances(); i++) {
if (barometer.get_type(i) == AP_Baro::BARO_TYPE_WATER) {
barometer.set_primary_baro(i);
depth_sensor_idx = i;
ap.depth_sensor_present = true;
sensor_health.depth = barometer.healthy(depth_sensor_idx); // initialize health flag
break; // Go with the first one we find
}
}
if (!ap.depth_sensor_present) {
// We only have onboard baro
// No external underwater depth sensor detected
barometer.set_primary_baro(0);
ahrs.set_alt_measurement_noise(10.0f); // Readings won't correspond with rest of INS
} else {
ahrs.set_alt_measurement_noise(0.1f);
}
leak_detector.init();
last_pilot_heading = ahrs.yaw_sensor;
// initialise rangefinder
#if RANGEFINDER_ENABLED == ENABLED
init_rangefinder();
#endif
// initialise AP_RPM library
#if AP_RPM_ENABLED
rpm_sensor.init();
#endif
// initialise mission library
mission.init();
// initialise AP_Logger library
#if LOGGING_ENABLED == ENABLED
logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&sub, &Sub::Log_Write_Vehicle_Startup_Messages, void));
#endif
startup_INS_ground();
#if AP_SCRIPTING_ENABLED
g2.scripting.init();
#endif // AP_SCRIPTING_ENABLED
// we don't want writes to the serial port to cause us to pause
// mid-flight, so set the serial ports non-blocking once we are
// ready to fly
serial_manager.set_blocking_writes_all(false);
// enable CPU failsafe
mainloop_failsafe_enable();
ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
// flag that initialisation has completed
ap.initialised = true;
}
//******************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//******************************************************************************
void Sub::startup_INS_ground()
{
// initialise ahrs (may push imu calibration into the mpu6000 if using that device).
ahrs.init();
ahrs.set_vehicle_class(AP_AHRS::VehicleClass::SUBMARINE);
// Warm up and calibrate gyro offsets
ins.init(scheduler.get_loop_rate_hz());
// reset ahrs including gyro bias
ahrs.reset();
}
// calibrate gyros - returns true if successfully calibrated
// position_ok - returns true if the horizontal absolute position is ok and home position is set
bool Sub::position_ok()
{
// return false if ekf failsafe has triggered
if (failsafe.ekf) {
return false;
}
// check ekf position estimate
return (ekf_position_ok() || optflow_position_ok());
}
// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set
bool Sub::ekf_position_ok()
{
if (!ahrs.have_inertial_nav()) {
// do not allow navigation with dcm position
return false;
}
// with EKF use filter status and ekf check
nav_filter_status filt_status = inertial_nav.get_filter_status();
// if disarmed we accept a predicted horizontal position
if (!motors.armed()) {
return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs));
}
// once armed we require a good absolute position and EKF must not be in const_pos_mode
return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode);
}
// optflow_position_ok - returns true if optical flow based position estimate is ok
bool Sub::optflow_position_ok()
{
// return immediately if EKF not used
if (!ahrs.have_inertial_nav()) {
return false;
}
// return immediately if neither optflow nor visual odometry is enabled
bool enabled = false;
#if AP_OPTICALFLOW_ENABLED
if (optflow.enabled()) {
enabled = true;
}
#endif
#if HAL_VISUALODOM_ENABLED
if (visual_odom.enabled()) {
enabled = true;
}
#endif
if (!enabled) {
return false;
}
// get filter status from EKF
nav_filter_status filt_status = inertial_nav.get_filter_status();
// if disarmed we accept a predicted horizontal relative position
if (!motors.armed()) {
return (filt_status.flags.pred_horiz_pos_rel);
}
return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode);
}
/*
should we log a message type now?
*/
bool Sub::should_log(uint32_t mask)
{
#if LOGGING_ENABLED == ENABLED
ap.logging_started = logger.logging_started();
return logger.should_log(mask);
#else
return false;
#endif
}
#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
#include <AP_Avoidance/AP_Avoidance.h>
#include <AP_ADSB/AP_ADSB.h>
// dummy method to avoid linking AFS
#if AP_ADVANCEDFAILSAFE_ENABLED
bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) { return false; }
AP_AdvancedFailsafe *AP::advancedfailsafe() { return nullptr; }
#endif
#if HAL_ADSB_ENABLED
// dummy method to avoid linking AP_Avoidance
AP_Avoidance *AP::ap_avoidance() { return nullptr; }
#endif