diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 78da92bb96..7ac452e352 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -1,837 +1,879 @@ -#include "Copter.h" - -// performs pre-arm checks. expects to be called at 1hz. -void AP_Arming_Copter::update(void) -{ - // perform pre-arm checks & display failures every 30 seconds - static uint8_t pre_arm_display_counter = PREARM_DISPLAY_PERIOD/2; - pre_arm_display_counter++; - bool display_fail = false; - if (pre_arm_display_counter >= PREARM_DISPLAY_PERIOD) { - display_fail = true; - pre_arm_display_counter = 0; - } - - pre_arm_checks(display_fail); -} - -bool AP_Arming_Copter::pre_arm_checks(bool display_failure) -{ - const bool passed = run_pre_arm_checks(display_failure); - set_pre_arm_check(passed); - return passed; -} - -// perform pre-arm checks -// return true if the checks pass successfully -bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure) -{ - // exit immediately if already armed - if (copter.motors->armed()) { - return true; - } - - // check if motor interlock and Emergency Stop aux switches are used - // at the same time. This cannot be allowed. - if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) && - rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP)){ - check_failed(display_failure, "Interlock/E-Stop Conflict"); - return false; - } - - // check if motor interlock aux switch is in use - // if it is, switch needs to be in disabled position to arm - // otherwise exit immediately. This check to be repeated, - // as state can change at any time. - if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) { - check_failed(display_failure, "Motor Interlock Enabled"); - } - - // if pre arm checks are disabled run only the mandatory checks - if (checks_to_perform == 0) { - return mandatory_checks(display_failure); - } - - return fence_checks(display_failure) - & parameter_checks(display_failure) - & motor_checks(display_failure) - & pilot_throttle_checks(display_failure) - & oa_checks(display_failure) & - AP_Arming::pre_arm_checks(display_failure); -} - -bool AP_Arming_Copter::barometer_checks(bool display_failure) -{ - if (!AP_Arming::barometer_checks(display_failure)) { - return false; - } - - bool ret = true; - // check Baro - if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_BARO)) { - // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode. - // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height - // that may differ from the baro height due to baro drift. - nav_filter_status filt_status = copter.inertial_nav.get_filter_status(); - bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); - if (using_baro_ref) { - if (fabsf(copter.inertial_nav.get_altitude() - copter.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { - check_failed(ARMING_CHECK_BARO, display_failure, "Altitude disparity"); - ret = false; - } - } - } - return ret; -} - -bool AP_Arming_Copter::compass_checks(bool display_failure) -{ - bool ret = AP_Arming::compass_checks(display_failure); - - if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_COMPASS)) { - // check compass offsets have been set. AP_Arming only checks - // this if learning is off; Copter *always* checks. - if (!AP::compass().configured()) { - check_failed(ARMING_CHECK_COMPASS, display_failure, "Compass not calibrated"); - ret = false; - } - } - - return ret; -} - -bool AP_Arming_Copter::ins_checks(bool display_failure) -{ - bool ret = AP_Arming::ins_checks(display_failure); - - if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) { - - // get ekf attitude (if bad, it's usually the gyro biases) - if (!pre_arm_ekf_attitude_check()) { - check_failed(ARMING_CHECK_INS, display_failure, "EKF attitude is bad"); - ret = false; - } - } - - return ret; -} - -bool AP_Arming_Copter::board_voltage_checks(bool display_failure) -{ - if (!AP_Arming::board_voltage_checks(display_failure)) { - return false; - } - - // check battery voltage - if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) { - if (copter.battery.has_failsafed()) { - check_failed(ARMING_CHECK_VOLTAGE, display_failure, "Battery failsafe"); - return false; - } - - // call parent battery checks - if (!AP_Arming::battery_checks(display_failure)) { - return false; - } - } - - return true; -} - -bool AP_Arming_Copter::parameter_checks(bool display_failure) -{ - // check various parameter values - if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) { - - // ensure all rc channels have different functions - if (rc().duplicate_options_exist()) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Duplicate Aux Switch Options"); - return false; - } - - // failsafe parameter checks - if (copter.g.failsafe_throttle) { - // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 - if (copter.channel_throttle->get_radio_min() <= copter.g.failsafe_throttle_value+10 || copter.g.failsafe_throttle_value < 910) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check FS_THR_VALUE"); - return false; - } - } - - // lean angle parameter check - if (copter.aparm.angle_max < 1000 || copter.aparm.angle_max > 8000) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check ANGLE_MAX"); - return false; - } - - // acro balance parameter check -#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED - if ((copter.g.acro_balance_roll > copter.attitude_control->get_angle_roll_p().kP()) || (copter.g.acro_balance_pitch > copter.attitude_control->get_angle_pitch_p().kP())) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "ACRO_BAL_ROLL/PITCH"); - return false; - } -#endif - - // pilot-speed-up parameter check - if (copter.g.pilot_speed_up <= 0) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check PILOT_SPEED_UP"); - return false; - } - - #if FRAME_CONFIG == HELI_FRAME - if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD && - copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_DUAL && - copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Invalid Heli FRAME_CLASS"); - return false; - } - - // check helicopter parameters - if (!copter.motors->parameter_check(display_failure)) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Heli motors checks failed"); - return false; - } - - char fail_msg[50]; - // check input mangager parameters - if (!copter.input_manager.parameter_check(fail_msg, ARRAY_SIZE(fail_msg))) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "%s", fail_msg); - return false; - } - - // Inverted flight feature disabled for Heli Single and Dual frames - if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD && - rc().find_channel_for_option(RC_Channel::aux_func_t::INVERTED) != nullptr) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Inverted flight option not supported"); - return false; - } - // Ensure an Aux Channel is configured for motor interlock - if (rc().find_channel_for_option(RC_Channel::aux_func_t::MOTOR_INTERLOCK) == nullptr) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Motor Interlock not configured"); - return false; - } - - #else - if (copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_QUAD || - copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_DUAL || - copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Invalid MultiCopter FRAME_CLASS"); - return false; - } - #endif // HELI_FRAME - - // check for missing terrain data - if (!pre_arm_terrain_check(display_failure)) { - return false; - } - - // check adsb avoidance failsafe -#if ADSB_ENABLED == ENABLE - if (copter.failsafe.adsb) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "ADSB threat detected"); - return false; - } -#endif - - // ensure controllers are OK with us arming: - char failure_msg[50]; - if (!copter.pos_control->pre_arm_checks("PSC", failure_msg, ARRAY_SIZE(failure_msg))) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg); - return false; - } - if (!copter.attitude_control->pre_arm_checks("ATC", failure_msg, ARRAY_SIZE(failure_msg))) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg); - return false; - } - } - - return true; -} - -// check motor setup was successful -bool AP_Arming_Copter::motor_checks(bool display_failure) -{ - // check motors initialised correctly - if (!copter.motors->initialised_ok()) { - check_failed(display_failure, "check firmware or FRAME_CLASS"); - return false; - } - - // if this is a multicopter using ToshibaCAN ESCs ensure MOT_PMW_MIN = 1000, MOT_PWM_MAX = 2000 -#if HAL_WITH_UAVCAN && (FRAME_CONFIG != HELI_FRAME) - bool tcan_active = false; - const uint8_t num_drivers = AP::can().get_num_drivers(); - for (uint8_t i = 0; i < num_drivers; i++) { - if (AP::can().get_protocol_type(i) == AP_BoardConfig_CAN::Protocol_Type_ToshibaCAN) { - tcan_active = true; - } - } - if (tcan_active) { - if (copter.motors->get_pwm_output_min() != 1000) { - check_failed(display_failure, "TCAN ESCs require MOT_PWM_MIN=1000"); - return false; - } - if (copter.motors->get_pwm_output_max() != 2000) { - check_failed(display_failure, "TCAN ESCs require MOT_PWM_MAX=2000"); - return false; - } - } -#endif - - return true; -} - -bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure) -{ - // check throttle is above failsafe throttle - // this is near the bottom to allow other failures to be displayed before checking pilot throttle - if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) { - if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) { - #if FRAME_CONFIG == HELI_FRAME - const char *failmsg = "Collective below Failsafe"; - #else - const char *failmsg = "Throttle below Failsafe"; - #endif - check_failed(ARMING_CHECK_RC, display_failure, "%s", failmsg); - return false; - } - } - - return true; -} - -bool AP_Arming_Copter::oa_checks(bool display_failure) -{ -#if AC_OAPATHPLANNER_ENABLED == ENABLED - char failure_msg[50]; - if (copter.g2.oa.pre_arm_check(failure_msg, ARRAY_SIZE(failure_msg))) { - return true; - } - // display failure - if (strlen(failure_msg) == 0) { - check_failed(display_failure, "%s", "Check Object Avoidance"); - } else { - check_failed(display_failure, "%s", failure_msg); - } - return false; -#else - return true; -#endif -} - -bool AP_Arming_Copter::rc_calibration_checks(bool display_failure) -{ - const RC_Channel *channels[] = { - copter.channel_roll, - copter.channel_pitch, - copter.channel_throttle, - copter.channel_yaw - }; - - copter.ap.pre_arm_rc_check = rc_checks_copter_sub(display_failure, channels) - & AP_Arming::rc_calibration_checks(display_failure); - - return copter.ap.pre_arm_rc_check; -} - -// performs pre_arm gps related checks and returns true if passed -bool AP_Arming_Copter::gps_checks(bool display_failure) -{ - // run mandatory gps checks first - if (!mandatory_gps_checks(display_failure)) { - AP_Notify::flags.pre_arm_gps_check = false; - return false; - } - - // check if flight mode requires GPS - bool mode_requires_gps = copter.flightmode->requires_GPS(); - - // check if fence requires GPS - bool fence_requires_gps = false; - #if AC_FENCE == ENABLED - // if circular or polygon fence is enabled we need GPS - fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0; - #endif - - // return true if GPS is not required - if (!mode_requires_gps && !fence_requires_gps) { - AP_Notify::flags.pre_arm_gps_check = true; - return true; - } - - // return true immediately if gps check is disabled - if (!(checks_to_perform == ARMING_CHECK_ALL || checks_to_perform & ARMING_CHECK_GPS)) { - AP_Notify::flags.pre_arm_gps_check = true; - return true; - } - - // warn about hdop separately - to prevent user confusion with no gps lock - if (copter.gps.get_hdop() > copter.g.gps_hdop_good) { - check_failed(ARMING_CHECK_GPS, display_failure, "High GPS HDOP"); - AP_Notify::flags.pre_arm_gps_check = false; - return false; - } - - // call parent gps checks - if (!AP_Arming::gps_checks(display_failure)) { - AP_Notify::flags.pre_arm_gps_check = false; - return false; - } - - // if we got here all must be ok - AP_Notify::flags.pre_arm_gps_check = true; - return true; -} - -// check ekf attitude is acceptable -bool AP_Arming_Copter::pre_arm_ekf_attitude_check() -{ - // get ekf filter status - nav_filter_status filt_status = copter.inertial_nav.get_filter_status(); - - return filt_status.flags.attitude; -} - -// check we have required terrain data -bool AP_Arming_Copter::pre_arm_terrain_check(bool display_failure) -{ -#if AP_TERRAIN_AVAILABLE && AC_TERRAIN - // succeed if not using terrain data - if (!copter.terrain_use()) { - return true; - } - - // check if terrain following is enabled, using a range finder but RTL_ALT is higher than rangefinder's max range - // To-Do: modify RTL return path to fly at or above the RTL_ALT and remove this check - - if (copter.rangefinder_state.enabled && (copter.g.rtl_altitude > copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270))) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "RTL_ALT above rangefinder max range"); - return false; - } - - // show terrain statistics - uint16_t terr_pending, terr_loaded; - copter.terrain.get_statistics(terr_pending, terr_loaded); - bool have_all_data = (terr_pending <= 0); - if (!have_all_data) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Waiting for Terrain data"); - } - return have_all_data; -#else - return true; -#endif -} - -// check nothing is too close to vehicle -bool AP_Arming_Copter::proximity_checks(bool display_failure) const -{ -#if PROXIMITY_ENABLED == ENABLED - - if (!AP_Arming::proximity_checks(display_failure)) { - return false; - } - - if (!((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS))) { - // check is disabled - return true; - } - - // get closest object if we might use it for avoidance -#if AC_AVOID_ENABLED == ENABLED - float angle_deg, distance; - if (copter.avoid.proximity_avoidance_enabled() && copter.g2.proximity.get_closest_object(angle_deg, distance)) { - // display error if something is within 60cm - if (distance <= 0.6f) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Proximity %d deg, %4.2fm", (int)angle_deg, (double)distance); - return false; - } - } -#endif - -#endif - return true; -} - -// performs mandatory gps checks. returns true if passed -bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure) -{ - // always check if inertial nav has started and is ready - const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); - if (!ahrs.prearm_healthy()) { - const char *reason = ahrs.prearm_failure_reason(); - if (reason == nullptr) { - reason = "AHRS not healthy"; - } - check_failed(display_failure, "%s", reason); - return false; - } - - // check if flight mode requires GPS - bool mode_requires_gps = copter.flightmode->requires_GPS(); - - // check if fence requires GPS - bool fence_requires_gps = false; - #if AC_FENCE == ENABLED - // if circular or polygon fence is enabled we need GPS - fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0; - #endif - - // return true if GPS is not required - if (!mode_requires_gps && !fence_requires_gps) { - return true; - } - - // ensure GPS is ok - if (!copter.position_ok()) { - const char *reason = ahrs.prearm_failure_reason(); - if (reason == nullptr) { - if (!mode_requires_gps && fence_requires_gps) { - // clarify to user why they need GPS in non-GPS flight mode - reason = "Fence enabled, need 3D Fix"; - } else { - reason = "Need 3D Fix"; - } - } - check_failed(display_failure, "%s", reason); - return false; - } - - // check for GPS glitch (as reported by EKF) - nav_filter_status filt_status; - if (ahrs.get_filter_status(filt_status)) { - if (filt_status.flags.gps_glitching) { - check_failed(display_failure, "GPS glitching"); - return false; - } - } - - // check EKF compass variance is below failsafe threshold - float vel_variance, pos_variance, hgt_variance, tas_variance; - Vector3f mag_variance; - Vector2f offset; - ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset); - if (copter.g.fs_ekf_thresh > 0 && mag_variance.length() >= copter.g.fs_ekf_thresh) { - check_failed(display_failure, "EKF compass variance"); - return false; - } - - // check home and EKF origin are not too far - if (copter.far_from_EKF_origin(ahrs.get_home())) { - check_failed(display_failure, "EKF-home variance"); - return false; - } - - // if we got here all must be ok - return true; -} - - -// arm_checks - perform final checks before arming -// always called just before arming. Return true if ok to arm -// has side-effect that logging is started -bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) -{ - const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); - - // always check if inertial nav has started and is ready - if (!ahrs.healthy()) { - check_failed(true, "AHRS not healthy"); - return false; - } - -#ifndef ALLOW_ARM_NO_COMPASS - // if external source of heading is available, we can skip compass health check - if (!ahrs.is_ext_nav_used_for_yaw()) { - const Compass &_compass = AP::compass(); - // check compass health - if (!_compass.healthy()) { - check_failed(true, "Compass not healthy"); - return false; - } - } -#endif - - Mode::Number control_mode = copter.control_mode; - - // always check if the current mode allows arming - if (!copter.flightmode->allows_arming(method == AP_Arming::Method::MAVLINK)) { - check_failed(true, "Mode not armable"); - return false; - } - - // always check motors - if (!motor_checks(true)) { - return false; - } - - // if we are using motor interlock switch and it's enabled, fail to arm - // skip check in Throw mode which takes control of the motor interlock - if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) { - check_failed(true, "Motor Interlock Enabled"); - return false; - } - - // if we are not using Emergency Stop switch option, force Estop false to ensure motors - // can run normally - if (!rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP)){ - SRV_Channels::set_emergency_stop(false); - // if we are using motor Estop switch, it must not be in Estop position - } else if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP) && SRV_Channels::get_emergency_stop()){ - gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped"); - return false; - } - - // succeed if arming checks are disabled - if (checks_to_perform == 0) { - return true; - } - - // check lean angle - if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) { - if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > copter.aparm.angle_max) { - check_failed(ARMING_CHECK_INS, true, "Leaning"); - return false; - } - } - - // check adsb -#if ADSB_ENABLED == ENABLE - if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) { - if (copter.failsafe.adsb) { - check_failed(ARMING_CHECK_PARAMETERS, true, "ADSB threat detected"); - return false; - } - } -#endif - - // check throttle - if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) { - #if FRAME_CONFIG == HELI_FRAME - const char *rc_item = "Collective"; - #else - const char *rc_item = "Throttle"; - #endif - // check throttle is not too low - must be above failsafe throttle - if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) { - check_failed(ARMING_CHECK_RC, true, "%s below failsafe", rc_item); - return false; - } - - // check throttle is not too high - skips checks if arming from GCS in Guided - if (!(method == AP_Arming::Method::MAVLINK && (control_mode == Mode::Number::GUIDED || control_mode == Mode::Number::GUIDED_NOGPS))) { - // above top of deadband is too always high - if (copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()) > 0.0f) { - check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item); - return false; - } - // in manual modes throttle must be at zero - #if FRAME_CONFIG != HELI_FRAME - if ((copter.flightmode->has_manual_throttle() || control_mode == Mode::Number::DRIFT) && copter.channel_throttle->get_control_in() > 0) { - check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item); - return false; - } - #endif - } - } - - // check if safety switch has been pushed - if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { - check_failed(true, "Safety Switch"); - return false; - } - - // superclass method should always be the last thing called; it - // has side-effects which would need to be cleaned up if one of - // our arm checks failed - return AP_Arming::arm_checks(method); -} - -// mandatory checks that will be run if ARMING_CHECK is zero or arming forced -bool AP_Arming_Copter::mandatory_checks(bool display_failure) -{ - // call mandatory gps checks and update notify status because regular gps checks will not run - const bool result = mandatory_gps_checks(display_failure); - AP_Notify::flags.pre_arm_gps_check = result; - return result; -} - -void AP_Arming_Copter::set_pre_arm_check(bool b) -{ - copter.ap.pre_arm_check = b; - AP_Notify::flags.pre_arm_check = b; -} - -bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_checks) -{ - static bool in_arm_motors = false; - - // exit immediately if already in this function - if (in_arm_motors) { - return false; - } - in_arm_motors = true; - - // return true if already armed - if (copter.motors->armed()) { - in_arm_motors = false; - return true; - } - - if (!AP_Arming::arm(method, do_arming_checks)) { - AP_Notify::events.arming_failed = true; - in_arm_motors = false; - return false; - } - - // let logger know that we're armed (it may open logs e.g.) - AP::logger().set_vehicle_armed(true); - - // disable cpu failsafe because initialising everything takes a while - copter.failsafe_disable(); - - // notify that arming will occur (we do this early to give plenty of warning) - AP_Notify::flags.armed = true; - // call notify update a few times to ensure the message gets out - for (uint8_t i=0; i<=10; i++) { - AP::notify().update(); - } - -#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "Arming motors"); -#endif - - // Remember Orientation - // -------------------- - copter.init_simple_bearing(); - - AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); - - copter.initial_armed_bearing = ahrs.yaw_sensor; - - if (!ahrs.home_is_set()) { - // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) - ahrs.resetHeightDatum(); - AP::logger().Write_Event(DATA_EKF_ALT_RESET); - - // we have reset height, so arming height is zero - copter.arming_altitude_m = 0; - } else if (!ahrs.home_is_locked()) { - // Reset home position if it has already been set before (but not locked) - if (!copter.set_home_to_current_location(false)) { - // ignore failure - } - - // remember the height when we armed - copter.arming_altitude_m = copter.inertial_nav.get_altitude() * 0.01; - } - copter.update_super_simple_bearing(false); - - // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point -#if MODE_SMARTRTL_ENABLED == ENABLED - copter.g2.smart_rtl.set_home(copter.position_ok()); -#endif - - // enable gps velocity based centrefugal force compensation - ahrs.set_correct_centrifugal(true); - hal.util->set_soft_armed(true); - -#if SPRAYER_ENABLED == ENABLED - // turn off sprayer's test if on - copter.sprayer.test_pump(false); -#endif - - // enable output to motors - copter.enable_motor_output(); - - // finally actually arm the motors - copter.motors->armed(true); - - AP::logger().Write_Event(DATA_ARMED); - - // log flight mode in case it was changed while vehicle was disarmed - AP::logger().Write_Mode((uint8_t)copter.control_mode, copter.control_mode_reason); - - // re-enable failsafe - copter.failsafe_enable(); - - // perf monitor ignores delay due to arming - AP::scheduler().perf_info.ignore_this_loop(); - - // flag exiting this function - in_arm_motors = false; - - // Log time stamp of arming event - copter.arm_time_ms = millis(); - - // Start the arming delay - copter.ap.in_arming_delay = true; - - // assumed armed without a arming, switch. Overridden in switches.cpp - copter.ap.armed_with_switch = false; - - // return success - return true; -} - -// arming.disarm - disarm motors -bool AP_Arming_Copter::disarm() -{ - // return immediately if we are already disarmed - if (!copter.motors->armed()) { - return true; - } - - if (!AP_Arming::disarm()) { - return false; - } - -#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors"); -#endif - - AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); - - // save compass offsets learned by the EKF if enabled - Compass &compass = AP::compass(); - if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) { - for(uint8_t i=0; iarmed(false); - -#if MODE_AUTO_ENABLED == ENABLED - // reset the mission - copter.mode_auto.mission.reset(); -#endif - - AP::logger().set_vehicle_armed(false); - - // disable gps velocity based centrefugal force compensation - ahrs.set_correct_centrifugal(false); - hal.util->set_soft_armed(false); - - copter.ap.in_arming_delay = false; - - return true; -} +#include "Copter.h" + +// performs pre-arm checks. expects to be called at 1hz. +void AP_Arming_Copter::update(void) +{ + // perform pre-arm checks & display failures every 30 seconds + static uint8_t pre_arm_display_counter = PREARM_DISPLAY_PERIOD/2; + pre_arm_display_counter++; + bool display_fail = false; + if (pre_arm_display_counter >= PREARM_DISPLAY_PERIOD) { + display_fail = true; + pre_arm_display_counter = 0; + } + + pre_arm_checks(display_fail); +} + +bool AP_Arming_Copter::pre_arm_checks(bool display_failure) +{ + const bool passed = run_pre_arm_checks(display_failure); + set_pre_arm_check(passed); + return passed; +} + +// perform pre-arm checks +// return true if the checks pass successfully +bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure) +{ + // exit immediately if already armed + if (copter.motors->armed()) { + return true; + } + + // check if motor interlock and Emergency Stop aux switches are used + // at the same time. This cannot be allowed. + if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) && + rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP)){ + check_failed(display_failure, "Interlock/E-Stop Conflict"); + return false; + } + + // check if motor interlock aux switch is in use + // if it is, switch needs to be in disabled position to arm + // otherwise exit immediately. This check to be repeated, + // as state can change at any time. + if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) { + check_failed(display_failure, "Motor Interlock Enabled"); + } + + // if pre arm checks are disabled run only the mandatory checks + if (checks_to_perform == 0) { + return mandatory_checks(display_failure); + } + + + return fence_checks(display_failure) + & parameter_checks(display_failure) + & motor_checks(display_failure) + & pilot_throttle_checks(display_failure) + & oa_checks(display_failure) & + AP_Arming::pre_arm_checks(display_failure); +} + +bool AP_Arming_Copter::barometer_checks(bool display_failure) +{ + if (!AP_Arming::barometer_checks(display_failure)) { + return false; + } + + bool ret = true; + // check Baro + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_BARO)) { + // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode. + // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height + // that may differ from the baro height due to baro drift. + nav_filter_status filt_status = copter.inertial_nav.get_filter_status(); + bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); + if (using_baro_ref) { + if (fabsf(copter.inertial_nav.get_altitude() - copter.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { + check_failed(ARMING_CHECK_BARO, display_failure, "Altitude disparity"); + ret = false; + } + } + } + return ret; +} + +bool AP_Arming_Copter::compass_checks(bool display_failure) +{ + bool ret = AP_Arming::compass_checks(display_failure); + + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_COMPASS)) { + // check compass offsets have been set. AP_Arming only checks + // this if learning is off; Copter *always* checks. + if (!AP::compass().configured()) { + check_failed(ARMING_CHECK_COMPASS, display_failure, "Compass not calibrated"); + ret = false; + } + } + + return ret; +} + +bool AP_Arming_Copter::ins_checks(bool display_failure) +{ + bool ret = AP_Arming::ins_checks(display_failure); + + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) { + + // get ekf attitude (if bad, it's usually the gyro biases) + if (!pre_arm_ekf_attitude_check()) { + check_failed(ARMING_CHECK_INS, display_failure, "EKF attitude is bad"); + ret = false; + } + } + + return ret; +} + +bool AP_Arming_Copter::board_voltage_checks(bool display_failure) +{ + if (!AP_Arming::board_voltage_checks(display_failure)) { + return false; + } + + // check battery voltage + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) { + if (copter.battery.has_failsafed()) { + check_failed(ARMING_CHECK_VOLTAGE, display_failure, "Battery failsafe"); + return false; + } + + // call parent battery checks + if (!AP_Arming::battery_checks(display_failure)) { + return false; + } + } + + return true; +} + +bool AP_Arming_Copter::parameter_checks(bool display_failure) +{ + // check various parameter values + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) { + + // ensure all rc channels have different functions + if (rc().duplicate_options_exist()) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Duplicate Aux Switch Options"); + return false; + } + + // failsafe parameter checks + if (copter.g.failsafe_throttle) { + // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 + if (copter.channel_throttle->get_radio_min() <= copter.g.failsafe_throttle_value+10 || copter.g.failsafe_throttle_value < 910) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check FS_THR_VALUE"); + return false; + } + } + + // lean angle parameter check + if (copter.aparm.angle_max < 1000 || copter.aparm.angle_max > 8000) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check ANGLE_MAX"); + return false; + } + + // acro balance parameter check +#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED + if ((copter.g.acro_balance_roll > copter.attitude_control->get_angle_roll_p().kP()) || (copter.g.acro_balance_pitch > copter.attitude_control->get_angle_pitch_p().kP())) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "ACRO_BAL_ROLL/PITCH"); + return false; + } +#endif + + // pilot-speed-up parameter check + if (copter.g.pilot_speed_up <= 0) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check PILOT_SPEED_UP"); + return false; + } + + #if FRAME_CONFIG == HELI_FRAME + if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD && + copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_DUAL && + copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Invalid Heli FRAME_CLASS"); + return false; + } + + // check helicopter parameters + if (!copter.motors->parameter_check(display_failure)) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Heli motors checks failed"); + return false; + } + + char fail_msg[50]; + // check input mangager parameters + if (!copter.input_manager.parameter_check(fail_msg, ARRAY_SIZE(fail_msg))) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "%s", fail_msg); + return false; + } + + // Inverted flight feature disabled for Heli Single and Dual frames + if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD && + rc().find_channel_for_option(RC_Channel::aux_func_t::INVERTED) != nullptr) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Inverted flight option not supported"); + return false; + } + // Ensure an Aux Channel is configured for motor interlock + if (rc().find_channel_for_option(RC_Channel::aux_func_t::MOTOR_INTERLOCK) == nullptr) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Motor Interlock not configured"); + return false; + } + + #else + if (copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_QUAD || + copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_DUAL || + copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Invalid MultiCopter FRAME_CLASS"); + return false; + } + #endif // HELI_FRAME + + // check for missing terrain data + if (!pre_arm_terrain_check(display_failure)) { + return false; + } + + // check adsb avoidance failsafe +#if ADSB_ENABLED == ENABLE + if (copter.failsafe.adsb) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "ADSB threat detected"); + return false; + } +#endif + + // ensure controllers are OK with us arming: + char failure_msg[50]; + if (!copter.pos_control->pre_arm_checks("PSC", failure_msg, ARRAY_SIZE(failure_msg))) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg); + return false; + } + if (!copter.attitude_control->pre_arm_checks("ATC", failure_msg, ARRAY_SIZE(failure_msg))) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg); + return false; + } + } + + return true; +} + +// check motor setup was successful +bool AP_Arming_Copter::motor_checks(bool display_failure) +{ + // check motors initialised correctly + if (!copter.motors->initialised_ok()) { + check_failed(display_failure, "check firmware or FRAME_CLASS"); + return false; + } + + // if this is a multicopter using ToshibaCAN ESCs ensure MOT_PMW_MIN = 1000, MOT_PWM_MAX = 2000 +#if HAL_WITH_UAVCAN && (FRAME_CONFIG != HELI_FRAME) + bool tcan_active = false; + const uint8_t num_drivers = AP::can().get_num_drivers(); + for (uint8_t i = 0; i < num_drivers; i++) { + if (AP::can().get_protocol_type(i) == AP_BoardConfig_CAN::Protocol_Type_ToshibaCAN) { + tcan_active = true; + } + } + if (tcan_active) { + if (copter.motors->get_pwm_output_min() != 1000) { + check_failed(display_failure, "TCAN ESCs require MOT_PWM_MIN=1000"); + return false; + } + if (copter.motors->get_pwm_output_max() != 2000) { + check_failed(display_failure, "TCAN ESCs require MOT_PWM_MAX=2000"); + return false; + } + } +#endif + + return true; +} + +bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure) +{ + // check throttle is above failsafe throttle + // this is near the bottom to allow other failures to be displayed before checking pilot throttle + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) { + if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) { + #if FRAME_CONFIG == HELI_FRAME + const char *failmsg = "Collective below Failsafe"; + #else + const char *failmsg = "Throttle below Failsafe"; + #endif + check_failed(ARMING_CHECK_RC, display_failure, "%s", failmsg); + return false; + } + } + + return true; +} + +bool AP_Arming_Copter::oa_checks(bool display_failure) +{ +#if AC_OAPATHPLANNER_ENABLED == ENABLED + char failure_msg[50]; + if (copter.g2.oa.pre_arm_check(failure_msg, ARRAY_SIZE(failure_msg))) { + return true; + } + // display failure + if (strlen(failure_msg) == 0) { + check_failed(display_failure, "%s", "Check Object Avoidance"); + } else { + check_failed(display_failure, "%s", failure_msg); + } + return false; +#else + return true; +#endif +} + +bool AP_Arming_Copter::rc_calibration_checks(bool display_failure) +{ + const RC_Channel *channels[] = { + copter.channel_roll, + copter.channel_pitch, + copter.channel_throttle, + copter.channel_yaw + }; + + copter.ap.pre_arm_rc_check = rc_checks_copter_sub(display_failure, channels) + & AP_Arming::rc_calibration_checks(display_failure); + + return copter.ap.pre_arm_rc_check; +} + +// performs pre_arm gps related checks and returns true if passed +bool AP_Arming_Copter::gps_checks(bool display_failure) +{ + // run mandatory gps checks first + if (!mandatory_gps_checks(display_failure)) { + AP_Notify::flags.pre_arm_gps_check = false; + return false; + } + + // check if flight mode requires GPS + bool mode_requires_gps = copter.flightmode->requires_GPS(); + + // check if fence requires GPS + bool fence_requires_gps = false; + #if AC_FENCE == ENABLED + // if circular or polygon fence is enabled we need GPS + fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0; + #endif + + // return true if GPS is not required + if (!mode_requires_gps && !fence_requires_gps) { + AP_Notify::flags.pre_arm_gps_check = true; + return true; + } + + // return true immediately if gps check is disabled + if (!(checks_to_perform == ARMING_CHECK_ALL || checks_to_perform & ARMING_CHECK_GPS)) { + AP_Notify::flags.pre_arm_gps_check = true; + return true; + } + + // warn about hdop separately - to prevent user confusion with no gps lock + if (copter.gps.get_hdop() > copter.g.gps_hdop_good) { + check_failed(ARMING_CHECK_GPS, display_failure, "High GPS HDOP"); + AP_Notify::flags.pre_arm_gps_check = false; + return false; + } + + // call parent gps checks + if (!AP_Arming::gps_checks(display_failure)) { + AP_Notify::flags.pre_arm_gps_check = false; + return false; + } + + // if we got here all must be ok + AP_Notify::flags.pre_arm_gps_check = true; + return true; +} + +// check ekf attitude is acceptable +bool AP_Arming_Copter::pre_arm_ekf_attitude_check() +{ + // get ekf filter status + nav_filter_status filt_status = copter.inertial_nav.get_filter_status(); + + return filt_status.flags.attitude; +} + +// check we have required terrain data +bool AP_Arming_Copter::pre_arm_terrain_check(bool display_failure) +{ +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + // succeed if not using terrain data + if (!copter.terrain_use()) { + return true; + } + + // check if terrain following is enabled, using a range finder but RTL_ALT is higher than rangefinder's max range + // To-Do: modify RTL return path to fly at or above the RTL_ALT and remove this check + + if (copter.rangefinder_state.enabled && (copter.g.rtl_altitude > copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270))) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "RTL_ALT above rangefinder max range"); + return false; + } + + // show terrain statistics + uint16_t terr_pending, terr_loaded; + copter.terrain.get_statistics(terr_pending, terr_loaded); + bool have_all_data = (terr_pending <= 0); + if (!have_all_data) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Waiting for Terrain data"); + } + return have_all_data; +#else + return true; +#endif +} + +// check nothing is too close to vehicle +bool AP_Arming_Copter::proximity_checks(bool display_failure) const +{ +#if PROXIMITY_ENABLED == ENABLED + + if (!AP_Arming::proximity_checks(display_failure)) { + return false; + } + + if (!((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS))) { + // check is disabled + return true; + } + + // get closest object if we might use it for avoidance +#if AC_AVOID_ENABLED == ENABLED + float angle_deg, distance; + if (copter.avoid.proximity_avoidance_enabled() && copter.g2.proximity.get_closest_object(angle_deg, distance)) { + // display error if something is within 60cm + if (distance <= 0.6f) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Proximity %d deg, %4.2fm", (int)angle_deg, (double)distance); + return false; + } + } +#endif + +#endif + return true; +} + +bool AP_Arming_Copter::mandatory_deadline_checks(bool display_failure) +{ + const AP_GPS &gps = AP::gps(); + if(gps.status()<2) + return true; + if (!copter.deadline_ok()) + { + //const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); + // const char *reason = ahrs.prearm_failure_reason(); + int32_t deadline = 0; + copter.get_deadline_params(deadline); + // sprintf(temp,"time is up at %d-%d-%d ", deadline / 10000, (deadline % 10000) / 100, deadline % 100); + + // reason = "Trial time has expired!"; + // gcs().send_text(MAV_SEVERITY_INFO, "无法解锁，您的无人机授权已于%d-%d-%d到期", deadline / 10000, (deadline % 10000) / 100, deadline % 100); + check_failed(display_failure, "Trial time has expired at %d-%d-%d !", deadline / 10000, (deadline % 10000) / 100, deadline % 100); + return false; + }else{ + return true; + } +} + +// performs mandatory gps checks. returns true if passed +bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure) +{ + // always check if inertial nav has started and is ready + const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); + if (!ahrs.prearm_healthy()) { + const char *reason = ahrs.prearm_failure_reason(); + if (reason == nullptr) { + reason = "AHRS not healthy"; + } + check_failed(display_failure, "%s", reason); + return false; + } + + // check if flight mode requires GPS + bool mode_requires_gps = copter.flightmode->requires_GPS(); + + // check if fence requires GPS + bool fence_requires_gps = false; + #if AC_FENCE == ENABLED + // if circular or polygon fence is enabled we need GPS + fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0; + #endif + + // return true if GPS is not required + if (!mode_requires_gps && !fence_requires_gps) + { + return mandatory_deadline_checks(display_failure); + // return true; + } + + // ensure GPS is ok + if (!copter.position_ok()) { + const char *reason = ahrs.prearm_failure_reason(); + if (reason == nullptr) { + if (!mode_requires_gps && fence_requires_gps) { + // clarify to user why they need GPS in non-GPS flight mode + reason = "Fence enabled, need 3D Fix"; + } else { + reason = "Need 3D Fix"; + } + } + check_failed(display_failure, "%s", reason); + return false; + } + else + { + return mandatory_deadline_checks(display_failure); + // if (!copter.deadline_ok()) + // { + // const char *reason = ahrs.prearm_failure_reason(); + // //char * temp =nullptr; + // int32_t deadline = 0; + // copter.get_deadline_params(deadline); + // // sprintf(temp,"time is up at %d-%d-%d ", deadline / 10000, (deadline % 10000) / 100, deadline % 100); + + // reason ="Trial time has expired!"; + // // gcs().send_text(MAV_SEVERITY_INFO, "无法解锁，您的无人机授权已于%d-%d-%d到期", deadline / 10000, (deadline % 10000) / 100, deadline % 100); + // check_failed(display_failure, "%s", reason); + // return false; + // } + } + + // check for GPS glitch (as reported by EKF) + nav_filter_status filt_status; + if (ahrs.get_filter_status(filt_status)) { + if (filt_status.flags.gps_glitching) { + check_failed(display_failure, "GPS glitching"); + return false; + } + } + + // check EKF compass variance is below failsafe threshold + float vel_variance, pos_variance, hgt_variance, tas_variance; + Vector3f mag_variance; + Vector2f offset; + ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset); + if (copter.g.fs_ekf_thresh > 0 && mag_variance.length() >= copter.g.fs_ekf_thresh) { + check_failed(display_failure, "EKF compass variance"); + return false; + } + + // check home and EKF origin are not too far + if (copter.far_from_EKF_origin(ahrs.get_home())) { + check_failed(display_failure, "EKF-home variance"); + return false; + } + + // if we got here all must be ok + return true; +} + + +// arm_checks - perform final checks before arming +// always called just before arming. Return true if ok to arm +// has side-effect that logging is started +bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) +{ + const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); + + // always check if inertial nav has started and is ready + if (!ahrs.healthy()) { + check_failed(true, "AHRS not healthy"); + return false; + } + +#ifndef ALLOW_ARM_NO_COMPASS + // if external source of heading is available, we can skip compass health check + if (!ahrs.is_ext_nav_used_for_yaw()) { + const Compass &_compass = AP::compass(); + // check compass health + if (!_compass.healthy()) { + check_failed(true, "Compass not healthy"); + return false; + } + } +#endif + + Mode::Number control_mode = copter.control_mode; + + // always check if the current mode allows arming + if (!copter.flightmode->allows_arming(method == AP_Arming::Method::MAVLINK)) { + check_failed(true, "Mode not armable"); + return false; + } + + // always check motors + if (!motor_checks(true)) { + return false; + } + + // if we are using motor interlock switch and it's enabled, fail to arm + // skip check in Throw mode which takes control of the motor interlock + if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) { + check_failed(true, "Motor Interlock Enabled"); + return false; + } + + // if we are not using Emergency Stop switch option, force Estop false to ensure motors + // can run normally + if (!rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP)){ + SRV_Channels::set_emergency_stop(false); + // if we are using motor Estop switch, it must not be in Estop position + } else if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP) && SRV_Channels::get_emergency_stop()){ + gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped"); + return false; + } + + // succeed if arming checks are disabled + if (checks_to_perform == 0) { + return true; + } + + // check lean angle + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) { + if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > copter.aparm.angle_max) { + check_failed(ARMING_CHECK_INS, true, "Leaning"); + return false; + } + } + + // check adsb +#if ADSB_ENABLED == ENABLE + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) { + if (copter.failsafe.adsb) { + check_failed(ARMING_CHECK_PARAMETERS, true, "ADSB threat detected"); + return false; + } + } +#endif + + // check throttle + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) { + #if FRAME_CONFIG == HELI_FRAME + const char *rc_item = "Collective"; + #else + const char *rc_item = "Throttle"; + #endif + // check throttle is not too low - must be above failsafe throttle + if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) { + check_failed(ARMING_CHECK_RC, true, "%s below failsafe", rc_item); + return false; + } + + // check throttle is not too high - skips checks if arming from GCS in Guided + if (!(method == AP_Arming::Method::MAVLINK && (control_mode == Mode::Number::GUIDED || control_mode == Mode::Number::GUIDED_NOGPS))) { + // above top of deadband is too always high + if (copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()) > 0.0f) { + check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item); + return false; + } + // in manual modes throttle must be at zero + #if FRAME_CONFIG != HELI_FRAME + if ((copter.flightmode->has_manual_throttle() || control_mode == Mode::Number::DRIFT) && copter.channel_throttle->get_control_in() > 0) { + check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item); + return false; + } + #endif + } + } + + // check if safety switch has been pushed + if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { + check_failed(true, "Safety Switch"); + return false; + } + + // superclass method should always be the last thing called; it + // has side-effects which would need to be cleaned up if one of + // our arm checks failed + return AP_Arming::arm_checks(method); +} + +// mandatory checks that will be run if ARMING_CHECK is zero or arming forced +bool AP_Arming_Copter::mandatory_checks(bool display_failure) +{ + // call mandatory gps checks and update notify status because regular gps checks will not run + const bool result = mandatory_gps_checks(display_failure); + AP_Notify::flags.pre_arm_gps_check = result; + return result; +} + +void AP_Arming_Copter::set_pre_arm_check(bool b) +{ + copter.ap.pre_arm_check = b; + AP_Notify::flags.pre_arm_check = b; +} + +bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_checks) +{ + static bool in_arm_motors = false; + + // exit immediately if already in this function + if (in_arm_motors) { + return false; + } + in_arm_motors = true; + + // return true if already armed + if (copter.motors->armed()) { + in_arm_motors = false; + return true; + } + + if (!AP_Arming::arm(method, do_arming_checks)) { + AP_Notify::events.arming_failed = true; + in_arm_motors = false; + return false; + } + + // let logger know that we're armed (it may open logs e.g.) + AP::logger().set_vehicle_armed(true); + + // disable cpu failsafe because initialising everything takes a while + copter.failsafe_disable(); + + // notify that arming will occur (we do this early to give plenty of warning) + AP_Notify::flags.armed = true; + // call notify update a few times to ensure the message gets out + for (uint8_t i=0; i<=10; i++) { + AP::notify().update(); + } + +#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL + gcs().send_text(MAV_SEVERITY_INFO, "Arming motors"); +#endif + + // Remember Orientation + // -------------------- + copter.init_simple_bearing(); + + AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); + + copter.initial_armed_bearing = ahrs.yaw_sensor; + + if (!ahrs.home_is_set()) { + // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) + ahrs.resetHeightDatum(); + AP::logger().Write_Event(DATA_EKF_ALT_RESET); + + // we have reset height, so arming height is zero + copter.arming_altitude_m = 0; + } else if (!ahrs.home_is_locked()) { + // Reset home position if it has already been set before (but not locked) + if (!copter.set_home_to_current_location(false)) { + // ignore failure + } + + // remember the height when we armed + copter.arming_altitude_m = copter.inertial_nav.get_altitude() * 0.01; + } + copter.update_super_simple_bearing(false); + + // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point +#if MODE_SMARTRTL_ENABLED == ENABLED + copter.g2.smart_rtl.set_home(copter.position_ok()); +#endif + + // enable gps velocity based centrefugal force compensation + ahrs.set_correct_centrifugal(true); + hal.util->set_soft_armed(true); + +#if SPRAYER_ENABLED == ENABLED + // turn off sprayer's test if on + copter.sprayer.test_pump(false); +#endif + + // enable output to motors + copter.enable_motor_output(); + + // finally actually arm the motors + copter.motors->armed(true); + + AP::logger().Write_Event(DATA_ARMED); + + // log flight mode in case it was changed while vehicle was disarmed + AP::logger().Write_Mode((uint8_t)copter.control_mode, copter.control_mode_reason); + + // re-enable failsafe + copter.failsafe_enable(); + + // perf monitor ignores delay due to arming + AP::scheduler().perf_info.ignore_this_loop(); + + // flag exiting this function + in_arm_motors = false; + + // Log time stamp of arming event + copter.arm_time_ms = millis(); + + // Start the arming delay + copter.ap.in_arming_delay = true; + + // assumed armed without a arming, switch. Overridden in switches.cpp + copter.ap.armed_with_switch = false; + + // return success + return true; +} + +// arming.disarm - disarm motors +bool AP_Arming_Copter::disarm() +{ + // return immediately if we are already disarmed + if (!copter.motors->armed()) { + return true; + } + + if (!AP_Arming::disarm()) { + return false; + } + +#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL + gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors"); +#endif + + AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); + + // save compass offsets learned by the EKF if enabled + Compass &compass = AP::compass(); + if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) { + for(uint8_t i=0; iarmed(false); + +#if MODE_AUTO_ENABLED == ENABLED + // reset the mission + copter.mode_auto.mission.reset(); +#endif + + AP::logger().set_vehicle_armed(false); + + // disable gps velocity based centrefugal force compensation + ahrs.set_correct_centrifugal(false); + hal.util->set_soft_armed(false); + + copter.ap.in_arming_delay = false; + + return true; +} diff --git a/ArduCopter/AP_Arming.h b/ArduCopter/AP_Arming.h index 824d0a8022..1075a0ff49 100644 --- a/ArduCopter/AP_Arming.h +++ b/ArduCopter/AP_Arming.h @@ -1,62 +1,62 @@ -#pragma once - -#include - -class AP_Arming_Copter : public AP_Arming -{ -public: - friend class Copter; - friend class ToyMode; - - AP_Arming_Copter() : AP_Arming() - { - // default REQUIRE parameter to 1 (Copter does not have an - // actual ARMING_REQUIRE parameter) - require.set_default((uint8_t)Required::YES_MIN_PWM); - } - - /* Do not allow copies */ - AP_Arming_Copter(const AP_Arming_Copter &other) = delete; - AP_Arming_Copter &operator=(const AP_Arming_Copter&) = delete; - - void update(void); - - bool rc_calibration_checks(bool display_failure) override; - - bool disarm() override; - bool arm(AP_Arming::Method method, bool do_arming_checks=true) override; - -protected: - - bool pre_arm_checks(bool display_failure) override; - bool pre_arm_ekf_attitude_check(); - bool pre_arm_terrain_check(bool display_failure); - bool proximity_checks(bool display_failure) const override; - bool arm_checks(AP_Arming::Method method) override; - - // mandatory checks that cannot be bypassed. This function will only be called if ARMING_CHECK is zero or arming forced - bool mandatory_checks(bool display_failure) override; - - // NOTE! the following check functions *DO* call into AP_Arming: - bool ins_checks(bool display_failure) override; - bool compass_checks(bool display_failure) override; - bool gps_checks(bool display_failure) override; - bool barometer_checks(bool display_failure) override; - bool board_voltage_checks(bool display_failure) override; - - // NOTE! the following check functions *DO NOT* call into AP_Arming! - bool parameter_checks(bool display_failure); - bool motor_checks(bool display_failure); - bool pilot_throttle_checks(bool display_failure); - bool oa_checks(bool display_failure); - bool mandatory_gps_checks(bool display_failure); - - void set_pre_arm_check(bool b); - -private: - - // actually contains the pre-arm checks. This is wrapped so that - // we can store away success/failure of the checks. - bool run_pre_arm_checks(bool display_failure); - -}; +#pragma once + +#include + +class AP_Arming_Copter : public AP_Arming +{ +public: + friend class Copter; + friend class ToyMode; + + AP_Arming_Copter() : AP_Arming() + { + // default REQUIRE parameter to 1 (Copter does not have an + // actual ARMING_REQUIRE parameter) + require.set_default((uint8_t)Required::YES_MIN_PWM); + } + + /* Do not allow copies */ + AP_Arming_Copter(const AP_Arming_Copter &other) = delete; + AP_Arming_Copter &operator=(const AP_Arming_Copter&) = delete; + + void update(void); + + bool rc_calibration_checks(bool display_failure) override; + + bool disarm() override; + bool arm(AP_Arming::Method method, bool do_arming_checks=true) override; + +protected: + + bool pre_arm_checks(bool display_failure) override; + bool pre_arm_ekf_attitude_check(); + bool pre_arm_terrain_check(bool display_failure); + bool proximity_checks(bool display_failure) const override; + bool arm_checks(AP_Arming::Method method) override; + + // mandatory checks that cannot be bypassed. This function will only be called if ARMING_CHECK is zero or arming forced + bool mandatory_checks(bool display_failure) override; + + // NOTE! the following check functions *DO* call into AP_Arming: + bool ins_checks(bool display_failure) override; + bool compass_checks(bool display_failure) override; + bool gps_checks(bool display_failure) override; + bool barometer_checks(bool display_failure) override; + bool board_voltage_checks(bool display_failure) override; + + // NOTE! the following check functions *DO NOT* call into AP_Arming! + bool parameter_checks(bool display_failure); + bool motor_checks(bool display_failure); + bool pilot_throttle_checks(bool display_failure); + bool oa_checks(bool display_failure); + bool mandatory_gps_checks(bool display_failure); + bool mandatory_deadline_checks(bool display_failure); + void set_pre_arm_check(bool b); + +private: + + // actually contains the pre-arm checks. This is wrapped so that + // we can store away success/failure of the checks. + bool run_pre_arm_checks(bool display_failure); + +}; diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 86a41a29e9..76371dba5f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -837,7 +837,7 @@ private: void convert_lgr_parameters(void); void convert_tradheli_parameters(void); void convert_fs_options_params(void); - + void get_deadline_params( int32_t &deadline ); // precision_landing.cpp void init_precland(); void update_precland(); @@ -895,7 +895,7 @@ private: const char* get_frame_string(); void allocate_motors(void); bool is_tradheli() const; - + bool deadline_ok(); // terrain.cpp void terrain_update(); void terrain_logging(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index f6ab7685dd..c205a08858 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1,1638 +1,1643 @@ -#include "Copter.h" - -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -/* - * ArduCopter parameter definitions - * - */ - -#define GSCALAR(v, name, def) { copter.g.v.vtype, name, Parameters::k_param_ ## v, &copter.g.v, {def_value : def} } -#define ASCALAR(v, name, def) { copter.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&copter.aparm.v, {def_value : def} } -#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &copter.g.v, {group_info : class::var_info} } -#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info : class::var_info} } -#define GOBJECTPTR(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info : class::var_info}, AP_PARAM_FLAG_POINTER } -#define GOBJECTVARPTR(v, name, var_info_ptr) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info_ptr : var_info_ptr}, AP_PARAM_FLAG_POINTER | AP_PARAM_FLAG_INFO_POINTER } -#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&copter.v, {group_info : class::var_info} } - -#if FRAME_CONFIG == HELI_FRAME -// 6 here is AP_Motors::MOTOR_FRAME_HELI -#define DEFAULT_FRAME_CLASS 6 -#else -#define DEFAULT_FRAME_CLASS 0 -#endif - -const AP_Param::Info Copter::var_info[] = { - // @Param: SYSID_SW_MREV - // @DisplayName: Eeprom format version number - // @Description: This value is incremented when changes are made to the eeprom format - // @User: Advanced - // @ReadOnly: True - GSCALAR(format_version, "SYSID_SW_MREV", 0), - - // @Param: BOARD_ID - // @DisplayName: Board ID - // @Description: For mark board id - // @User: @Brown - GSCALAR(sysid_board_id, "SYSID_BOARD_ID", 100), - - // @Param: SYSID_BD_NAME1 - // @DisplayName: Board Name 1st - // @Description: For mark board name first - // @Range: 1-4294967269 - // @User: @Binsir - GSCALAR(sysid_board_name_1st, "SYSID_BDNAME_P1", 12848 ), - - // @Param: SYSID_BD_NAME2 - // @DisplayName: Board Name 2nd - // @Description: For mark board name last - // @User: @Binsir - GSCALAR(sysid_board_name_2nd, "SYSID_BDNAME_P2", 5329970), - - // @Param: SYSID_THISMAV - // @DisplayName: MAVLink system ID of this vehicle - // @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network - // @Range: 1 255 - // @User: Advanced - GSCALAR(sysid_this_mav, "SYSID_THISMAV", MAV_SYSTEM_ID), - - // @Param: SYSID_MYGCS - // @DisplayName: My ground station number - // @Description: Allows restricting radio overrides to only come from my ground station - // @Values: 255:Mission Planner and DroidPlanner, 252: AP Planner 2 - // @User: Advanced - GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), - - // @Param: PILOT_THR_FILT - // @DisplayName: Throttle filter cutoff - // @Description: Throttle filter cutoff (Hz) - active whenever altitude control is inactive - 0 to disable - // @User: Advanced - // @Units: Hz - // @Range: 0 10 - // @Increment: .5 - GSCALAR(throttle_filt, "PILOT_THR_FILT", 0), - - // @Param: PILOT_TKOFF_ALT - // @DisplayName: Pilot takeoff altitude - // @Description: Altitude that altitude control modes will climb to when a takeoff is triggered with the throttle stick. - // @User: Standard - // @Units: cm - // @Range: 0.0 1000.0 - // @Increment: 10 - GSCALAR(pilot_takeoff_alt, "PILOT_TKOFF_ALT", PILOT_TKOFF_ALT_DEFAULT), - - // @Param: PILOT_THR_BHV - // @DisplayName: Throttle stick behavior - // @Description: Bitmask containing various throttle stick options. TX with sprung throttle can set PILOT_THR_BHV to "1" so motor feedback when landed starts from mid-stick instead of bottom of stick. - // @User: Standard - // @Values: 0:None,1:Feedback from mid stick,2:High throttle cancels landing,4:Disarm on land detection - // @Bitmask: 0:Feedback from mid stick,1:High throttle cancels landing,2:Disarm on land detection - GSCALAR(throttle_behavior, "PILOT_THR_BHV", 0), - - // @Group: SERIAL - // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp - GOBJECT(serial_manager, "SERIAL", AP_SerialManager), - - // @Param: TELEM_DELAY - // @DisplayName: Telemetry startup delay - // @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up - // @User: Advanced - // @Units: s - // @Range: 0 30 - // @Increment: 1 - GSCALAR(telem_delay, "TELEM_DELAY", 0), - - // @Param: GCS_PID_MASK - // @DisplayName: GCS PID tuning mask - // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for - // @User: Advanced - // @Values: 0:None,1:Roll,2:Pitch,4:Yaw,8:AccelZ - // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:AccelZ - GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), - -#if MODE_RTL_ENABLED == ENABLED - // @Param: RTL_ALT - // @DisplayName: RTL Altitude - // @Description: The minimum alt above home the vehicle will climb to before returning. If the vehicle is flying higher than this value it will return at its current altitude. - // @Units: cm - // @Range: 200 8000 - // @Increment: 1 - // @User: Standard - GSCALAR(rtl_altitude, "RTL_ALT", RTL_ALT), - - // @Param: RTL_CONE_SLOPE - // @DisplayName: RTL cone slope - // @Description: Defines a cone above home which determines maximum climb - // @Range: 0.5 10.0 - // @Increment: .1 - // @Values: 0:Disabled,1:Shallow,3:Steep - // @User: Standard - GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE_DEFAULT), - - // @Param: RTL_SPEED - // @DisplayName: RTL speed - // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WPNAV_SPEED will be used instead. - // @Units: cm/s - // @Range: 0 2000 - // @Increment: 50 - // @User: Standard - GSCALAR(rtl_speed_cms, "RTL_SPEED", 0), - - // @Param: RTL_ALT_FINAL - // @DisplayName: RTL Final Altitude - // @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission. Set to zero to land. - // @Units: cm - // @Range: -1 1000 - // @Increment: 1 - // @User: Standard - GSCALAR(rtl_alt_final, "RTL_ALT_FINAL", RTL_ALT_FINAL), - - // @Param: RTL_CLIMB_MIN - // @DisplayName: RTL minimum climb - // @Description: The vehicle will climb this many cm during the initial climb portion of the RTL - // @Units: cm - // @Range: 0 3000 - // @Increment: 10 - // @User: Standard - GSCALAR(rtl_climb_min, "RTL_CLIMB_MIN", RTL_CLIMB_MIN_DEFAULT), - - // @Param: RTL_LOIT_TIME - // @DisplayName: RTL loiter time - // @Description: Time (in milliseconds) to loiter above home before beginning final descent - // @Units: ms - // @Range: 0 60000 - // @Increment: 1000 - // @User: Standard - GSCALAR(rtl_loiter_time, "RTL_LOIT_TIME", RTL_LOITER_TIME), -#endif - -#if RANGEFINDER_ENABLED == ENABLED - // @Param: RNGFND_GAIN - // @DisplayName: Rangefinder gain - // @Description: Used to adjust the speed with which the target altitude is changed when objects are sensed below the copter - // @Range: 0.01 2.0 - // @Increment: 0.01 - // @User: Standard - GSCALAR(rangefinder_gain, "RNGFND_GAIN", RANGEFINDER_GAIN_DEFAULT), -#endif - - // @Param: FS_GCS_ENABLE - // @DisplayName: Ground Station Failsafe Enable - // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. NB. The GCS Failsafe is only active when RC_OVERRIDE is being used to control the vehicle. - // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Deprecated in 4.0+),3:Enabled always SmartRTL or RTL,4:Enabled always SmartRTL or Land,5:Enabled always land (4.0+ Only) - // @User: Standard - GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED), - - // @Param: GPS_HDOP_GOOD - // @DisplayName: GPS Hdop Good - // @Description: GPS Hdop value at or below this value represent a good position. Used for pre-arm checks - // @Range: 100 900 - // @User: Advanced - GSCALAR(gps_hdop_good, "GPS_HDOP_GOOD", GPS_HDOP_GOOD_DEFAULT), - - // @Param: SUPER_SIMPLE - // @DisplayName: Super Simple Mode - // @Description: Bitmask to enable Super Simple mode for some flight modes. Setting this to Disabled(0) will disable Super Simple Mode - // @Values: 0:Disabled,1:Mode1,2:Mode2,3:Mode1+2,4:Mode3,5:Mode1+3,6:Mode2+3,7:Mode1+2+3,8:Mode4,9:Mode1+4,10:Mode2+4,11:Mode1+2+4,12:Mode3+4,13:Mode1+3+4,14:Mode2+3+4,15:Mode1+2+3+4,16:Mode5,17:Mode1+5,18:Mode2+5,19:Mode1+2+5,20:Mode3+5,21:Mode1+3+5,22:Mode2+3+5,23:Mode1+2+3+5,24:Mode4+5,25:Mode1+4+5,26:Mode2+4+5,27:Mode1+2+4+5,28:Mode3+4+5,29:Mode1+3+4+5,30:Mode2+3+4+5,31:Mode1+2+3+4+5,32:Mode6,33:Mode1+6,34:Mode2+6,35:Mode1+2+6,36:Mode3+6,37:Mode1+3+6,38:Mode2+3+6,39:Mode1+2+3+6,40:Mode4+6,41:Mode1+4+6,42:Mode2+4+6,43:Mode1+2+4+6,44:Mode3+4+6,45:Mode1+3+4+6,46:Mode2+3+4+6,47:Mode1+2+3+4+6,48:Mode5+6,49:Mode1+5+6,50:Mode2+5+6,51:Mode1+2+5+6,52:Mode3+5+6,53:Mode1+3+5+6,54:Mode2+3+5+6,55:Mode1+2+3+5+6,56:Mode4+5+6,57:Mode1+4+5+6,58:Mode2+4+5+6,59:Mode1+2+4+5+6,60:Mode3+4+5+6,61:Mode1+3+4+5+6,62:Mode2+3+4+5+6,63:Mode1+2+3+4+5+6 - // @User: Standard - GSCALAR(super_simple, "SUPER_SIMPLE", 0), - - // @Param: WP_YAW_BEHAVIOR - // @DisplayName: Yaw behaviour during missions - // @Description: Determines how the autopilot controls the yaw during missions and RTL - // @Values: 0:Never change yaw, 1:Face next waypoint, 2:Face next waypoint except RTL, 3:Face along GPS course - // @User: Standard - GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_DEFAULT), - - // @Param: LAND_SPEED - // @DisplayName: Land speed - // @Description: The descent speed for the final stage of landing in cm/s - // @Units: cm/s - // @Range: 30 200 - // @Increment: 10 - // @User: Standard - GSCALAR(land_speed, "LAND_SPEED", LAND_SPEED), - - // @Param: LAND_SPEED_HIGH - // @DisplayName: Land speed high - // @Description: The descent speed for the first stage of landing in cm/s. If this is zero then WPNAV_SPEED_DN is used - // @Units: cm/s - // @Range: 0 500 - // @Increment: 10 - // @User: Standard - GSCALAR(land_speed_high, "LAND_SPEED_HIGH", 0), - - // @Param: PILOT_SPEED_UP - // @DisplayName: Pilot maximum vertical speed ascending - // @Description: The maximum vertical ascending velocity the pilot may request in cm/s - // @Units: cm/s - // @Range: 50 500 - // @Increment: 10 - // @User: Standard - GSCALAR(pilot_speed_up, "PILOT_SPEED_UP", PILOT_VELZ_MAX), - - // @Param: PILOT_ACCEL_Z - // @DisplayName: Pilot vertical acceleration - // @Description: The vertical acceleration used when pilot is controlling the altitude - // @Units: cm/s/s - // @Range: 50 500 - // @Increment: 10 - // @User: Standard - GSCALAR(pilot_accel_z, "PILOT_ACCEL_Z", PILOT_ACCEL_Z_DEFAULT), - - // @Param: FS_THR_ENABLE - // @DisplayName: Throttle Failsafe Enable - // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel - // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Deprecated in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land - // @User: Standard - GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL), - - // @Param: FS_THR_VALUE - // @DisplayName: Throttle Failsafe Value - // @Description: The PWM level in microseconds on channel 3 below which throttle failsafe triggers - // @Range: 925 1100 - // @Units: PWM - // @Increment: 1 - // @User: Standard - GSCALAR(failsafe_throttle_value, "FS_THR_VALUE", FS_THR_VALUE_DEFAULT), - - // @Param: THR_DZ - // @DisplayName: Throttle deadzone - // @Description: The deadzone above and below mid throttle in PWM microseconds. Used in AltHold, Loiter, PosHold flight modes - // @User: Standard - // @Range: 0 300 - // @Units: PWM - // @Increment: 1 - GSCALAR(throttle_deadzone, "THR_DZ", THR_DZ_DEFAULT), - - // @Param: FLTMODE1 - // @DisplayName: Flight Mode 1 - // @Description: Flight mode when Channel 5 pwm is <= 1230 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate - // @User: Standard - GSCALAR(flight_mode1, "FLTMODE1", (uint8_t)FLIGHT_MODE_1), - - // @Param: FLTMODE2 - // @DisplayName: Flight Mode 2 - // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate - // @User: Standard - GSCALAR(flight_mode2, "FLTMODE2", (uint8_t)FLIGHT_MODE_2), - - // @Param: FLTMODE3 - // @DisplayName: Flight Mode 3 - // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate - // @User: Standard - GSCALAR(flight_mode3, "FLTMODE3", (uint8_t)FLIGHT_MODE_3), - - // @Param: FLTMODE4 - // @DisplayName: Flight Mode 4 - // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate - // @User: Standard - GSCALAR(flight_mode4, "FLTMODE4", (uint8_t)FLIGHT_MODE_4), - - // @Param: FLTMODE5 - // @DisplayName: Flight Mode 5 - // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate - // @User: Standard - GSCALAR(flight_mode5, "FLTMODE5", (uint8_t)FLIGHT_MODE_5), - - // @Param: FLTMODE6 - // @DisplayName: Flight Mode 6 - // @Description: Flight mode when Channel 5 pwm is >=1750 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate - // @User: Standard - GSCALAR(flight_mode6, "FLTMODE6", (uint8_t)FLIGHT_MODE_6), - - // @Param: FLTMODE_CH - // @DisplayName: Flightmode channel - // @Description: RC Channel to use for flight mode control - // @Values: 0:Disabled,5:Channel5,6:Channel6,7:Channel7,8:Channel8 - // @User: Advanced - GSCALAR(flight_mode_chan, "FLTMODE_CH", CH_MODE_DEFAULT), - - // @Param: SIMPLE - // @DisplayName: Simple mode bitmask - // @Description: Bitmask which holds which flight modes use simple heading mode (eg bit 0 = 1 means Flight Mode 0 uses simple mode) - // @User: Advanced - GSCALAR(simple_modes, "SIMPLE", 0), - - // @Param: LOG_BITMASK - // @DisplayName: Log bitmask - // @Description: 4 byte bitmap of log types to enable - // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll-AC315,45054:NearlyAll,131071:All+FastATT,262142:All+MotBatt,393214:All+FastIMU,397310:All+FastIMU+PID,655358:All+FullIMU,0:Disabled - // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:RCIN,7:IMU,8:CMD,9:CURRENT,10:RCOUT,11:OPTFLOW,12:PID,13:COMPASS,14:INAV,15:CAMERA,17:MOTBATT,18:IMU_FAST,19:IMU_RAW - // @User: Standard - GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), - - // @Param: ESC_CALIBRATION - // @DisplayName: ESC Calibration - // @Description: Controls whether ArduCopter will enter ESC calibration on the next restart. Do not adjust this parameter manually. - // @User: Advanced - // @Values: 0:Normal Start-up, 1:Start-up in ESC Calibration mode if throttle high, 2:Start-up in ESC Calibration mode regardless of throttle, 3:Start-up and automatically calibrate ESCs, 9:Disabled - GSCALAR(esc_calibrate, "ESC_CALIBRATION", 0), - - // @Param: TUNE - // @DisplayName: Channel 6 Tuning - // @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob - // @User: Standard - // @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,57:Winch,58:SysID Magnitude - GSCALAR(radio_tuning, "TUNE", 0), - - // @Param: FRAME_TYPE - // @DisplayName: Frame Type (+, X, V, etc) - // @Description: Controls motor mixing for multicopters. Not used for Tri or Traditional Helicopters. - // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B, 11:Y6F, 12:BetaFlightX, 13:DJIX, 14:ClockwiseX, 15: I - // @User: Standard - // @RebootRequired: True - GSCALAR(frame_type, "FRAME_TYPE", HAL_FRAME_TYPE_DEFAULT), - - // @Group: ARMING_ - // @Path: ../libraries/AP_Arming/AP_Arming.cpp - GOBJECT(arming, "ARMING_", AP_Arming_Copter), - - // @Param: DISARM_DELAY - // @DisplayName: Disarm delay - // @Description: Delay before automatic disarm in seconds. A value of zero disables auto disarm. - // @Units: s - // @Range: 0 127 - // @User: Advanced - GSCALAR(disarm_delay, "DISARM_DELAY", AUTO_DISARMING_DELAY), - - // @Param: ANGLE_MAX - // @DisplayName: Angle Max - // @Description: Maximum lean angle in all flight modes - // @Units: cdeg - // @Range: 1000 8000 - // @User: Advanced - ASCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX), - - // @Param: PHLD_BRAKE_RATE - // @DisplayName: PosHold braking rate - // @Description: PosHold flight mode's rotation rate during braking in deg/sec - // @Units: deg/s - // @Range: 4 12 - // @User: Advanced - GSCALAR(poshold_brake_rate, "PHLD_BRAKE_RATE", POSHOLD_BRAKE_RATE_DEFAULT), - - // @Param: PHLD_BRAKE_ANGLE - // @DisplayName: PosHold braking angle max - // @Description: PosHold flight mode's max lean angle during braking in centi-degrees - // @Units: cdeg - // @Range: 2000 4500 - // @User: Advanced - GSCALAR(poshold_brake_angle_max, "PHLD_BRAKE_ANGLE", POSHOLD_BRAKE_ANGLE_DEFAULT), - - // @Param: LAND_REPOSITION - // @DisplayName: Land repositioning - // @Description: Enables user input during LAND mode, the landing phase of RTL, and auto mode landings. - // @Values: 0:No repositioning, 1:Repositioning - // @User: Advanced - GSCALAR(land_repositioning, "LAND_REPOSITION", LAND_REPOSITION_DEFAULT), - - // @Param: FS_EKF_ACTION - // @DisplayName: EKF Failsafe Action - // @Description: Controls the action that will be taken when an EKF failsafe is invoked - // @Values: 1:Land, 2:AltHold, 3:Land even in Stabilize - // @User: Advanced - GSCALAR(fs_ekf_action, "FS_EKF_ACTION", FS_EKF_ACTION_DEFAULT), - - // @Param: FS_EKF_THRESH - // @DisplayName: EKF failsafe variance threshold - // @Description: Allows setting the maximum acceptable compass and velocity variance - // @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed - // @User: Advanced - GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", FS_EKF_THRESHOLD_DEFAULT), - - // @Param: FS_CRASH_CHECK - // @DisplayName: Crash check enable - // @Description: This enables automatic crash checking. When enabled the motors will disarm if a crash is detected. - // @Values: 0:Disabled, 1:Enabled - // @User: Advanced - GSCALAR(fs_crash_check, "FS_CRASH_CHECK", 1), - - // @Param: RC_SPEED - // @DisplayName: ESC Update Speed - // @Description: This is the speed in Hertz that your ESCs will receive updates - // @Units: Hz - // @Range: 50 490 - // @Increment: 1 - // @User: Advanced - GSCALAR(rc_speed, "RC_SPEED", RC_FAST_SPEED), - - // @Param: ACRO_RP_P - // @DisplayName: Acro Roll and Pitch P gain - // @Description: Converts pilot roll and pitch into a desired rate of rotation in ACRO and SPORT mode. Higher values mean faster rate of rotation. - // @Range: 1 10 - // @User: Standard - GSCALAR(acro_rp_p, "ACRO_RP_P", ACRO_RP_P), - - // @Param: ACRO_YAW_P - // @DisplayName: Acro Yaw P gain - // @Description: Converts pilot yaw input into a desired rate of rotation. Higher values mean faster rate of rotation. - // @Range: 1 10 - // @User: Standard - GSCALAR(acro_yaw_p, "ACRO_YAW_P", ACRO_YAW_P), - -#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED - // @Param: ACRO_BAL_ROLL - // @DisplayName: Acro Balance Roll - // @Description: rate at which roll angle returns to level in acro and sport mode. A higher value causes the vehicle to return to level faster. For helicopter sets the decay rate of the virtual flybar in the roll axis. A higher value causes faster decay of desired to actual attitude. - // @Range: 0 3 - // @Increment: 0.1 - // @User: Advanced - GSCALAR(acro_balance_roll, "ACRO_BAL_ROLL", ACRO_BALANCE_ROLL), - - // @Param: ACRO_BAL_PITCH - // @DisplayName: Acro Balance Pitch - // @Description: rate at which pitch angle returns to level in acro and sport mode. A higher value causes the vehicle to return to level faster. For helicopter sets the decay rate of the virtual flybar in the pitch axis. A higher value causes faster decay of desired to actual attitude. - // @Range: 0 3 - // @Increment: 0.1 - // @User: Advanced - GSCALAR(acro_balance_pitch, "ACRO_BAL_PITCH", ACRO_BALANCE_PITCH), -#endif - -#if MODE_ACRO_ENABLED == ENABLED - // @Param: ACRO_TRAINER - // @DisplayName: Acro Trainer - // @Description: Type of trainer used in acro mode - // @Values: 0:Disabled,1:Leveling,2:Leveling and Limited - // @User: Advanced - GSCALAR(acro_trainer, "ACRO_TRAINER", ACRO_TRAINER_LIMITED), - - // @Param: ACRO_RP_EXPO - // @DisplayName: Acro Roll/Pitch Expo - // @Description: Acro roll/pitch Expo to allow faster rotation when stick at edges - // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High - // @Range: -0.5 1.0 - // @User: Advanced - GSCALAR(acro_rp_expo, "ACRO_RP_EXPO", ACRO_RP_EXPO_DEFAULT), -#endif - - // variables not in the g class which contain EEPROM saved variables - -#if CAMERA == ENABLED - // @Group: CAM_ - // @Path: ../libraries/AP_Camera/AP_Camera.cpp - GOBJECT(camera, "CAM_", AP_Camera), -#endif - - // @Group: RELAY_ - // @Path: ../libraries/AP_Relay/AP_Relay.cpp - GOBJECT(relay, "RELAY_", AP_Relay), - -#if PARACHUTE == ENABLED - // @Group: CHUTE_ - // @Path: ../libraries/AP_Parachute/AP_Parachute.cpp - GOBJECT(parachute, "CHUTE_", AP_Parachute), -#endif - - // @Group: LGR_ - // @Path: ../libraries/AP_LandingGear/AP_LandingGear.cpp - GOBJECT(landinggear, "LGR_", AP_LandingGear), - -#if FRAME_CONFIG == HELI_FRAME - // @Group: IM_ - // @Path: ../libraries/AC_InputManager/AC_InputManager_Heli.cpp - GOBJECT(input_manager, "IM_", AC_InputManager_Heli), -#endif - - // @Group: COMPASS_ - // @Path: ../libraries/AP_Compass/AP_Compass.cpp - GOBJECT(compass, "COMPASS_", Compass), - - // @Group: INS_ - // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp - GOBJECT(ins, "INS_", AP_InertialSensor), - - // @Group: WPNAV_ - // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp - GOBJECTPTR(wp_nav, "WPNAV_", AC_WPNav), - - // @Group: LOIT_ - // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp - GOBJECTPTR(loiter_nav, "LOIT_", AC_Loiter), - -#if MODE_CIRCLE_ENABLED == ENABLED - // @Group: CIRCLE_ - // @Path: ../libraries/AC_WPNav/AC_Circle.cpp - GOBJECTPTR(circle_nav, "CIRCLE_", AC_Circle), -#endif - - // @Group: ATC_ - // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp -#if FRAME_CONFIG == HELI_FRAME - GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Heli), -#else - GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Multi), -#endif - - // @Group: PSC - // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp - GOBJECTPTR(pos_control, "PSC", AC_PosControl), - - // @Group: SR0_ - // @Path: GCS_Mavlink.cpp - GOBJECTN(_gcs.chan_parameters[0], gcs0, "SR0_", GCS_MAVLINK_Parameters), - - // @Group: SR1_ - // @Path: GCS_Mavlink.cpp - GOBJECTN(_gcs.chan_parameters[1], gcs1, "SR1_", GCS_MAVLINK_Parameters), - - // @Group: SR2_ - // @Path: GCS_Mavlink.cpp - GOBJECTN(_gcs.chan_parameters[2], gcs2, "SR2_", GCS_MAVLINK_Parameters), - - // @Group: SR3_ - // @Path: GCS_Mavlink.cpp - GOBJECTN(_gcs.chan_parameters[3], gcs3, "SR3_", GCS_MAVLINK_Parameters), - - // @Group: AHRS_ - // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp - GOBJECT(ahrs, "AHRS_", AP_AHRS), - -#if MOUNT == ENABLED - // @Group: MNT - // @Path: ../libraries/AP_Mount/AP_Mount.cpp - GOBJECT(camera_mount, "MNT", AP_Mount), -#endif - - // @Group: LOG - // @Path: ../libraries/AP_Logger/AP_Logger.cpp - GOBJECT(logger, "LOG", AP_Logger), - - // @Group: BATT - // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp - GOBJECT(battery, "BATT", AP_BattMonitor), - - // @Group: BRD_ - // @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp - GOBJECT(BoardConfig, "BRD_", AP_BoardConfig), - -#if HAL_WITH_UAVCAN - // @Group: CAN_ - // @Path: ../libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp - GOBJECT(BoardConfig_CAN, "CAN_", AP_BoardConfig_CAN), -#endif - -#if SPRAYER_ENABLED == ENABLED - // @Group: SPRAY_ - // @Path: ../libraries/AC_Sprayer/AC_Sprayer.cpp - GOBJECT(sprayer, "SPRAY_", AC_Sprayer), -#endif - -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - GOBJECT(sitl, "SIM_", SITL::SITL), -#endif - - // @Group: GND_ - // @Path: ../libraries/AP_Baro/AP_Baro.cpp - GOBJECT(barometer, "GND_", AP_Baro), - - // GPS driver - // @Group: GPS_ - // @Path: ../libraries/AP_GPS/AP_GPS.cpp - GOBJECT(gps, "GPS_", AP_GPS), - - // @Group: SCHED_ - // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp - GOBJECT(scheduler, "SCHED_", AP_Scheduler), - -#if AC_FENCE == ENABLED - // @Group: FENCE_ - // @Path: ../libraries/AC_Fence/AC_Fence.cpp - GOBJECT(fence, "FENCE_", AC_Fence), -#endif - - // @Group: AVOID_ - // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp -#if AC_AVOID_ENABLED == ENABLED - GOBJECT(avoid, "AVOID_", AC_Avoid), -#endif - -#if AC_RALLY == ENABLED - // @Group: RALLY_ - // @Path: AP_Rally.cpp,../libraries/AP_Rally/AP_Rally.cpp - GOBJECT(rally, "RALLY_", AP_Rally_Copter), -#endif - -#if FRAME_CONFIG == HELI_FRAME - // @Group: H_ - // @Path: ../libraries/AP_Motors/AP_MotorsHeli_Single.cpp,../libraries/AP_Motors/AP_MotorsHeli_Dual.cpp,../libraries/AP_Motors/AP_MotorsHeli.cpp - GOBJECTVARPTR(motors, "H_", &copter.motors_var_info), -#else - // @Group: MOT_ - // @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp - GOBJECTVARPTR(motors, "MOT_", &copter.motors_var_info), -#endif - - // @Group: RCMAP_ - // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp - GOBJECT(rcmap, "RCMAP_", RCMapper), - - // @Group: EK2_ - // @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp - GOBJECTN(EKF2, NavEKF2, "EK2_", NavEKF2), - - // @Group: EK3_ - // @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp - GOBJECTN(EKF3, NavEKF3, "EK3_", NavEKF3), - -#if MODE_AUTO_ENABLED == ENABLED - // @Group: MIS_ - // @Path: ../libraries/AP_Mission/AP_Mission.cpp - GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission), -#endif - - // @Group: RSSI_ - // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp - GOBJECT(rssi, "RSSI_", AP_RSSI), - -#if RANGEFINDER_ENABLED == ENABLED - // @Group: RNGFND - // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp - GOBJECT(rangefinder, "RNGFND", RangeFinder), -#endif - -#if AP_TERRAIN_AVAILABLE && AC_TERRAIN - // @Group: TERRAIN_ - // @Path: ../libraries/AP_Terrain/AP_Terrain.cpp - GOBJECT(terrain, "TERRAIN_", AP_Terrain), -#endif - -#if OPTFLOW == ENABLED - // @Group: FLOW - // @Path: ../libraries/AP_OpticalFlow/OpticalFlow.cpp - GOBJECT(optflow, "FLOW", OpticalFlow), -#endif - -#if PRECISION_LANDING == ENABLED - // @Group: PLND_ - // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp - GOBJECT(precland, "PLND_", AC_PrecLand), -#endif - -#if RPM_ENABLED == ENABLED - // @Group: RPM - // @Path: ../libraries/AP_RPM/AP_RPM.cpp - GOBJECT(rpm_sensor, "RPM", AP_RPM), -#endif - -#if ADSB_ENABLED == ENABLED - // @Group: ADSB_ - // @Path: ../libraries/AP_ADSB/AP_ADSB.cpp - GOBJECT(adsb, "ADSB_", AP_ADSB), - - // @Group: AVD_ - // @Path: ../libraries/AP_Avoidance/AP_Avoidance.cpp - GOBJECT(avoidance_adsb, "AVD_", AP_Avoidance_Copter), -#endif - - // @Group: NTF_ - // @Path: ../libraries/AP_Notify/AP_Notify.cpp - GOBJECT(notify, "NTF_", AP_Notify), - -#if MODE_THROW_ENABLED == ENABLED - // @Param: THROW_MOT_START - // @DisplayName: Start motors before throwing is detected - // @Description: Used by THROW mode. Controls whether motors will run at the speed set by THR_MIN or will be stopped when armed and waiting for the throw. - // @Values: 0:Stopped,1:Running - // @User: Standard - GSCALAR(throw_motor_start, "THROW_MOT_START", 0), -#endif - -#if AP_TERRAIN_AVAILABLE && AC_TERRAIN - // @Param: TERRAIN_FOLLOW - // @DisplayName: Terrain Following use control - // @Description: This enables terrain following for RTL and LAND flight modes. To use this option TERRAIN_ENABLE must be 1 and the GCS must support sending terrain data to the aircraft. In RTL the RTL_ALT will be considered a height above the terrain. In LAND mode the vehicle will slow to LAND_SPEED 10m above terrain (instead of 10m above home). This parameter does not affect AUTO and Guided which use a per-command flag to determine if the height is above-home, absolute or above-terrain. - // @Values: 0:Do Not Use in RTL and Land,1:Use in RTL and Land - // @User: Standard - GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0), -#endif - -#if OSD_ENABLED == ENABLED - // @Group: OSD - // @Path: ../libraries/AP_OSD/AP_OSD.cpp - GOBJECT(osd, "OSD", AP_OSD), -#endif - - // @Group: - // @Path: Parameters.cpp - GOBJECT(g2, "", ParametersG2), - - AP_VAREND -}; - -/* - 2nd group of parameters - */ -const AP_Param::GroupInfo ParametersG2::var_info[] = { - - // @Param: WP_NAVALT_MIN - // @DisplayName: Minimum navigation altitude - // @Description: This is the altitude in meters above which for navigation can begin. This applies in auto takeoff and auto landing. - // @Range: 0 5 - // @User: Standard - AP_GROUPINFO("WP_NAVALT_MIN", 1, ParametersG2, wp_navalt_min, 0), - -#if BUTTON_ENABLED == ENABLED - // @Group: BTN_ - // @Path: ../libraries/AP_Button/AP_Button.cpp - AP_SUBGROUPINFO(button, "BTN_", 2, ParametersG2, AP_Button), -#endif - -#if MODE_THROW_ENABLED == ENABLED - // @Param: THROW_NEXTMODE - // @DisplayName: Throw mode's follow up mode - // @Description: Vehicle will switch to this mode after the throw is successfully completed. Default is to stay in throw mode (18) - // @Values: 3:Auto,4:Guided,5:LOITER,6:RTL,9:Land,17:Brake,18:Throw - // @User: Standard - AP_GROUPINFO("THROW_NEXTMODE", 3, ParametersG2, throw_nextmode, 18), - - // @Param: THROW_TYPE - // @DisplayName: Type of Type - // @Description: Used by THROW mode. Specifies whether Copter is thrown upward or dropped. - // @Values: 0:Upward Throw,1:Drop - // @User: Standard - AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, ModeThrow::ThrowType_Upward), -#endif - - // @Param: GND_EFFECT_COMP - // @DisplayName: Ground Effect Compensation Enable/Disable - // @Description: Ground Effect Compensation Enable/Disable - // @Values: 0:Disabled,1:Enabled - // @User: Advanced - AP_GROUPINFO("GND_EFFECT_COMP", 5, ParametersG2, gndeffect_comp_enabled, 1), - -#if ADVANCED_FAILSAFE == ENABLED - // @Group: AFS_ - // @Path: ../libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp - AP_SUBGROUPINFO(afs, "AFS_", 6, ParametersG2, AP_AdvancedFailsafe), -#endif - - // @Param: DEV_OPTIONS - // @DisplayName: Development options - // @Description: Bitmask of developer options. The meanings of the bit fields in this parameter may vary at any time. Developers should check the source code for current meaning - // @Bitmask: 0:ADSBMavlinkProcessing,1:DevOptionVFR_HUDRelativeAlt - // @User: Advanced - AP_GROUPINFO("DEV_OPTIONS", 7, ParametersG2, dev_options, 0), - -#if BEACON_ENABLED == ENABLED - // @Group: BCN - // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp - AP_SUBGROUPINFO(beacon, "BCN", 14, ParametersG2, AP_Beacon), -#endif - -#if PROXIMITY_ENABLED == ENABLED - // @Group: PRX - // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp - AP_SUBGROUPINFO(proximity, "PRX", 8, ParametersG2, AP_Proximity), -#endif - - // @Param: ACRO_Y_EXPO - // @DisplayName: Acro Yaw Expo - // @Description: Acro yaw expo to allow faster rotation when stick at edges - // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High - // @Range: -0.5 1.0 - // @User: Advanced - AP_GROUPINFO("ACRO_Y_EXPO", 9, ParametersG2, acro_y_expo, ACRO_Y_EXPO_DEFAULT), - -#if MODE_ACRO_ENABLED == ENABLED - // @Param: ACRO_THR_MID - // @DisplayName: Acro Thr Mid - // @Description: Acro Throttle Mid - // @Range: 0 1 - // @User: Advanced - AP_GROUPINFO("ACRO_THR_MID", 10, ParametersG2, acro_thr_mid, ACRO_THR_MID_DEFAULT), -#endif - - // @Param: SYSID_ENFORCE - // @DisplayName: GCS sysid enforcement - // @Description: This controls whether packets from other than the expected GCS system ID will be accepted - // @Values: 0:NotEnforced,1:Enforced - // @User: Advanced - AP_GROUPINFO("SYSID_ENFORCE", 11, ParametersG2, sysid_enforce, 0), - -#if STATS_ENABLED == ENABLED - // @Group: STAT - // @Path: ../libraries/AP_Stats/AP_Stats.cpp - AP_SUBGROUPINFO(stats, "STAT", 12, ParametersG2, AP_Stats), -#endif - -#if GRIPPER_ENABLED == ENABLED - // @Group: GRIP_ - // @Path: ../libraries/AP_Gripper/AP_Gripper.cpp - AP_SUBGROUPINFO(gripper, "GRIP_", 13, ParametersG2, AP_Gripper), -#endif - - // @Param: FRAME_CLASS - // @DisplayName: Frame Class - // @Description: Controls major frame class for multicopter component - // @Values: 0:Undefined, 1:Quad, 2:Hexa, 3:Octa, 4:OctaQuad, 5:Y6, 6:Heli, 7:Tri, 8:SingleCopter, 9:CoaxCopter, 10:BiCopter, 11:Heli_Dual, 12:DodecaHexa, 13:HeliQuad - // @User: Standard - // @RebootRequired: True - AP_GROUPINFO("FRAME_CLASS", 15, ParametersG2, frame_class, DEFAULT_FRAME_CLASS), - - // @Group: SERVO - // @Path: ../libraries/SRV_Channel/SRV_Channels.cpp - AP_SUBGROUPINFO(servo_channels, "SERVO", 16, ParametersG2, SRV_Channels), - - // @Group: RC - // @Path: ../libraries/RC_Channel/RC_Channels_VarInfo.h - AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels_Copter), - -#if VISUAL_ODOMETRY_ENABLED == ENABLED - // @Group: VISO - // @Path: ../libraries/AP_VisualOdom/AP_VisualOdom.cpp - AP_SUBGROUPINFO(visual_odom, "VISO", 18, ParametersG2, AP_VisualOdom), -#endif - - // @Group: TCAL - // @Path: ../libraries/AP_TempCalibration/AP_TempCalibration.cpp - AP_SUBGROUPINFO(temp_calibration, "TCAL", 19, ParametersG2, AP_TempCalibration), - -#if TOY_MODE_ENABLED == ENABLED - // @Group: TMODE - // @Path: toy_mode.cpp - AP_SUBGROUPINFO(toy_mode, "TMODE", 20, ParametersG2, ToyMode), -#endif - -#if MODE_SMARTRTL_ENABLED == ENABLED - // @Group: SRTL_ - // @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp - AP_SUBGROUPINFO(smart_rtl, "SRTL_", 21, ParametersG2, AP_SmartRTL), -#endif - -#if WINCH_ENABLED == ENABLED - // @Group: WENC - // @Path: ../libraries/AP_WheelEncoder/AP_WheelEncoder.cpp - AP_SUBGROUPINFO(wheel_encoder, "WENC", 22, ParametersG2, AP_WheelEncoder), - - // @Group: WINCH_ - // @Path: ../libraries/AP_Winch/AP_Winch.cpp - AP_SUBGROUPINFO(winch, "WINCH", 23, ParametersG2, AP_Winch), -#endif - - // @Param: PILOT_SPEED_DN - // @DisplayName: Pilot maximum vertical speed descending - // @Description: The maximum vertical descending velocity the pilot may request in cm/s - // @Units: cm/s - // @Range: 50 500 - // @Increment: 10 - // @User: Standard - AP_GROUPINFO("PILOT_SPEED_DN", 24, ParametersG2, pilot_speed_dn, 0), - - // @Param: LAND_ALT_LOW - // @DisplayName: Land alt low - // @Description: Altitude during Landing at which vehicle slows to LAND_SPEED - // @Units: cm - // @Range: 100 10000 - // @Increment: 10 - // @User: Advanced - AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low, 1000), - -#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED - // @Group: FHLD - // @Path: mode_flowhold.cpp - AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, ModeFlowHold), -#endif - -#if MODE_FOLLOW_ENABLED == ENABLED - // @Group: FOLL - // @Path: ../libraries/AP_Follow/AP_Follow.cpp - AP_SUBGROUPINFO(follow, "FOLL", 27, ParametersG2, AP_Follow), -#endif - -#ifdef USER_PARAMS_ENABLED - AP_SUBGROUPINFO(user_parameters, "USR", 28, ParametersG2, UserParameters), -#endif - -#if AUTOTUNE_ENABLED == ENABLED - // @Group: AUTOTUNE_ - // @Path: ../libraries/AC_AutoTune/AC_AutoTune.cpp - AP_SUBGROUPPTR(autotune_ptr, "AUTOTUNE_", 29, ParametersG2, AutoTune), -#endif - -#ifdef ENABLE_SCRIPTING - // @Group: SCR_ - // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp - AP_SUBGROUPINFO(scripting, "SCR_", 30, ParametersG2, AP_Scripting), -#endif - - // @Param: TUNE_MIN - // @DisplayName: Tuning minimum - // @Description: Minimum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to - // @User: Standard - AP_GROUPINFO("TUNE_MIN", 31, ParametersG2, tuning_min, 0), - - // @Param: TUNE_MAX - // @DisplayName: Tuning maximum - // @Description: Maximum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to - // @User: Standard - AP_GROUPINFO("TUNE_MAX", 32, ParametersG2, tuning_max, 0), - -#if AC_OAPATHPLANNER_ENABLED == ENABLED - // @Group: OA_ - // @Path: ../libraries/AC_Avoidance/AP_OAPathPlanner.cpp - AP_SUBGROUPINFO(oa, "OA_", 33, ParametersG2, AP_OAPathPlanner), -#endif - -#if MODE_SYSTEMID_ENABLED == ENABLED - // @Group: SID - // @Path: mode_systemid.cpp - AP_SUBGROUPPTR(mode_systemid_ptr, "SID", 34, ParametersG2, ModeSystemId), -#endif - - // @Param: FS_VIBE_ENABLE - // @DisplayName: Vibration Failsafe enable - // @Description: This enables the vibration failsafe which will use modified altitude estimation and control during high vibrations - // @Values: 0:Disabled, 1:Enabled - // @User: Standard - AP_GROUPINFO("FS_VIBE_ENABLE", 35, ParametersG2, fs_vibe_enabled, 1), - - // @Param: FS_OPTIONS - // @DisplayName: Failsafe options bitmask - // @Description: Bitmask of additional options for battery, radio, & GCS failsafes. 0 (default) disables all options. - // @Values: 0:Disabled, 1:Continue if in Auto on RC failsafe only, 2:Continue if in Auto on GCS failsafe only, 3:Continue if in Auto on RC and/or GCS failsafe, 4:Continue if in Guided on RC failsafe only, 8:Continue if landing on any failsafe, 16:Continue if in pilot controlled modes on GCS failsafe, 19:Continue if in Auto on RC and/or GCS failsafe and continue if in pilot controlled modes on GCS failsafe - // @Bitmask: 0:Continue if in Auto on RC failsafe, 1:Continue if in Auto on GCS failsafe, 2:Continue if in Guided on RC failsafe, 3:Continue if landing on any failsafe, 4:Continue if in pilot controlled modes on GCS failsafe - // @User: Advanced - AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, 0), - -#if MODE_AUTOROTATE_ENABLED == ENABLED - // @Group: AROT_ - // @Path: ../libraries/AC_Autorotation/AC_Autorotation.cpp - AP_SUBGROUPINFO(arot, "AROT_", 37, ParametersG2, AC_Autorotation), -#endif - - - - AP_GROUPEND -}; - -// These param descriptions are here so that users of beta Mission Planner (which uses the master branch as its source of descriptions) -// can get them. These lines can be removed once Copter-3.7.0-beta testing begins or we improve the source of descriptions for GCSs. -// -// @Param: CH7_OPT -// @DisplayName: Channel 7 option -// @Description: Select which function is performed when CH7 is above 1800 pwm -// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl -// @User: Standard - -// @Param: CH8_OPT -// @DisplayName: Channel 8 option -// @Description: Select which function is performed when CH8 is above 1800 pwm -// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl -// @User: Standard - -// @Param: CH9_OPT -// @DisplayName: Channel 9 option -// @Description: Select which function is performed when CH9 is above 1800 pwm -// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl -// @User: Standard - -// @Param: CH10_OPT -// @DisplayName: Channel 10 option -// @Description: Select which function is performed when CH10 is above 1800 pwm -// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl -// @User: Standard - -// @Param: CH11_OPT -// @DisplayName: Channel 11 option -// @Description: Select which function is performed when CH11 is above 1800 pwm -// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl -// @User: Standard - -// @Param: CH12_OPT -// @DisplayName: Channel 12 option -// @Description: Select which function is performed when CH12 is above 1800 pwm -// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl -// @User: Standard - -// @Param: TUNE_LOW -// @DisplayName: Tuning minimum -// @Description: The minimum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob -// @User: Standard -// @Range: 0 32767 - -// @Param: TUNE_HIGH -// @DisplayName: Tuning maximum -// @Description: The maximum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob -// @User: Standard -// @Range: 0 32767 - -/* - constructor for g2 object - */ -ParametersG2::ParametersG2(void) - : temp_calibration() // this doesn't actually need constructing, but removing it here is problematic syntax-wise -#if BEACON_ENABLED == ENABLED - , beacon(copter.serial_manager) -#endif -#if PROXIMITY_ENABLED == ENABLED - , proximity() -#endif -#if ADVANCED_FAILSAFE == ENABLED - ,afs(copter.mode_auto.mission) -#endif -#if MODE_SMARTRTL_ENABLED == ENABLED - ,smart_rtl() -#endif -#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED - ,mode_flowhold_ptr(&copter.mode_flowhold) -#endif -#if MODE_FOLLOW_ENABLED == ENABLED - ,follow() -#endif -#ifdef USER_PARAMS_ENABLED - ,user_parameters() -#endif -#if AUTOTUNE_ENABLED == ENABLED - ,autotune_ptr(&copter.autotune) -#endif -#if MODE_SYSTEMID_ENABLED == ENABLED - ,mode_systemid_ptr(&copter.mode_systemid) -#endif -#if MODE_AUTOROTATE_ENABLED == ENABLED - ,arot(copter.inertial_nav) -#endif -{ - AP_Param::setup_object_defaults(this, var_info); -} - -/* - This is a conversion table from old parameter values to new - parameter names. The startup code looks for saved values of the old - parameters and will copy them across to the new parameters if the - new parameter does not yet have a saved value. It then saves the new - value. - - Note that this works even if the old parameter has been removed. It - relies on the old k_param index not being removed - - The second column below is the index in the var_info[] table for the - old object. This should be zero for top level parameters. - */ -const AP_Param::ConversionInfo conversion_table[] = { - { Parameters::k_param_log_bitmask_old, 0, AP_PARAM_INT16, "LOG_BITMASK" }, - { Parameters::k_param_serial0_baud, 0, AP_PARAM_INT16, "SERIAL0_BAUD" }, - { Parameters::k_param_serial1_baud, 0, AP_PARAM_INT16, "SERIAL1_BAUD" }, - { Parameters::k_param_serial2_baud, 0, AP_PARAM_INT16, "SERIAL2_BAUD" }, - { Parameters::k_param_arming_check_old, 0, AP_PARAM_INT8, "ARMING_CHECK" }, - // battery - { Parameters::k_param_fs_batt_voltage, 0, AP_PARAM_FLOAT, "BATT_LOW_VOLT" }, - { Parameters::k_param_fs_batt_mah, 0, AP_PARAM_FLOAT, "BATT_LOW_MAH" }, - { Parameters::k_param_failsafe_battery_enabled,0, AP_PARAM_INT8, "BATT_FS_LOW_ACT" }, - - { Parameters::Parameters::k_param_ch7_option_old, 0, AP_PARAM_INT8, "RC7_OPTION" }, - { Parameters::Parameters::k_param_ch8_option_old, 0, AP_PARAM_INT8, "RC8_OPTION" }, - { Parameters::Parameters::k_param_ch9_option_old, 0, AP_PARAM_INT8, "RC9_OPTION" }, - { Parameters::Parameters::k_param_ch10_option_old, 0, AP_PARAM_INT8, "RC10_OPTION" }, - { Parameters::Parameters::k_param_ch11_option_old, 0, AP_PARAM_INT8, "RC11_OPTION" }, - { Parameters::Parameters::k_param_ch12_option_old, 0, AP_PARAM_INT8, "RC12_OPTION" }, - { Parameters::k_param_compass_enabled_deprecated, 0, AP_PARAM_INT8, "COMPASS_ENABLE" }, - { Parameters::k_param_arming, 2, AP_PARAM_INT16, "ARMING_CHECK" }, -}; - -void Copter::load_parameters(void) -{ - if (!AP_Param::check_var_info()) { - hal.console->printf("Bad var table\n"); - AP_HAL::panic("Bad var table"); - } - - // disable centrifugal force correction, it will be enabled as part of the arming process - ahrs.set_correct_centrifugal(false); - hal.util->set_soft_armed(false); - - if (!g.format_version.load() || - g.format_version != Parameters::k_format_version) { - - // erase all parameters - hal.console->printf("Firmware change: erasing EEPROM...\n"); - StorageManager::erase(); - AP_Param::erase_all(); - - // save the current format version - g.format_version.set_and_save(Parameters::k_format_version); - hal.console->printf("done.\n"); - } - - uint32_t before = micros(); - // Load all auto-loaded EEPROM variables - AP_Param::load_all(); - AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table)); - - // convert landing gear parameters - convert_lgr_parameters(); - - // convert fs_options parameters - convert_fs_options_params(); - - hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); - - // setup AP_Param frame type flags - AP_Param::set_frame_type_flags(AP_PARAM_FRAME_COPTER); - -} - -// handle conversion of PID gains -void Copter::convert_pid_parameters(void) -{ - // conversion info - const AP_Param::ConversionInfo pid_conversion_info[] = { - { Parameters::k_param_pid_rate_roll, 0, AP_PARAM_FLOAT, "ATC_RAT_RLL_P" }, - { Parameters::k_param_pid_rate_roll, 1, AP_PARAM_FLOAT, "ATC_RAT_RLL_I" }, - { Parameters::k_param_pid_rate_roll, 2, AP_PARAM_FLOAT, "ATC_RAT_RLL_D" }, - { Parameters::k_param_pid_rate_pitch, 0, AP_PARAM_FLOAT, "ATC_RAT_PIT_P" }, - { Parameters::k_param_pid_rate_pitch, 1, AP_PARAM_FLOAT, "ATC_RAT_PIT_I" }, - { Parameters::k_param_pid_rate_pitch, 2, AP_PARAM_FLOAT, "ATC_RAT_PIT_D" }, - { Parameters::k_param_pid_rate_yaw, 0, AP_PARAM_FLOAT, "ATC_RAT_YAW_P" }, - { Parameters::k_param_pid_rate_yaw, 1, AP_PARAM_FLOAT, "ATC_RAT_YAW_I" }, - { Parameters::k_param_pid_rate_yaw, 2, AP_PARAM_FLOAT, "ATC_RAT_YAW_D" }, -#if FRAME_CONFIG == HELI_FRAME - { Parameters::k_param_pid_rate_roll, 4, AP_PARAM_FLOAT, "ATC_RAT_RLL_VFF" }, - { Parameters::k_param_pid_rate_pitch, 4, AP_PARAM_FLOAT, "ATC_RAT_PIT_VFF" }, - { Parameters::k_param_pid_rate_yaw , 4, AP_PARAM_FLOAT, "ATC_RAT_YAW_VFF" }, -#endif - }; - const AP_Param::ConversionInfo imax_conversion_info[] = { - { Parameters::k_param_pid_rate_roll, 5, AP_PARAM_FLOAT, "ATC_RAT_RLL_IMAX" }, - { Parameters::k_param_pid_rate_pitch, 5, AP_PARAM_FLOAT, "ATC_RAT_PIT_IMAX" }, - { Parameters::k_param_pid_rate_yaw, 5, AP_PARAM_FLOAT, "ATC_RAT_YAW_IMAX" }, -#if FRAME_CONFIG == HELI_FRAME - { Parameters::k_param_pid_rate_roll, 7, AP_PARAM_FLOAT, "ATC_RAT_RLL_ILMI" }, - { Parameters::k_param_pid_rate_pitch, 7, AP_PARAM_FLOAT, "ATC_RAT_PIT_ILMI" }, - { Parameters::k_param_pid_rate_yaw, 7, AP_PARAM_FLOAT, "ATC_RAT_YAW_ILMI" }, -#endif - }; - // conversion from Copter-3.3 to Copter-3.4 - const AP_Param::ConversionInfo angle_and_filt_conversion_info[] = { - { Parameters::k_param_p_stabilize_roll, 0, AP_PARAM_FLOAT, "ATC_ANG_RLL_P" }, - { Parameters::k_param_p_stabilize_pitch, 0, AP_PARAM_FLOAT, "ATC_ANG_PIT_P" }, - { Parameters::k_param_p_stabilize_yaw, 0, AP_PARAM_FLOAT, "ATC_ANG_YAW_P" }, - { Parameters::k_param_pid_rate_roll, 6, AP_PARAM_FLOAT, "ATC_RAT_RLL_FILT" }, - { Parameters::k_param_pid_rate_pitch, 6, AP_PARAM_FLOAT, "ATC_RAT_PIT_FILT" }, - { Parameters::k_param_pid_rate_yaw, 6, AP_PARAM_FLOAT, "ATC_RAT_YAW_FILT" }, - { Parameters::k_param_pi_vel_xy, 0, AP_PARAM_FLOAT, "PSC_VELXY_P" }, - { Parameters::k_param_pi_vel_xy, 1, AP_PARAM_FLOAT, "PSC_VELXY_I" }, - { Parameters::k_param_pi_vel_xy, 2, AP_PARAM_FLOAT, "PSC_VELXY_IMAX" }, - { Parameters::k_param_pi_vel_xy, 3, AP_PARAM_FLOAT, "PSC_VELXY_FILT" }, - { Parameters::k_param_p_vel_z, 0, AP_PARAM_FLOAT, "PSC_VELZ_P" }, - { Parameters::k_param_pid_accel_z, 0, AP_PARAM_FLOAT, "PSC_ACCZ_P" }, - { Parameters::k_param_pid_accel_z, 1, AP_PARAM_FLOAT, "PSC_ACCZ_I" }, - { Parameters::k_param_pid_accel_z, 2, AP_PARAM_FLOAT, "PSC_ACCZ_D" }, - { Parameters::k_param_pid_accel_z, 5, AP_PARAM_FLOAT, "PSC_ACCZ_IMAX" }, - { Parameters::k_param_pid_accel_z, 6, AP_PARAM_FLOAT, "PSC_ACCZ_FLTE" }, - { Parameters::k_param_p_alt_hold, 0, AP_PARAM_FLOAT, "PSC_POSZ_P" }, - { Parameters::k_param_p_pos_xy, 0, AP_PARAM_FLOAT, "PSC_POSXY_P" }, - }; - const AP_Param::ConversionInfo throttle_conversion_info[] = { - { Parameters::k_param_throttle_min, 0, AP_PARAM_FLOAT, "MOT_SPIN_MIN" }, - { Parameters::k_param_throttle_mid, 0, AP_PARAM_FLOAT, "MOT_THST_HOVER" } - }; - const AP_Param::ConversionInfo loiter_conversion_info[] = { - { Parameters::k_param_wp_nav, 4, AP_PARAM_FLOAT, "LOIT_SPEED" }, - { Parameters::k_param_wp_nav, 7, AP_PARAM_FLOAT, "LOIT_BRK_JERK" }, - { Parameters::k_param_wp_nav, 8, AP_PARAM_FLOAT, "LOIT_ACC_MAX" }, - { Parameters::k_param_wp_nav, 9, AP_PARAM_FLOAT, "LOIT_BRK_ACCEL" } - }; - - // gains increase by 27% due to attitude controller's switch to use radians instead of centi-degrees - // and motor libraries switch to accept inputs in -1 to +1 range instead of -4500 ~ +4500 - float pid_scaler = 1.27f; - -#if FRAME_CONFIG != HELI_FRAME - // Multicopter x-frame gains are 40% lower because -1 or +1 input to motors now results in maximum rotation - if (g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_X || g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_V || g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_H) { - pid_scaler = 0.9f; - } -#endif - - // scale PID gains - uint8_t table_size = ARRAY_SIZE(pid_conversion_info); - for (uint8_t i=0; iconfigured_in_storage() || - servo_max->configured_in_storage() || - servo_trim->configured_in_storage() || - servo_reversed->configured_in_storage()) { - // has been previously saved, don't upgrade - return; - } - - // get the old PWM values - AP_Int16 old_pwm; - uint16_t old_retract=0, old_deploy=0; - const AP_Param::ConversionInfo cinfo_ret { Parameters::k_param_landinggear, 0, AP_PARAM_INT16, nullptr }; - const AP_Param::ConversionInfo cinfo_dep { Parameters::k_param_landinggear, 1, AP_PARAM_INT16, nullptr }; - if (AP_Param::find_old_parameter(&cinfo_ret, &old_pwm)) { - old_retract = (uint16_t)old_pwm.get(); - } - if (AP_Param::find_old_parameter(&cinfo_dep, &old_pwm)) { - old_deploy = (uint16_t)old_pwm.get(); - } - - if (old_retract == 0 && old_deploy == 0) { - // old parameters were never set - return; - } - - // use old defaults - if (old_retract == 0) { - old_retract = 1250; - } - if (old_deploy == 0) { - old_deploy = 1750; - } - - // set and save correct values on the servo - if (old_retract <= old_deploy) { - servo_max->set_and_save(old_deploy); - servo_min->set_and_save(old_retract); - servo_trim->set_and_save(old_retract); - servo_reversed->set_and_save_ifchanged(0); - } else { - servo_max->set_and_save(old_retract); - servo_min->set_and_save(old_deploy); - servo_trim->set_and_save(old_deploy); - servo_reversed->set_and_save_ifchanged(1); - } -} - -#if FRAME_CONFIG == HELI_FRAME -// handle conversion of tradheli parameters from Copter-3.6 to Copter-3.7 -void Copter::convert_tradheli_parameters(void) -{ - if (g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI) { - // single heli conversion info - const AP_Param::ConversionInfo singleheli_conversion_info[] = { - { Parameters::k_param_motors, 1, AP_PARAM_INT16, "H_SW_H3_SV1_POS" }, - { Parameters::k_param_motors, 2, AP_PARAM_INT16, "H_SW_H3_SV2_POS" }, - { Parameters::k_param_motors, 3, AP_PARAM_INT16, "H_SW_H3_SV3_POS" }, - { Parameters::k_param_motors, 7, AP_PARAM_INT16, "H_SW_H3_PHANG" }, - { Parameters::k_param_motors, 19, AP_PARAM_INT8, "H_SW_COL_DIR" }, - }; - - // convert single heli parameters without scaling - uint8_t table_size = ARRAY_SIZE(singleheli_conversion_info); - for (uint8_t i=0; iconfigured_in_storage()) { - // the new parameter is not in storage so set generic swash - AP_Param::set_and_save_by_name("H_SW_TYPE", SwashPlateType::SWASHPLATE_TYPE_H3); - } - } - } - } - } else if (g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_DUAL) { - // dual heli conversion info - const AP_Param::ConversionInfo dualheli_conversion_info[] = { - { Parameters::k_param_motors, 1, AP_PARAM_INT16, "H_SW_H3_SV1_POS" }, - { Parameters::k_param_motors, 2, AP_PARAM_INT16, "H_SW_H3_SV2_POS" }, - { Parameters::k_param_motors, 3, AP_PARAM_INT16, "H_SW_H3_SV3_POS" }, - { Parameters::k_param_motors, 4, AP_PARAM_INT16, "H_SW2_H3_SV1_POS" }, - { Parameters::k_param_motors, 5, AP_PARAM_INT16, "H_SW2_H3_SV2_POS" }, - { Parameters::k_param_motors, 6, AP_PARAM_INT16, "H_SW2_H3_SV3_POS" }, - { Parameters::k_param_motors, 7, AP_PARAM_INT16, "H_SW_H3_PHANG" }, - { Parameters::k_param_motors, 8, AP_PARAM_INT16, "H_SW2_H3_PHANG" }, - { Parameters::k_param_motors, 19, AP_PARAM_INT8, "H_SW_COL_DIR" }, - { Parameters::k_param_motors, 19, AP_PARAM_INT8, "H_SW2_COL_DIR" }, - }; - - // convert dual heli parameters without scaling - uint8_t table_size = ARRAY_SIZE(dualheli_conversion_info); - for (uint8_t i=0; iconfigured_in_storage()) { - // the new parameter is not in storage so set generic swash - AP_Param::set_and_save_by_name("H_SW_TYPE", SwashPlateType::SWASHPLATE_TYPE_H3); - } - } - } - //SWASH 2 - // old swash type is not in eeprom and thus type is default value of generic swash - if (swash2_pos1_exist || swash2_pos2_exist || swash2_pos3_exist || swash2_phang_exist) { - // if any params exist with the generic swash then the upgraded swash type must be generic - // find the new variable in the variable structures - enum ap_var_type ptype; - AP_Param *ap2; - ap2 = AP_Param::find("H_SW2_TYPE", &ptype); - // make sure the pointer is valid - if (ap2 != nullptr) { - // see if we can load it from EEPROM - if (!ap2->configured_in_storage()) { - // the new parameter is not in storage so set generic swash - AP_Param::set_and_save_by_name("H_SW2_TYPE", SwashPlateType::SWASHPLATE_TYPE_H3); - } - } - } - } - - // table of rsc parameters to be converted with scaling - const AP_Param::ConversionInfo rschelipct_conversion_info[] = { - { Parameters::k_param_motors, 1280, AP_PARAM_INT16, "H_RSC_THRCRV_0" }, - { Parameters::k_param_motors, 1344, AP_PARAM_INT16, "H_RSC_THRCRV_25" }, - { Parameters::k_param_motors, 1408, AP_PARAM_INT16, "H_RSC_THRCRV_50" }, - { Parameters::k_param_motors, 1472, AP_PARAM_INT16, "H_RSC_THRCRV_75" }, - { Parameters::k_param_motors, 1536, AP_PARAM_INT16, "H_RSC_THRCRV_100" }, - { Parameters::k_param_motors, 448, AP_PARAM_INT16, "H_RSC_SETPOINT" }, - { Parameters::k_param_motors, 768, AP_PARAM_INT16, "H_RSC_CRITICAL" }, - { Parameters::k_param_motors, 832, AP_PARAM_INT16, "H_RSC_IDLE" }, - }; - // convert heli rsc parameters with scaling - uint8_t table_size = ARRAY_SIZE(rschelipct_conversion_info); - for (uint8_t i=0; iget() > 100 ) { - uint16_t tailspeed_pct = (uint16_t)(0.1f * tailspeed->get()); - AP_Param::set_and_save_by_name("H_TAIL_SPEED", tailspeed_pct ); - } - - // table of stabilize collective parameters to be converted with scaling - const AP_Param::ConversionInfo collhelipct_conversion_info[] = { - { Parameters::k_param_input_manager, 1, AP_PARAM_INT16, "IM_STB_COL_1" }, - { Parameters::k_param_input_manager, 2, AP_PARAM_INT16, "IM_STB_COL_2" }, - { Parameters::k_param_input_manager, 3, AP_PARAM_INT16, "IM_STB_COL_3" }, - { Parameters::k_param_input_manager, 4, AP_PARAM_INT16, "IM_STB_COL_4" }, - }; - - // convert stabilize collective parameters with scaling - table_size = ARRAY_SIZE(collhelipct_conversion_info); - for (uint8_t i=0; iconfigured_in_storage() || ptype != AP_PARAM_INT32) { - return; - } - - // Variable to capture the new FS_OPTIONS setting - int32_t fs_options_converted = 0; - - // If FS_THR_ENABLED is 2 (continue mission), change to RTL and add continue mission to the new FS_OPTIONS parameter - if (g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) { - fs_options_converted |= int32_t(FailsafeOption::RC_CONTINUE_IF_AUTO); - AP_Param::set_and_save_by_name("FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL); - } - - // If FS_GCS_ENABLED is 2 (continue mission), change to RTL and add continue mission to the new FS_OPTIONS parameter - if (g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) { - fs_options_converted |= int32_t(FailsafeOption::GCS_CONTINUE_IF_AUTO); - AP_Param::set_and_save_by_name("FS_GCS_ENABLE", FS_GCS_ENABLED_ALWAYS_RTL); - } - - // Write the new value to FS_OPTIONS - // AP_Param::set_and_save_by_name("FS_OPTIONS", fs_options_converted); - fs_opt->set_and_save(fs_options_converted); -} - - -// modify by @Brown -const char* Copter::get_sysid_board_id(void) -{ - - static char buf[50]; - // snprintf(buf, sizeof(buf), "Version: v3.5.6 ,Board ID: ZRZK.19QT3.%d",(int)g.sysid_board_id); - - int32_t nameValue1 =(int32_t) g.sysid_board_name_1st; - int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd; - char name[6] = {' ',' ',' ',' ',' ','.'}; - // memset(name,0,6); - nameValue2 = nameValue2 & 0xffffff; - // name[5] = "a"; - name[4] = nameValue2&0xFF; - name[3] = nameValue2>>8&0xFF; - name[2] = nameValue2>>16& 0xFF; - name[1] = nameValue1&0xFF; - name[0] = nameValue1>>8 & 0xFF; - snprintf(buf, sizeof(buf), "Version: zr-v4.0 ,Board ID: ZRZK.%5s%d",name,(int)g.sysid_board_id); - return buf; -} \ No newline at end of file +#include "Copter.h" + +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * ArduCopter parameter definitions + * + */ + +#define GSCALAR(v, name, def) { copter.g.v.vtype, name, Parameters::k_param_ ## v, &copter.g.v, {def_value : def} } +#define ASCALAR(v, name, def) { copter.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&copter.aparm.v, {def_value : def} } +#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &copter.g.v, {group_info : class::var_info} } +#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info : class::var_info} } +#define GOBJECTPTR(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info : class::var_info}, AP_PARAM_FLAG_POINTER } +#define GOBJECTVARPTR(v, name, var_info_ptr) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info_ptr : var_info_ptr}, AP_PARAM_FLAG_POINTER | AP_PARAM_FLAG_INFO_POINTER } +#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&copter.v, {group_info : class::var_info} } + +#if FRAME_CONFIG == HELI_FRAME +// 6 here is AP_Motors::MOTOR_FRAME_HELI +#define DEFAULT_FRAME_CLASS 6 +#else +#define DEFAULT_FRAME_CLASS 0 +#endif + +const AP_Param::Info Copter::var_info[] = { + // @Param: SYSID_SW_MREV + // @DisplayName: Eeprom format version number + // @Description: This value is incremented when changes are made to the eeprom format + // @User: Advanced + // @ReadOnly: True + GSCALAR(format_version, "SYSID_SW_MREV", 0), + + // @Param: BOARD_ID + // @DisplayName: Board ID + // @Description: For mark board id + // @User: @Brown + GSCALAR(sysid_board_id, "SYSID_BOARD_ID", 100), + + // @Param: SYSID_BD_NAME1 + // @DisplayName: Board Name 1st + // @Description: For mark board name first + // @Range: 1-4294967269 + // @User: @Binsir + GSCALAR(sysid_board_name_1st, "SYSID_BDNAME_P1", 12848 ), + + // @Param: SYSID_BD_NAME2 + // @DisplayName: Board Name 2nd + // @Description: For mark board name last + // @User: @Binsir + GSCALAR(sysid_board_name_2nd, "SYSID_BDNAME_P2", 5329970), + + // @Param: SYSID_THISMAV + // @DisplayName: MAVLink system ID of this vehicle + // @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network + // @Range: 1 255 + // @User: Advanced + GSCALAR(sysid_this_mav, "SYSID_THISMAV", MAV_SYSTEM_ID), + + // @Param: SYSID_MYGCS + // @DisplayName: My ground station number + // @Description: Allows restricting radio overrides to only come from my ground station + // @Values: 255:Mission Planner and DroidPlanner, 252: AP Planner 2 + // @User: Advanced + GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), + + GSCALAR(sysid_dead_line, "SYSID_DEADLINE", 401221), + + // @Param: PILOT_THR_FILT + // @DisplayName: Throttle filter cutoff + // @Description: Throttle filter cutoff (Hz) - active whenever altitude control is inactive - 0 to disable + // @User: Advanced + // @Units: Hz + // @Range: 0 10 + // @Increment: .5 + GSCALAR(throttle_filt, "PILOT_THR_FILT", 0), + + // @Param: PILOT_TKOFF_ALT + // @DisplayName: Pilot takeoff altitude + // @Description: Altitude that altitude control modes will climb to when a takeoff is triggered with the throttle stick. + // @User: Standard + // @Units: cm + // @Range: 0.0 1000.0 + // @Increment: 10 + GSCALAR(pilot_takeoff_alt, "PILOT_TKOFF_ALT", PILOT_TKOFF_ALT_DEFAULT), + + // @Param: PILOT_THR_BHV + // @DisplayName: Throttle stick behavior + // @Description: Bitmask containing various throttle stick options. TX with sprung throttle can set PILOT_THR_BHV to "1" so motor feedback when landed starts from mid-stick instead of bottom of stick. + // @User: Standard + // @Values: 0:None,1:Feedback from mid stick,2:High throttle cancels landing,4:Disarm on land detection + // @Bitmask: 0:Feedback from mid stick,1:High throttle cancels landing,2:Disarm on land detection + GSCALAR(throttle_behavior, "PILOT_THR_BHV", 0), + + // @Group: SERIAL + // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp + GOBJECT(serial_manager, "SERIAL", AP_SerialManager), + + // @Param: TELEM_DELAY + // @DisplayName: Telemetry startup delay + // @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up + // @User: Advanced + // @Units: s + // @Range: 0 30 + // @Increment: 1 + GSCALAR(telem_delay, "TELEM_DELAY", 0), + + // @Param: GCS_PID_MASK + // @DisplayName: GCS PID tuning mask + // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for + // @User: Advanced + // @Values: 0:None,1:Roll,2:Pitch,4:Yaw,8:AccelZ + // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:AccelZ + GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), + +#if MODE_RTL_ENABLED == ENABLED + // @Param: RTL_ALT + // @DisplayName: RTL Altitude + // @Description: The minimum alt above home the vehicle will climb to before returning. If the vehicle is flying higher than this value it will return at its current altitude. + // @Units: cm + // @Range: 200 8000 + // @Increment: 1 + // @User: Standard + GSCALAR(rtl_altitude, "RTL_ALT", RTL_ALT), + + // @Param: RTL_CONE_SLOPE + // @DisplayName: RTL cone slope + // @Description: Defines a cone above home which determines maximum climb + // @Range: 0.5 10.0 + // @Increment: .1 + // @Values: 0:Disabled,1:Shallow,3:Steep + // @User: Standard + GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE_DEFAULT), + + // @Param: RTL_SPEED + // @DisplayName: RTL speed + // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WPNAV_SPEED will be used instead. + // @Units: cm/s + // @Range: 0 2000 + // @Increment: 50 + // @User: Standard + GSCALAR(rtl_speed_cms, "RTL_SPEED", 0), + + // @Param: RTL_ALT_FINAL + // @DisplayName: RTL Final Altitude + // @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission. Set to zero to land. + // @Units: cm + // @Range: -1 1000 + // @Increment: 1 + // @User: Standard + GSCALAR(rtl_alt_final, "RTL_ALT_FINAL", RTL_ALT_FINAL), + + // @Param: RTL_CLIMB_MIN + // @DisplayName: RTL minimum climb + // @Description: The vehicle will climb this many cm during the initial climb portion of the RTL + // @Units: cm + // @Range: 0 3000 + // @Increment: 10 + // @User: Standard + GSCALAR(rtl_climb_min, "RTL_CLIMB_MIN", RTL_CLIMB_MIN_DEFAULT), + + // @Param: RTL_LOIT_TIME + // @DisplayName: RTL loiter time + // @Description: Time (in milliseconds) to loiter above home before beginning final descent + // @Units: ms + // @Range: 0 60000 + // @Increment: 1000 + // @User: Standard + GSCALAR(rtl_loiter_time, "RTL_LOIT_TIME", RTL_LOITER_TIME), +#endif + +#if RANGEFINDER_ENABLED == ENABLED + // @Param: RNGFND_GAIN + // @DisplayName: Rangefinder gain + // @Description: Used to adjust the speed with which the target altitude is changed when objects are sensed below the copter + // @Range: 0.01 2.0 + // @Increment: 0.01 + // @User: Standard + GSCALAR(rangefinder_gain, "RNGFND_GAIN", RANGEFINDER_GAIN_DEFAULT), +#endif + + // @Param: FS_GCS_ENABLE + // @DisplayName: Ground Station Failsafe Enable + // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. NB. The GCS Failsafe is only active when RC_OVERRIDE is being used to control the vehicle. + // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Deprecated in 4.0+),3:Enabled always SmartRTL or RTL,4:Enabled always SmartRTL or Land,5:Enabled always land (4.0+ Only) + // @User: Standard + GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED), + + // @Param: GPS_HDOP_GOOD + // @DisplayName: GPS Hdop Good + // @Description: GPS Hdop value at or below this value represent a good position. Used for pre-arm checks + // @Range: 100 900 + // @User: Advanced + GSCALAR(gps_hdop_good, "GPS_HDOP_GOOD", GPS_HDOP_GOOD_DEFAULT), + + // @Param: SUPER_SIMPLE + // @DisplayName: Super Simple Mode + // @Description: Bitmask to enable Super Simple mode for some flight modes. Setting this to Disabled(0) will disable Super Simple Mode + // @Values: 0:Disabled,1:Mode1,2:Mode2,3:Mode1+2,4:Mode3,5:Mode1+3,6:Mode2+3,7:Mode1+2+3,8:Mode4,9:Mode1+4,10:Mode2+4,11:Mode1+2+4,12:Mode3+4,13:Mode1+3+4,14:Mode2+3+4,15:Mode1+2+3+4,16:Mode5,17:Mode1+5,18:Mode2+5,19:Mode1+2+5,20:Mode3+5,21:Mode1+3+5,22:Mode2+3+5,23:Mode1+2+3+5,24:Mode4+5,25:Mode1+4+5,26:Mode2+4+5,27:Mode1+2+4+5,28:Mode3+4+5,29:Mode1+3+4+5,30:Mode2+3+4+5,31:Mode1+2+3+4+5,32:Mode6,33:Mode1+6,34:Mode2+6,35:Mode1+2+6,36:Mode3+6,37:Mode1+3+6,38:Mode2+3+6,39:Mode1+2+3+6,40:Mode4+6,41:Mode1+4+6,42:Mode2+4+6,43:Mode1+2+4+6,44:Mode3+4+6,45:Mode1+3+4+6,46:Mode2+3+4+6,47:Mode1+2+3+4+6,48:Mode5+6,49:Mode1+5+6,50:Mode2+5+6,51:Mode1+2+5+6,52:Mode3+5+6,53:Mode1+3+5+6,54:Mode2+3+5+6,55:Mode1+2+3+5+6,56:Mode4+5+6,57:Mode1+4+5+6,58:Mode2+4+5+6,59:Mode1+2+4+5+6,60:Mode3+4+5+6,61:Mode1+3+4+5+6,62:Mode2+3+4+5+6,63:Mode1+2+3+4+5+6 + // @User: Standard + GSCALAR(super_simple, "SUPER_SIMPLE", 0), + + // @Param: WP_YAW_BEHAVIOR + // @DisplayName: Yaw behaviour during missions + // @Description: Determines how the autopilot controls the yaw during missions and RTL + // @Values: 0:Never change yaw, 1:Face next waypoint, 2:Face next waypoint except RTL, 3:Face along GPS course + // @User: Standard + GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_DEFAULT), + + // @Param: LAND_SPEED + // @DisplayName: Land speed + // @Description: The descent speed for the final stage of landing in cm/s + // @Units: cm/s + // @Range: 30 200 + // @Increment: 10 + // @User: Standard + GSCALAR(land_speed, "LAND_SPEED", LAND_SPEED), + + // @Param: LAND_SPEED_HIGH + // @DisplayName: Land speed high + // @Description: The descent speed for the first stage of landing in cm/s. If this is zero then WPNAV_SPEED_DN is used + // @Units: cm/s + // @Range: 0 500 + // @Increment: 10 + // @User: Standard + GSCALAR(land_speed_high, "LAND_SPEED_HIGH", 0), + + // @Param: PILOT_SPEED_UP + // @DisplayName: Pilot maximum vertical speed ascending + // @Description: The maximum vertical ascending velocity the pilot may request in cm/s + // @Units: cm/s + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + GSCALAR(pilot_speed_up, "PILOT_SPEED_UP", PILOT_VELZ_MAX), + + // @Param: PILOT_ACCEL_Z + // @DisplayName: Pilot vertical acceleration + // @Description: The vertical acceleration used when pilot is controlling the altitude + // @Units: cm/s/s + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + GSCALAR(pilot_accel_z, "PILOT_ACCEL_Z", PILOT_ACCEL_Z_DEFAULT), + + // @Param: FS_THR_ENABLE + // @DisplayName: Throttle Failsafe Enable + // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel + // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Deprecated in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land + // @User: Standard + GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL), + + // @Param: FS_THR_VALUE + // @DisplayName: Throttle Failsafe Value + // @Description: The PWM level in microseconds on channel 3 below which throttle failsafe triggers + // @Range: 925 1100 + // @Units: PWM + // @Increment: 1 + // @User: Standard + GSCALAR(failsafe_throttle_value, "FS_THR_VALUE", FS_THR_VALUE_DEFAULT), + + // @Param: THR_DZ + // @DisplayName: Throttle deadzone + // @Description: The deadzone above and below mid throttle in PWM microseconds. Used in AltHold, Loiter, PosHold flight modes + // @User: Standard + // @Range: 0 300 + // @Units: PWM + // @Increment: 1 + GSCALAR(throttle_deadzone, "THR_DZ", THR_DZ_DEFAULT), + + // @Param: FLTMODE1 + // @DisplayName: Flight Mode 1 + // @Description: Flight mode when Channel 5 pwm is <= 1230 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate + // @User: Standard + GSCALAR(flight_mode1, "FLTMODE1", (uint8_t)FLIGHT_MODE_1), + + // @Param: FLTMODE2 + // @DisplayName: Flight Mode 2 + // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate + // @User: Standard + GSCALAR(flight_mode2, "FLTMODE2", (uint8_t)FLIGHT_MODE_2), + + // @Param: FLTMODE3 + // @DisplayName: Flight Mode 3 + // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate + // @User: Standard + GSCALAR(flight_mode3, "FLTMODE3", (uint8_t)FLIGHT_MODE_3), + + // @Param: FLTMODE4 + // @DisplayName: Flight Mode 4 + // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate + // @User: Standard + GSCALAR(flight_mode4, "FLTMODE4", (uint8_t)FLIGHT_MODE_4), + + // @Param: FLTMODE5 + // @DisplayName: Flight Mode 5 + // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate + // @User: Standard + GSCALAR(flight_mode5, "FLTMODE5", (uint8_t)FLIGHT_MODE_5), + + // @Param: FLTMODE6 + // @DisplayName: Flight Mode 6 + // @Description: Flight mode when Channel 5 pwm is >=1750 + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate + // @User: Standard + GSCALAR(flight_mode6, "FLTMODE6", (uint8_t)FLIGHT_MODE_6), + + // @Param: FLTMODE_CH + // @DisplayName: Flightmode channel + // @Description: RC Channel to use for flight mode control + // @Values: 0:Disabled,5:Channel5,6:Channel6,7:Channel7,8:Channel8 + // @User: Advanced + GSCALAR(flight_mode_chan, "FLTMODE_CH", CH_MODE_DEFAULT), + + // @Param: SIMPLE + // @DisplayName: Simple mode bitmask + // @Description: Bitmask which holds which flight modes use simple heading mode (eg bit 0 = 1 means Flight Mode 0 uses simple mode) + // @User: Advanced + GSCALAR(simple_modes, "SIMPLE", 0), + + // @Param: LOG_BITMASK + // @DisplayName: Log bitmask + // @Description: 4 byte bitmap of log types to enable + // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll-AC315,45054:NearlyAll,131071:All+FastATT,262142:All+MotBatt,393214:All+FastIMU,397310:All+FastIMU+PID,655358:All+FullIMU,0:Disabled + // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:RCIN,7:IMU,8:CMD,9:CURRENT,10:RCOUT,11:OPTFLOW,12:PID,13:COMPASS,14:INAV,15:CAMERA,17:MOTBATT,18:IMU_FAST,19:IMU_RAW + // @User: Standard + GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), + + // @Param: ESC_CALIBRATION + // @DisplayName: ESC Calibration + // @Description: Controls whether ArduCopter will enter ESC calibration on the next restart. Do not adjust this parameter manually. + // @User: Advanced + // @Values: 0:Normal Start-up, 1:Start-up in ESC Calibration mode if throttle high, 2:Start-up in ESC Calibration mode regardless of throttle, 3:Start-up and automatically calibrate ESCs, 9:Disabled + GSCALAR(esc_calibrate, "ESC_CALIBRATION", 0), + + // @Param: TUNE + // @DisplayName: Channel 6 Tuning + // @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob + // @User: Standard + // @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,57:Winch,58:SysID Magnitude + GSCALAR(radio_tuning, "TUNE", 0), + + // @Param: FRAME_TYPE + // @DisplayName: Frame Type (+, X, V, etc) + // @Description: Controls motor mixing for multicopters. Not used for Tri or Traditional Helicopters. + // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B, 11:Y6F, 12:BetaFlightX, 13:DJIX, 14:ClockwiseX, 15: I + // @User: Standard + // @RebootRequired: True + GSCALAR(frame_type, "FRAME_TYPE", HAL_FRAME_TYPE_DEFAULT), + + // @Group: ARMING_ + // @Path: ../libraries/AP_Arming/AP_Arming.cpp + GOBJECT(arming, "ARMING_", AP_Arming_Copter), + + // @Param: DISARM_DELAY + // @DisplayName: Disarm delay + // @Description: Delay before automatic disarm in seconds. A value of zero disables auto disarm. + // @Units: s + // @Range: 0 127 + // @User: Advanced + GSCALAR(disarm_delay, "DISARM_DELAY", AUTO_DISARMING_DELAY), + + // @Param: ANGLE_MAX + // @DisplayName: Angle Max + // @Description: Maximum lean angle in all flight modes + // @Units: cdeg + // @Range: 1000 8000 + // @User: Advanced + ASCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX), + + // @Param: PHLD_BRAKE_RATE + // @DisplayName: PosHold braking rate + // @Description: PosHold flight mode's rotation rate during braking in deg/sec + // @Units: deg/s + // @Range: 4 12 + // @User: Advanced + GSCALAR(poshold_brake_rate, "PHLD_BRAKE_RATE", POSHOLD_BRAKE_RATE_DEFAULT), + + // @Param: PHLD_BRAKE_ANGLE + // @DisplayName: PosHold braking angle max + // @Description: PosHold flight mode's max lean angle during braking in centi-degrees + // @Units: cdeg + // @Range: 2000 4500 + // @User: Advanced + GSCALAR(poshold_brake_angle_max, "PHLD_BRAKE_ANGLE", POSHOLD_BRAKE_ANGLE_DEFAULT), + + // @Param: LAND_REPOSITION + // @DisplayName: Land repositioning + // @Description: Enables user input during LAND mode, the landing phase of RTL, and auto mode landings. + // @Values: 0:No repositioning, 1:Repositioning + // @User: Advanced + GSCALAR(land_repositioning, "LAND_REPOSITION", LAND_REPOSITION_DEFAULT), + + // @Param: FS_EKF_ACTION + // @DisplayName: EKF Failsafe Action + // @Description: Controls the action that will be taken when an EKF failsafe is invoked + // @Values: 1:Land, 2:AltHold, 3:Land even in Stabilize + // @User: Advanced + GSCALAR(fs_ekf_action, "FS_EKF_ACTION", FS_EKF_ACTION_DEFAULT), + + // @Param: FS_EKF_THRESH + // @DisplayName: EKF failsafe variance threshold + // @Description: Allows setting the maximum acceptable compass and velocity variance + // @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed + // @User: Advanced + GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", FS_EKF_THRESHOLD_DEFAULT), + + // @Param: FS_CRASH_CHECK + // @DisplayName: Crash check enable + // @Description: This enables automatic crash checking. When enabled the motors will disarm if a crash is detected. + // @Values: 0:Disabled, 1:Enabled + // @User: Advanced + GSCALAR(fs_crash_check, "FS_CRASH_CHECK", 1), + + // @Param: RC_SPEED + // @DisplayName: ESC Update Speed + // @Description: This is the speed in Hertz that your ESCs will receive updates + // @Units: Hz + // @Range: 50 490 + // @Increment: 1 + // @User: Advanced + GSCALAR(rc_speed, "RC_SPEED", RC_FAST_SPEED), + + // @Param: ACRO_RP_P + // @DisplayName: Acro Roll and Pitch P gain + // @Description: Converts pilot roll and pitch into a desired rate of rotation in ACRO and SPORT mode. Higher values mean faster rate of rotation. + // @Range: 1 10 + // @User: Standard + GSCALAR(acro_rp_p, "ACRO_RP_P", ACRO_RP_P), + + // @Param: ACRO_YAW_P + // @DisplayName: Acro Yaw P gain + // @Description: Converts pilot yaw input into a desired rate of rotation. Higher values mean faster rate of rotation. + // @Range: 1 10 + // @User: Standard + GSCALAR(acro_yaw_p, "ACRO_YAW_P", ACRO_YAW_P), + +#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED + // @Param: ACRO_BAL_ROLL + // @DisplayName: Acro Balance Roll + // @Description: rate at which roll angle returns to level in acro and sport mode. A higher value causes the vehicle to return to level faster. For helicopter sets the decay rate of the virtual flybar in the roll axis. A higher value causes faster decay of desired to actual attitude. + // @Range: 0 3 + // @Increment: 0.1 + // @User: Advanced + GSCALAR(acro_balance_roll, "ACRO_BAL_ROLL", ACRO_BALANCE_ROLL), + + // @Param: ACRO_BAL_PITCH + // @DisplayName: Acro Balance Pitch + // @Description: rate at which pitch angle returns to level in acro and sport mode. A higher value causes the vehicle to return to level faster. For helicopter sets the decay rate of the virtual flybar in the pitch axis. A higher value causes faster decay of desired to actual attitude. + // @Range: 0 3 + // @Increment: 0.1 + // @User: Advanced + GSCALAR(acro_balance_pitch, "ACRO_BAL_PITCH", ACRO_BALANCE_PITCH), +#endif + +#if MODE_ACRO_ENABLED == ENABLED + // @Param: ACRO_TRAINER + // @DisplayName: Acro Trainer + // @Description: Type of trainer used in acro mode + // @Values: 0:Disabled,1:Leveling,2:Leveling and Limited + // @User: Advanced + GSCALAR(acro_trainer, "ACRO_TRAINER", ACRO_TRAINER_LIMITED), + + // @Param: ACRO_RP_EXPO + // @DisplayName: Acro Roll/Pitch Expo + // @Description: Acro roll/pitch Expo to allow faster rotation when stick at edges + // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High + // @Range: -0.5 1.0 + // @User: Advanced + GSCALAR(acro_rp_expo, "ACRO_RP_EXPO", ACRO_RP_EXPO_DEFAULT), +#endif + + // variables not in the g class which contain EEPROM saved variables + +#if CAMERA == ENABLED + // @Group: CAM_ + // @Path: ../libraries/AP_Camera/AP_Camera.cpp + GOBJECT(camera, "CAM_", AP_Camera), +#endif + + // @Group: RELAY_ + // @Path: ../libraries/AP_Relay/AP_Relay.cpp + GOBJECT(relay, "RELAY_", AP_Relay), + +#if PARACHUTE == ENABLED + // @Group: CHUTE_ + // @Path: ../libraries/AP_Parachute/AP_Parachute.cpp + GOBJECT(parachute, "CHUTE_", AP_Parachute), +#endif + + // @Group: LGR_ + // @Path: ../libraries/AP_LandingGear/AP_LandingGear.cpp + GOBJECT(landinggear, "LGR_", AP_LandingGear), + +#if FRAME_CONFIG == HELI_FRAME + // @Group: IM_ + // @Path: ../libraries/AC_InputManager/AC_InputManager_Heli.cpp + GOBJECT(input_manager, "IM_", AC_InputManager_Heli), +#endif + + // @Group: COMPASS_ + // @Path: ../libraries/AP_Compass/AP_Compass.cpp + GOBJECT(compass, "COMPASS_", Compass), + + // @Group: INS_ + // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp + GOBJECT(ins, "INS_", AP_InertialSensor), + + // @Group: WPNAV_ + // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp + GOBJECTPTR(wp_nav, "WPNAV_", AC_WPNav), + + // @Group: LOIT_ + // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp + GOBJECTPTR(loiter_nav, "LOIT_", AC_Loiter), + +#if MODE_CIRCLE_ENABLED == ENABLED + // @Group: CIRCLE_ + // @Path: ../libraries/AC_WPNav/AC_Circle.cpp + GOBJECTPTR(circle_nav, "CIRCLE_", AC_Circle), +#endif + + // @Group: ATC_ + // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +#if FRAME_CONFIG == HELI_FRAME + GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Heli), +#else + GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Multi), +#endif + + // @Group: PSC + // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp + GOBJECTPTR(pos_control, "PSC", AC_PosControl), + + // @Group: SR0_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(_gcs.chan_parameters[0], gcs0, "SR0_", GCS_MAVLINK_Parameters), + + // @Group: SR1_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(_gcs.chan_parameters[1], gcs1, "SR1_", GCS_MAVLINK_Parameters), + + // @Group: SR2_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(_gcs.chan_parameters[2], gcs2, "SR2_", GCS_MAVLINK_Parameters), + + // @Group: SR3_ + // @Path: GCS_Mavlink.cpp + GOBJECTN(_gcs.chan_parameters[3], gcs3, "SR3_", GCS_MAVLINK_Parameters), + + // @Group: AHRS_ + // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp + GOBJECT(ahrs, "AHRS_", AP_AHRS), + +#if MOUNT == ENABLED + // @Group: MNT + // @Path: ../libraries/AP_Mount/AP_Mount.cpp + GOBJECT(camera_mount, "MNT", AP_Mount), +#endif + + // @Group: LOG + // @Path: ../libraries/AP_Logger/AP_Logger.cpp + GOBJECT(logger, "LOG", AP_Logger), + + // @Group: BATT + // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp + GOBJECT(battery, "BATT", AP_BattMonitor), + + // @Group: BRD_ + // @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp + GOBJECT(BoardConfig, "BRD_", AP_BoardConfig), + +#if HAL_WITH_UAVCAN + // @Group: CAN_ + // @Path: ../libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp + GOBJECT(BoardConfig_CAN, "CAN_", AP_BoardConfig_CAN), +#endif + +#if SPRAYER_ENABLED == ENABLED + // @Group: SPRAY_ + // @Path: ../libraries/AC_Sprayer/AC_Sprayer.cpp + GOBJECT(sprayer, "SPRAY_", AC_Sprayer), +#endif + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + GOBJECT(sitl, "SIM_", SITL::SITL), +#endif + + // @Group: GND_ + // @Path: ../libraries/AP_Baro/AP_Baro.cpp + GOBJECT(barometer, "GND_", AP_Baro), + + // GPS driver + // @Group: GPS_ + // @Path: ../libraries/AP_GPS/AP_GPS.cpp + GOBJECT(gps, "GPS_", AP_GPS), + + // @Group: SCHED_ + // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp + GOBJECT(scheduler, "SCHED_", AP_Scheduler), + +#if AC_FENCE == ENABLED + // @Group: FENCE_ + // @Path: ../libraries/AC_Fence/AC_Fence.cpp + GOBJECT(fence, "FENCE_", AC_Fence), +#endif + + // @Group: AVOID_ + // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp +#if AC_AVOID_ENABLED == ENABLED + GOBJECT(avoid, "AVOID_", AC_Avoid), +#endif + +#if AC_RALLY == ENABLED + // @Group: RALLY_ + // @Path: AP_Rally.cpp,../libraries/AP_Rally/AP_Rally.cpp + GOBJECT(rally, "RALLY_", AP_Rally_Copter), +#endif + +#if FRAME_CONFIG == HELI_FRAME + // @Group: H_ + // @Path: ../libraries/AP_Motors/AP_MotorsHeli_Single.cpp,../libraries/AP_Motors/AP_MotorsHeli_Dual.cpp,../libraries/AP_Motors/AP_MotorsHeli.cpp + GOBJECTVARPTR(motors, "H_", &copter.motors_var_info), +#else + // @Group: MOT_ + // @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp + GOBJECTVARPTR(motors, "MOT_", &copter.motors_var_info), +#endif + + // @Group: RCMAP_ + // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp + GOBJECT(rcmap, "RCMAP_", RCMapper), + + // @Group: EK2_ + // @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp + GOBJECTN(EKF2, NavEKF2, "EK2_", NavEKF2), + + // @Group: EK3_ + // @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp + GOBJECTN(EKF3, NavEKF3, "EK3_", NavEKF3), + +#if MODE_AUTO_ENABLED == ENABLED + // @Group: MIS_ + // @Path: ../libraries/AP_Mission/AP_Mission.cpp + GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission), +#endif + + // @Group: RSSI_ + // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp + GOBJECT(rssi, "RSSI_", AP_RSSI), + +#if RANGEFINDER_ENABLED == ENABLED + // @Group: RNGFND + // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp + GOBJECT(rangefinder, "RNGFND", RangeFinder), +#endif + +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + // @Group: TERRAIN_ + // @Path: ../libraries/AP_Terrain/AP_Terrain.cpp + GOBJECT(terrain, "TERRAIN_", AP_Terrain), +#endif + +#if OPTFLOW == ENABLED + // @Group: FLOW + // @Path: ../libraries/AP_OpticalFlow/OpticalFlow.cpp + GOBJECT(optflow, "FLOW", OpticalFlow), +#endif + +#if PRECISION_LANDING == ENABLED + // @Group: PLND_ + // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp + GOBJECT(precland, "PLND_", AC_PrecLand), +#endif + +#if RPM_ENABLED == ENABLED + // @Group: RPM + // @Path: ../libraries/AP_RPM/AP_RPM.cpp + GOBJECT(rpm_sensor, "RPM", AP_RPM), +#endif + +#if ADSB_ENABLED == ENABLED + // @Group: ADSB_ + // @Path: ../libraries/AP_ADSB/AP_ADSB.cpp + GOBJECT(adsb, "ADSB_", AP_ADSB), + + // @Group: AVD_ + // @Path: ../libraries/AP_Avoidance/AP_Avoidance.cpp + GOBJECT(avoidance_adsb, "AVD_", AP_Avoidance_Copter), +#endif + + // @Group: NTF_ + // @Path: ../libraries/AP_Notify/AP_Notify.cpp + GOBJECT(notify, "NTF_", AP_Notify), + +#if MODE_THROW_ENABLED == ENABLED + // @Param: THROW_MOT_START + // @DisplayName: Start motors before throwing is detected + // @Description: Used by THROW mode. Controls whether motors will run at the speed set by THR_MIN or will be stopped when armed and waiting for the throw. + // @Values: 0:Stopped,1:Running + // @User: Standard + GSCALAR(throw_motor_start, "THROW_MOT_START", 0), +#endif + +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + // @Param: TERRAIN_FOLLOW + // @DisplayName: Terrain Following use control + // @Description: This enables terrain following for RTL and LAND flight modes. To use this option TERRAIN_ENABLE must be 1 and the GCS must support sending terrain data to the aircraft. In RTL the RTL_ALT will be considered a height above the terrain. In LAND mode the vehicle will slow to LAND_SPEED 10m above terrain (instead of 10m above home). This parameter does not affect AUTO and Guided which use a per-command flag to determine if the height is above-home, absolute or above-terrain. + // @Values: 0:Do Not Use in RTL and Land,1:Use in RTL and Land + // @User: Standard + GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0), +#endif + +#if OSD_ENABLED == ENABLED + // @Group: OSD + // @Path: ../libraries/AP_OSD/AP_OSD.cpp + GOBJECT(osd, "OSD", AP_OSD), +#endif + + // @Group: + // @Path: Parameters.cpp + GOBJECT(g2, "", ParametersG2), + + AP_VAREND +}; + +/* + 2nd group of parameters + */ +const AP_Param::GroupInfo ParametersG2::var_info[] = { + + // @Param: WP_NAVALT_MIN + // @DisplayName: Minimum navigation altitude + // @Description: This is the altitude in meters above which for navigation can begin. This applies in auto takeoff and auto landing. + // @Range: 0 5 + // @User: Standard + AP_GROUPINFO("WP_NAVALT_MIN", 1, ParametersG2, wp_navalt_min, 0), + +#if BUTTON_ENABLED == ENABLED + // @Group: BTN_ + // @Path: ../libraries/AP_Button/AP_Button.cpp + AP_SUBGROUPINFO(button, "BTN_", 2, ParametersG2, AP_Button), +#endif + +#if MODE_THROW_ENABLED == ENABLED + // @Param: THROW_NEXTMODE + // @DisplayName: Throw mode's follow up mode + // @Description: Vehicle will switch to this mode after the throw is successfully completed. Default is to stay in throw mode (18) + // @Values: 3:Auto,4:Guided,5:LOITER,6:RTL,9:Land,17:Brake,18:Throw + // @User: Standard + AP_GROUPINFO("THROW_NEXTMODE", 3, ParametersG2, throw_nextmode, 18), + + // @Param: THROW_TYPE + // @DisplayName: Type of Type + // @Description: Used by THROW mode. Specifies whether Copter is thrown upward or dropped. + // @Values: 0:Upward Throw,1:Drop + // @User: Standard + AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, ModeThrow::ThrowType_Upward), +#endif + + // @Param: GND_EFFECT_COMP + // @DisplayName: Ground Effect Compensation Enable/Disable + // @Description: Ground Effect Compensation Enable/Disable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("GND_EFFECT_COMP", 5, ParametersG2, gndeffect_comp_enabled, 1), + +#if ADVANCED_FAILSAFE == ENABLED + // @Group: AFS_ + // @Path: ../libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp + AP_SUBGROUPINFO(afs, "AFS_", 6, ParametersG2, AP_AdvancedFailsafe), +#endif + + // @Param: DEV_OPTIONS + // @DisplayName: Development options + // @Description: Bitmask of developer options. The meanings of the bit fields in this parameter may vary at any time. Developers should check the source code for current meaning + // @Bitmask: 0:ADSBMavlinkProcessing,1:DevOptionVFR_HUDRelativeAlt + // @User: Advanced + AP_GROUPINFO("DEV_OPTIONS", 7, ParametersG2, dev_options, 0), + +#if BEACON_ENABLED == ENABLED + // @Group: BCN + // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp + AP_SUBGROUPINFO(beacon, "BCN", 14, ParametersG2, AP_Beacon), +#endif + +#if PROXIMITY_ENABLED == ENABLED + // @Group: PRX + // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp + AP_SUBGROUPINFO(proximity, "PRX", 8, ParametersG2, AP_Proximity), +#endif + + // @Param: ACRO_Y_EXPO + // @DisplayName: Acro Yaw Expo + // @Description: Acro yaw expo to allow faster rotation when stick at edges + // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High + // @Range: -0.5 1.0 + // @User: Advanced + AP_GROUPINFO("ACRO_Y_EXPO", 9, ParametersG2, acro_y_expo, ACRO_Y_EXPO_DEFAULT), + +#if MODE_ACRO_ENABLED == ENABLED + // @Param: ACRO_THR_MID + // @DisplayName: Acro Thr Mid + // @Description: Acro Throttle Mid + // @Range: 0 1 + // @User: Advanced + AP_GROUPINFO("ACRO_THR_MID", 10, ParametersG2, acro_thr_mid, ACRO_THR_MID_DEFAULT), +#endif + + // @Param: SYSID_ENFORCE + // @DisplayName: GCS sysid enforcement + // @Description: This controls whether packets from other than the expected GCS system ID will be accepted + // @Values: 0:NotEnforced,1:Enforced + // @User: Advanced + AP_GROUPINFO("SYSID_ENFORCE", 11, ParametersG2, sysid_enforce, 0), + +#if STATS_ENABLED == ENABLED + // @Group: STAT + // @Path: ../libraries/AP_Stats/AP_Stats.cpp + AP_SUBGROUPINFO(stats, "STAT", 12, ParametersG2, AP_Stats), +#endif + +#if GRIPPER_ENABLED == ENABLED + // @Group: GRIP_ + // @Path: ../libraries/AP_Gripper/AP_Gripper.cpp + AP_SUBGROUPINFO(gripper, "GRIP_", 13, ParametersG2, AP_Gripper), +#endif + + // @Param: FRAME_CLASS + // @DisplayName: Frame Class + // @Description: Controls major frame class for multicopter component + // @Values: 0:Undefined, 1:Quad, 2:Hexa, 3:Octa, 4:OctaQuad, 5:Y6, 6:Heli, 7:Tri, 8:SingleCopter, 9:CoaxCopter, 10:BiCopter, 11:Heli_Dual, 12:DodecaHexa, 13:HeliQuad + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO("FRAME_CLASS", 15, ParametersG2, frame_class, DEFAULT_FRAME_CLASS), + + // @Group: SERVO + // @Path: ../libraries/SRV_Channel/SRV_Channels.cpp + AP_SUBGROUPINFO(servo_channels, "SERVO", 16, ParametersG2, SRV_Channels), + + // @Group: RC + // @Path: ../libraries/RC_Channel/RC_Channels_VarInfo.h + AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels_Copter), + +#if VISUAL_ODOMETRY_ENABLED == ENABLED + // @Group: VISO + // @Path: ../libraries/AP_VisualOdom/AP_VisualOdom.cpp + AP_SUBGROUPINFO(visual_odom, "VISO", 18, ParametersG2, AP_VisualOdom), +#endif + + // @Group: TCAL + // @Path: ../libraries/AP_TempCalibration/AP_TempCalibration.cpp + AP_SUBGROUPINFO(temp_calibration, "TCAL", 19, ParametersG2, AP_TempCalibration), + +#if TOY_MODE_ENABLED == ENABLED + // @Group: TMODE + // @Path: toy_mode.cpp + AP_SUBGROUPINFO(toy_mode, "TMODE", 20, ParametersG2, ToyMode), +#endif + +#if MODE_SMARTRTL_ENABLED == ENABLED + // @Group: SRTL_ + // @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp + AP_SUBGROUPINFO(smart_rtl, "SRTL_", 21, ParametersG2, AP_SmartRTL), +#endif + +#if WINCH_ENABLED == ENABLED + // @Group: WENC + // @Path: ../libraries/AP_WheelEncoder/AP_WheelEncoder.cpp + AP_SUBGROUPINFO(wheel_encoder, "WENC", 22, ParametersG2, AP_WheelEncoder), + + // @Group: WINCH_ + // @Path: ../libraries/AP_Winch/AP_Winch.cpp + AP_SUBGROUPINFO(winch, "WINCH", 23, ParametersG2, AP_Winch), +#endif + + // @Param: PILOT_SPEED_DN + // @DisplayName: Pilot maximum vertical speed descending + // @Description: The maximum vertical descending velocity the pilot may request in cm/s + // @Units: cm/s + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("PILOT_SPEED_DN", 24, ParametersG2, pilot_speed_dn, 0), + + // @Param: LAND_ALT_LOW + // @DisplayName: Land alt low + // @Description: Altitude during Landing at which vehicle slows to LAND_SPEED + // @Units: cm + // @Range: 100 10000 + // @Increment: 10 + // @User: Advanced + AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low, 1000), + +#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED + // @Group: FHLD + // @Path: mode_flowhold.cpp + AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, ModeFlowHold), +#endif + +#if MODE_FOLLOW_ENABLED == ENABLED + // @Group: FOLL + // @Path: ../libraries/AP_Follow/AP_Follow.cpp + AP_SUBGROUPINFO(follow, "FOLL", 27, ParametersG2, AP_Follow), +#endif + +#ifdef USER_PARAMS_ENABLED + AP_SUBGROUPINFO(user_parameters, "USR", 28, ParametersG2, UserParameters), +#endif + +#if AUTOTUNE_ENABLED == ENABLED + // @Group: AUTOTUNE_ + // @Path: ../libraries/AC_AutoTune/AC_AutoTune.cpp + AP_SUBGROUPPTR(autotune_ptr, "AUTOTUNE_", 29, ParametersG2, AutoTune), +#endif + +#ifdef ENABLE_SCRIPTING + // @Group: SCR_ + // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp + AP_SUBGROUPINFO(scripting, "SCR_", 30, ParametersG2, AP_Scripting), +#endif + + // @Param: TUNE_MIN + // @DisplayName: Tuning minimum + // @Description: Minimum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to + // @User: Standard + AP_GROUPINFO("TUNE_MIN", 31, ParametersG2, tuning_min, 0), + + // @Param: TUNE_MAX + // @DisplayName: Tuning maximum + // @Description: Maximum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to + // @User: Standard + AP_GROUPINFO("TUNE_MAX", 32, ParametersG2, tuning_max, 0), + +#if AC_OAPATHPLANNER_ENABLED == ENABLED + // @Group: OA_ + // @Path: ../libraries/AC_Avoidance/AP_OAPathPlanner.cpp + AP_SUBGROUPINFO(oa, "OA_", 33, ParametersG2, AP_OAPathPlanner), +#endif + +#if MODE_SYSTEMID_ENABLED == ENABLED + // @Group: SID + // @Path: mode_systemid.cpp + AP_SUBGROUPPTR(mode_systemid_ptr, "SID", 34, ParametersG2, ModeSystemId), +#endif + + // @Param: FS_VIBE_ENABLE + // @DisplayName: Vibration Failsafe enable + // @Description: This enables the vibration failsafe which will use modified altitude estimation and control during high vibrations + // @Values: 0:Disabled, 1:Enabled + // @User: Standard + AP_GROUPINFO("FS_VIBE_ENABLE", 35, ParametersG2, fs_vibe_enabled, 1), + + // @Param: FS_OPTIONS + // @DisplayName: Failsafe options bitmask + // @Description: Bitmask of additional options for battery, radio, & GCS failsafes. 0 (default) disables all options. + // @Values: 0:Disabled, 1:Continue if in Auto on RC failsafe only, 2:Continue if in Auto on GCS failsafe only, 3:Continue if in Auto on RC and/or GCS failsafe, 4:Continue if in Guided on RC failsafe only, 8:Continue if landing on any failsafe, 16:Continue if in pilot controlled modes on GCS failsafe, 19:Continue if in Auto on RC and/or GCS failsafe and continue if in pilot controlled modes on GCS failsafe + // @Bitmask: 0:Continue if in Auto on RC failsafe, 1:Continue if in Auto on GCS failsafe, 2:Continue if in Guided on RC failsafe, 3:Continue if landing on any failsafe, 4:Continue if in pilot controlled modes on GCS failsafe + // @User: Advanced + AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, 0), + +#if MODE_AUTOROTATE_ENABLED == ENABLED + // @Group: AROT_ + // @Path: ../libraries/AC_Autorotation/AC_Autorotation.cpp + AP_SUBGROUPINFO(arot, "AROT_", 37, ParametersG2, AC_Autorotation), +#endif + + + + AP_GROUPEND +}; + +// These param descriptions are here so that users of beta Mission Planner (which uses the master branch as its source of descriptions) +// can get them. These lines can be removed once Copter-3.7.0-beta testing begins or we improve the source of descriptions for GCSs. +// +// @Param: CH7_OPT +// @DisplayName: Channel 7 option +// @Description: Select which function is performed when CH7 is above 1800 pwm +// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl +// @User: Standard + +// @Param: CH8_OPT +// @DisplayName: Channel 8 option +// @Description: Select which function is performed when CH8 is above 1800 pwm +// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl +// @User: Standard + +// @Param: CH9_OPT +// @DisplayName: Channel 9 option +// @Description: Select which function is performed when CH9 is above 1800 pwm +// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl +// @User: Standard + +// @Param: CH10_OPT +// @DisplayName: Channel 10 option +// @Description: Select which function is performed when CH10 is above 1800 pwm +// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl +// @User: Standard + +// @Param: CH11_OPT +// @DisplayName: Channel 11 option +// @Description: Select which function is performed when CH11 is above 1800 pwm +// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl +// @User: Standard + +// @Param: CH12_OPT +// @DisplayName: Channel 12 option +// @Description: Select which function is performed when CH12 is above 1800 pwm +// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl +// @User: Standard + +// @Param: TUNE_LOW +// @DisplayName: Tuning minimum +// @Description: The minimum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob +// @User: Standard +// @Range: 0 32767 + +// @Param: TUNE_HIGH +// @DisplayName: Tuning maximum +// @Description: The maximum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob +// @User: Standard +// @Range: 0 32767 + +/* + constructor for g2 object + */ +ParametersG2::ParametersG2(void) + : temp_calibration() // this doesn't actually need constructing, but removing it here is problematic syntax-wise +#if BEACON_ENABLED == ENABLED + , beacon(copter.serial_manager) +#endif +#if PROXIMITY_ENABLED == ENABLED + , proximity() +#endif +#if ADVANCED_FAILSAFE == ENABLED + ,afs(copter.mode_auto.mission) +#endif +#if MODE_SMARTRTL_ENABLED == ENABLED + ,smart_rtl() +#endif +#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED + ,mode_flowhold_ptr(&copter.mode_flowhold) +#endif +#if MODE_FOLLOW_ENABLED == ENABLED + ,follow() +#endif +#ifdef USER_PARAMS_ENABLED + ,user_parameters() +#endif +#if AUTOTUNE_ENABLED == ENABLED + ,autotune_ptr(&copter.autotune) +#endif +#if MODE_SYSTEMID_ENABLED == ENABLED + ,mode_systemid_ptr(&copter.mode_systemid) +#endif +#if MODE_AUTOROTATE_ENABLED == ENABLED + ,arot(copter.inertial_nav) +#endif +{ + AP_Param::setup_object_defaults(this, var_info); +} + +/* + This is a conversion table from old parameter values to new + parameter names. The startup code looks for saved values of the old + parameters and will copy them across to the new parameters if the + new parameter does not yet have a saved value. It then saves the new + value. + + Note that this works even if the old parameter has been removed. It + relies on the old k_param index not being removed + + The second column below is the index in the var_info[] table for the + old object. This should be zero for top level parameters. + */ +const AP_Param::ConversionInfo conversion_table[] = { + { Parameters::k_param_log_bitmask_old, 0, AP_PARAM_INT16, "LOG_BITMASK" }, + { Parameters::k_param_serial0_baud, 0, AP_PARAM_INT16, "SERIAL0_BAUD" }, + { Parameters::k_param_serial1_baud, 0, AP_PARAM_INT16, "SERIAL1_BAUD" }, + { Parameters::k_param_serial2_baud, 0, AP_PARAM_INT16, "SERIAL2_BAUD" }, + { Parameters::k_param_arming_check_old, 0, AP_PARAM_INT8, "ARMING_CHECK" }, + // battery + { Parameters::k_param_fs_batt_voltage, 0, AP_PARAM_FLOAT, "BATT_LOW_VOLT" }, + { Parameters::k_param_fs_batt_mah, 0, AP_PARAM_FLOAT, "BATT_LOW_MAH" }, + { Parameters::k_param_failsafe_battery_enabled,0, AP_PARAM_INT8, "BATT_FS_LOW_ACT" }, + + { Parameters::Parameters::k_param_ch7_option_old, 0, AP_PARAM_INT8, "RC7_OPTION" }, + { Parameters::Parameters::k_param_ch8_option_old, 0, AP_PARAM_INT8, "RC8_OPTION" }, + { Parameters::Parameters::k_param_ch9_option_old, 0, AP_PARAM_INT8, "RC9_OPTION" }, + { Parameters::Parameters::k_param_ch10_option_old, 0, AP_PARAM_INT8, "RC10_OPTION" }, + { Parameters::Parameters::k_param_ch11_option_old, 0, AP_PARAM_INT8, "RC11_OPTION" }, + { Parameters::Parameters::k_param_ch12_option_old, 0, AP_PARAM_INT8, "RC12_OPTION" }, + { Parameters::k_param_compass_enabled_deprecated, 0, AP_PARAM_INT8, "COMPASS_ENABLE" }, + { Parameters::k_param_arming, 2, AP_PARAM_INT16, "ARMING_CHECK" }, +}; + +void Copter::load_parameters(void) +{ + if (!AP_Param::check_var_info()) { + hal.console->printf("Bad var table\n"); + AP_HAL::panic("Bad var table"); + } + + // disable centrifugal force correction, it will be enabled as part of the arming process + ahrs.set_correct_centrifugal(false); + hal.util->set_soft_armed(false); + + if (!g.format_version.load() || + g.format_version != Parameters::k_format_version) { + + // erase all parameters + hal.console->printf("Firmware change: erasing EEPROM...\n"); + StorageManager::erase(); + AP_Param::erase_all(); + + // save the current format version + g.format_version.set_and_save(Parameters::k_format_version); + hal.console->printf("done.\n"); + } + + uint32_t before = micros(); + // Load all auto-loaded EEPROM variables + AP_Param::load_all(); + AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table)); + + // convert landing gear parameters + convert_lgr_parameters(); + + // convert fs_options parameters + convert_fs_options_params(); + + hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); + + // setup AP_Param frame type flags + AP_Param::set_frame_type_flags(AP_PARAM_FRAME_COPTER); + +} + +// handle conversion of PID gains +void Copter::convert_pid_parameters(void) +{ + // conversion info + const AP_Param::ConversionInfo pid_conversion_info[] = { + { Parameters::k_param_pid_rate_roll, 0, AP_PARAM_FLOAT, "ATC_RAT_RLL_P" }, + { Parameters::k_param_pid_rate_roll, 1, AP_PARAM_FLOAT, "ATC_RAT_RLL_I" }, + { Parameters::k_param_pid_rate_roll, 2, AP_PARAM_FLOAT, "ATC_RAT_RLL_D" }, + { Parameters::k_param_pid_rate_pitch, 0, AP_PARAM_FLOAT, "ATC_RAT_PIT_P" }, + { Parameters::k_param_pid_rate_pitch, 1, AP_PARAM_FLOAT, "ATC_RAT_PIT_I" }, + { Parameters::k_param_pid_rate_pitch, 2, AP_PARAM_FLOAT, "ATC_RAT_PIT_D" }, + { Parameters::k_param_pid_rate_yaw, 0, AP_PARAM_FLOAT, "ATC_RAT_YAW_P" }, + { Parameters::k_param_pid_rate_yaw, 1, AP_PARAM_FLOAT, "ATC_RAT_YAW_I" }, + { Parameters::k_param_pid_rate_yaw, 2, AP_PARAM_FLOAT, "ATC_RAT_YAW_D" }, +#if FRAME_CONFIG == HELI_FRAME + { Parameters::k_param_pid_rate_roll, 4, AP_PARAM_FLOAT, "ATC_RAT_RLL_VFF" }, + { Parameters::k_param_pid_rate_pitch, 4, AP_PARAM_FLOAT, "ATC_RAT_PIT_VFF" }, + { Parameters::k_param_pid_rate_yaw , 4, AP_PARAM_FLOAT, "ATC_RAT_YAW_VFF" }, +#endif + }; + const AP_Param::ConversionInfo imax_conversion_info[] = { + { Parameters::k_param_pid_rate_roll, 5, AP_PARAM_FLOAT, "ATC_RAT_RLL_IMAX" }, + { Parameters::k_param_pid_rate_pitch, 5, AP_PARAM_FLOAT, "ATC_RAT_PIT_IMAX" }, + { Parameters::k_param_pid_rate_yaw, 5, AP_PARAM_FLOAT, "ATC_RAT_YAW_IMAX" }, +#if FRAME_CONFIG == HELI_FRAME + { Parameters::k_param_pid_rate_roll, 7, AP_PARAM_FLOAT, "ATC_RAT_RLL_ILMI" }, + { Parameters::k_param_pid_rate_pitch, 7, AP_PARAM_FLOAT, "ATC_RAT_PIT_ILMI" }, + { Parameters::k_param_pid_rate_yaw, 7, AP_PARAM_FLOAT, "ATC_RAT_YAW_ILMI" }, +#endif + }; + // conversion from Copter-3.3 to Copter-3.4 + const AP_Param::ConversionInfo angle_and_filt_conversion_info[] = { + { Parameters::k_param_p_stabilize_roll, 0, AP_PARAM_FLOAT, "ATC_ANG_RLL_P" }, + { Parameters::k_param_p_stabilize_pitch, 0, AP_PARAM_FLOAT, "ATC_ANG_PIT_P" }, + { Parameters::k_param_p_stabilize_yaw, 0, AP_PARAM_FLOAT, "ATC_ANG_YAW_P" }, + { Parameters::k_param_pid_rate_roll, 6, AP_PARAM_FLOAT, "ATC_RAT_RLL_FILT" }, + { Parameters::k_param_pid_rate_pitch, 6, AP_PARAM_FLOAT, "ATC_RAT_PIT_FILT" }, + { Parameters::k_param_pid_rate_yaw, 6, AP_PARAM_FLOAT, "ATC_RAT_YAW_FILT" }, + { Parameters::k_param_pi_vel_xy, 0, AP_PARAM_FLOAT, "PSC_VELXY_P" }, + { Parameters::k_param_pi_vel_xy, 1, AP_PARAM_FLOAT, "PSC_VELXY_I" }, + { Parameters::k_param_pi_vel_xy, 2, AP_PARAM_FLOAT, "PSC_VELXY_IMAX" }, + { Parameters::k_param_pi_vel_xy, 3, AP_PARAM_FLOAT, "PSC_VELXY_FILT" }, + { Parameters::k_param_p_vel_z, 0, AP_PARAM_FLOAT, "PSC_VELZ_P" }, + { Parameters::k_param_pid_accel_z, 0, AP_PARAM_FLOAT, "PSC_ACCZ_P" }, + { Parameters::k_param_pid_accel_z, 1, AP_PARAM_FLOAT, "PSC_ACCZ_I" }, + { Parameters::k_param_pid_accel_z, 2, AP_PARAM_FLOAT, "PSC_ACCZ_D" }, + { Parameters::k_param_pid_accel_z, 5, AP_PARAM_FLOAT, "PSC_ACCZ_IMAX" }, + { Parameters::k_param_pid_accel_z, 6, AP_PARAM_FLOAT, "PSC_ACCZ_FLTE" }, + { Parameters::k_param_p_alt_hold, 0, AP_PARAM_FLOAT, "PSC_POSZ_P" }, + { Parameters::k_param_p_pos_xy, 0, AP_PARAM_FLOAT, "PSC_POSXY_P" }, + }; + const AP_Param::ConversionInfo throttle_conversion_info[] = { + { Parameters::k_param_throttle_min, 0, AP_PARAM_FLOAT, "MOT_SPIN_MIN" }, + { Parameters::k_param_throttle_mid, 0, AP_PARAM_FLOAT, "MOT_THST_HOVER" } + }; + const AP_Param::ConversionInfo loiter_conversion_info[] = { + { Parameters::k_param_wp_nav, 4, AP_PARAM_FLOAT, "LOIT_SPEED" }, + { Parameters::k_param_wp_nav, 7, AP_PARAM_FLOAT, "LOIT_BRK_JERK" }, + { Parameters::k_param_wp_nav, 8, AP_PARAM_FLOAT, "LOIT_ACC_MAX" }, + { Parameters::k_param_wp_nav, 9, AP_PARAM_FLOAT, "LOIT_BRK_ACCEL" } + }; + + // gains increase by 27% due to attitude controller's switch to use radians instead of centi-degrees + // and motor libraries switch to accept inputs in -1 to +1 range instead of -4500 ~ +4500 + float pid_scaler = 1.27f; + +#if FRAME_CONFIG != HELI_FRAME + // Multicopter x-frame gains are 40% lower because -1 or +1 input to motors now results in maximum rotation + if (g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_X || g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_V || g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_H) { + pid_scaler = 0.9f; + } +#endif + + // scale PID gains + uint8_t table_size = ARRAY_SIZE(pid_conversion_info); + for (uint8_t i=0; iconfigured_in_storage() || + servo_max->configured_in_storage() || + servo_trim->configured_in_storage() || + servo_reversed->configured_in_storage()) { + // has been previously saved, don't upgrade + return; + } + + // get the old PWM values + AP_Int16 old_pwm; + uint16_t old_retract=0, old_deploy=0; + const AP_Param::ConversionInfo cinfo_ret { Parameters::k_param_landinggear, 0, AP_PARAM_INT16, nullptr }; + const AP_Param::ConversionInfo cinfo_dep { Parameters::k_param_landinggear, 1, AP_PARAM_INT16, nullptr }; + if (AP_Param::find_old_parameter(&cinfo_ret, &old_pwm)) { + old_retract = (uint16_t)old_pwm.get(); + } + if (AP_Param::find_old_parameter(&cinfo_dep, &old_pwm)) { + old_deploy = (uint16_t)old_pwm.get(); + } + + if (old_retract == 0 && old_deploy == 0) { + // old parameters were never set + return; + } + + // use old defaults + if (old_retract == 0) { + old_retract = 1250; + } + if (old_deploy == 0) { + old_deploy = 1750; + } + + // set and save correct values on the servo + if (old_retract <= old_deploy) { + servo_max->set_and_save(old_deploy); + servo_min->set_and_save(old_retract); + servo_trim->set_and_save(old_retract); + servo_reversed->set_and_save_ifchanged(0); + } else { + servo_max->set_and_save(old_retract); + servo_min->set_and_save(old_deploy); + servo_trim->set_and_save(old_deploy); + servo_reversed->set_and_save_ifchanged(1); + } +} + +#if FRAME_CONFIG == HELI_FRAME +// handle conversion of tradheli parameters from Copter-3.6 to Copter-3.7 +void Copter::convert_tradheli_parameters(void) +{ + if (g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI) { + // single heli conversion info + const AP_Param::ConversionInfo singleheli_conversion_info[] = { + { Parameters::k_param_motors, 1, AP_PARAM_INT16, "H_SW_H3_SV1_POS" }, + { Parameters::k_param_motors, 2, AP_PARAM_INT16, "H_SW_H3_SV2_POS" }, + { Parameters::k_param_motors, 3, AP_PARAM_INT16, "H_SW_H3_SV3_POS" }, + { Parameters::k_param_motors, 7, AP_PARAM_INT16, "H_SW_H3_PHANG" }, + { Parameters::k_param_motors, 19, AP_PARAM_INT8, "H_SW_COL_DIR" }, + }; + + // convert single heli parameters without scaling + uint8_t table_size = ARRAY_SIZE(singleheli_conversion_info); + for (uint8_t i=0; iconfigured_in_storage()) { + // the new parameter is not in storage so set generic swash + AP_Param::set_and_save_by_name("H_SW_TYPE", SwashPlateType::SWASHPLATE_TYPE_H3); + } + } + } + } + } else if (g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_DUAL) { + // dual heli conversion info + const AP_Param::ConversionInfo dualheli_conversion_info[] = { + { Parameters::k_param_motors, 1, AP_PARAM_INT16, "H_SW_H3_SV1_POS" }, + { Parameters::k_param_motors, 2, AP_PARAM_INT16, "H_SW_H3_SV2_POS" }, + { Parameters::k_param_motors, 3, AP_PARAM_INT16, "H_SW_H3_SV3_POS" }, + { Parameters::k_param_motors, 4, AP_PARAM_INT16, "H_SW2_H3_SV1_POS" }, + { Parameters::k_param_motors, 5, AP_PARAM_INT16, "H_SW2_H3_SV2_POS" }, + { Parameters::k_param_motors, 6, AP_PARAM_INT16, "H_SW2_H3_SV3_POS" }, + { Parameters::k_param_motors, 7, AP_PARAM_INT16, "H_SW_H3_PHANG" }, + { Parameters::k_param_motors, 8, AP_PARAM_INT16, "H_SW2_H3_PHANG" }, + { Parameters::k_param_motors, 19, AP_PARAM_INT8, "H_SW_COL_DIR" }, + { Parameters::k_param_motors, 19, AP_PARAM_INT8, "H_SW2_COL_DIR" }, + }; + + // convert dual heli parameters without scaling + uint8_t table_size = ARRAY_SIZE(dualheli_conversion_info); + for (uint8_t i=0; iconfigured_in_storage()) { + // the new parameter is not in storage so set generic swash + AP_Param::set_and_save_by_name("H_SW_TYPE", SwashPlateType::SWASHPLATE_TYPE_H3); + } + } + } + //SWASH 2 + // old swash type is not in eeprom and thus type is default value of generic swash + if (swash2_pos1_exist || swash2_pos2_exist || swash2_pos3_exist || swash2_phang_exist) { + // if any params exist with the generic swash then the upgraded swash type must be generic + // find the new variable in the variable structures + enum ap_var_type ptype; + AP_Param *ap2; + ap2 = AP_Param::find("H_SW2_TYPE", &ptype); + // make sure the pointer is valid + if (ap2 != nullptr) { + // see if we can load it from EEPROM + if (!ap2->configured_in_storage()) { + // the new parameter is not in storage so set generic swash + AP_Param::set_and_save_by_name("H_SW2_TYPE", SwashPlateType::SWASHPLATE_TYPE_H3); + } + } + } + } + + // table of rsc parameters to be converted with scaling + const AP_Param::ConversionInfo rschelipct_conversion_info[] = { + { Parameters::k_param_motors, 1280, AP_PARAM_INT16, "H_RSC_THRCRV_0" }, + { Parameters::k_param_motors, 1344, AP_PARAM_INT16, "H_RSC_THRCRV_25" }, + { Parameters::k_param_motors, 1408, AP_PARAM_INT16, "H_RSC_THRCRV_50" }, + { Parameters::k_param_motors, 1472, AP_PARAM_INT16, "H_RSC_THRCRV_75" }, + { Parameters::k_param_motors, 1536, AP_PARAM_INT16, "H_RSC_THRCRV_100" }, + { Parameters::k_param_motors, 448, AP_PARAM_INT16, "H_RSC_SETPOINT" }, + { Parameters::k_param_motors, 768, AP_PARAM_INT16, "H_RSC_CRITICAL" }, + { Parameters::k_param_motors, 832, AP_PARAM_INT16, "H_RSC_IDLE" }, + }; + // convert heli rsc parameters with scaling + uint8_t table_size = ARRAY_SIZE(rschelipct_conversion_info); + for (uint8_t i=0; iget() > 100 ) { + uint16_t tailspeed_pct = (uint16_t)(0.1f * tailspeed->get()); + AP_Param::set_and_save_by_name("H_TAIL_SPEED", tailspeed_pct ); + } + + // table of stabilize collective parameters to be converted with scaling + const AP_Param::ConversionInfo collhelipct_conversion_info[] = { + { Parameters::k_param_input_manager, 1, AP_PARAM_INT16, "IM_STB_COL_1" }, + { Parameters::k_param_input_manager, 2, AP_PARAM_INT16, "IM_STB_COL_2" }, + { Parameters::k_param_input_manager, 3, AP_PARAM_INT16, "IM_STB_COL_3" }, + { Parameters::k_param_input_manager, 4, AP_PARAM_INT16, "IM_STB_COL_4" }, + }; + + // convert stabilize collective parameters with scaling + table_size = ARRAY_SIZE(collhelipct_conversion_info); + for (uint8_t i=0; iconfigured_in_storage() || ptype != AP_PARAM_INT32) { + return; + } + + // Variable to capture the new FS_OPTIONS setting + int32_t fs_options_converted = 0; + + // If FS_THR_ENABLED is 2 (continue mission), change to RTL and add continue mission to the new FS_OPTIONS parameter + if (g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) { + fs_options_converted |= int32_t(FailsafeOption::RC_CONTINUE_IF_AUTO); + AP_Param::set_and_save_by_name("FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL); + } + + // If FS_GCS_ENABLED is 2 (continue mission), change to RTL and add continue mission to the new FS_OPTIONS parameter + if (g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) { + fs_options_converted |= int32_t(FailsafeOption::GCS_CONTINUE_IF_AUTO); + AP_Param::set_and_save_by_name("FS_GCS_ENABLE", FS_GCS_ENABLED_ALWAYS_RTL); + } + + // Write the new value to FS_OPTIONS + // AP_Param::set_and_save_by_name("FS_OPTIONS", fs_options_converted); + fs_opt->set_and_save(fs_options_converted); +} + + +// modify by @Brown +const char* Copter::get_sysid_board_id(void) +{ + + static char buf[50]; + // snprintf(buf, sizeof(buf), "Version: v3.5.6 ,Board ID: ZRZK.19QT3.%d",(int)g.sysid_board_id); + + int32_t nameValue1 =(int32_t) g.sysid_board_name_1st; + int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd; + char name[6] = {' ',' ',' ',' ',' ','.'}; + // memset(name,0,6); + nameValue2 = nameValue2 & 0xffffff; + // name[5] = "a"; + name[4] = nameValue2&0xFF; + name[3] = nameValue2>>8&0xFF; + name[2] = nameValue2>>16& 0xFF; + name[1] = nameValue1&0xFF; + name[0] = nameValue1>>8 & 0xFF; + snprintf(buf, sizeof(buf), "Version: zr-v4.0 ,Board ID: ZRZK.%5s%d",name,(int)g.sysid_board_id); + return buf; +} +void Copter::get_deadline_params(int32_t &deadline ){ + deadline = 20000000+ g.sysid_dead_line; +} diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 7b55d373c6..55fa86948d 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -1,633 +1,635 @@ -#pragma once - -#include -#include "RC_Channel.h" - -#if GRIPPER_ENABLED == ENABLED - # include -#endif -#if MODE_FOLLOW_ENABLED == ENABLED - # include -#endif -#if VISUAL_ODOMETRY_ENABLED == ENABLED - # include -#endif - -// Global parameter class. -// -class Parameters { -public: - // The version of the layout as described by the parameter enum. - // - // When changing the parameter enum in an incompatible fashion, this - // value should be incremented by one. - // - // The increment will prevent old parameters from being used incorrectly - // by newer code. - // - static const uint16_t k_format_version = 120; - - // Parameter identities. - // - // The enumeration defined here is used to ensure that every parameter - // or parameter group has a unique ID number. This number is used by - // AP_Param to store and locate parameters in EEPROM. - // - // Note that entries without a number are assigned the next number after - // the entry preceding them. When adding new entries, ensure that they - // don't overlap. - // - // Try to group related variables together, and assign them a set - // range in the enumeration. Place these groups in numerical order - // at the end of the enumeration. - // - // WARNING: Care should be taken when editing this enumeration as the - // AP_Param load/save code depends on the values here to identify - // variables saved in EEPROM. - // - // - enum { - // Layout version number, always key zero. - // - k_param_format_version = 0, - k_param_software_type, // deprecated - k_param_ins_old, // *** Deprecated, remove with next eeprom number change - k_param_ins, // libraries/AP_InertialSensor variables - k_param_NavEKF2_old, // deprecated - remove - k_param_NavEKF2, - k_param_g2, // 2nd block of parameters - k_param_NavEKF3, - k_param_BoardConfig_CAN, - k_param_osd, - - // simulation - k_param_sitl = 10, - - // barometer object (needed for SITL) - k_param_barometer, - - // scheduler object (for debugging) - k_param_scheduler, - - // relay object - k_param_relay, - - // (old) EPM object - k_param_epm_unused, - - // BoardConfig object - k_param_BoardConfig, - - // GPS object - k_param_gps, - - // Parachute object - k_param_parachute, - - // Landing gear object - k_param_landinggear, // 18 - - // Input Management object - k_param_input_manager, // 19 - - // Misc - // - k_param_log_bitmask_old = 20, // Deprecated - k_param_log_last_filenumber, // *** Deprecated - remove - // with next eeprom number - // change - k_param_toy_yaw_rate, // deprecated - remove - k_param_crosstrack_min_distance, // deprecated - remove with next eeprom number change - k_param_rssi_pin, // unused, replaced by rssi_ library parameters - k_param_throttle_accel_enabled, // deprecated - remove - k_param_wp_yaw_behavior, - k_param_acro_trainer, - k_param_pilot_speed_up, // renamed from k_param_pilot_velocity_z_max - k_param_circle_rate, // deprecated - remove - k_param_rangefinder_gain, - k_param_ch8_option_old, // deprecated - k_param_arming_check_old, // deprecated - remove - k_param_sprayer, - k_param_angle_max, - k_param_gps_hdop_good, - k_param_battery, - k_param_fs_batt_mah, // unused - moved to AP_BattMonitor - k_param_angle_rate_max, // remove - k_param_rssi_range, // unused, replaced by rssi_ library parameters - k_param_rc_feel_rp, // deprecated - k_param_NavEKF, // deprecated - remove - k_param_mission, // mission library - k_param_rc_13_old, - k_param_rc_14_old, - k_param_rally, - k_param_poshold_brake_rate, - k_param_poshold_brake_angle_max, - k_param_pilot_accel_z, - k_param_serial0_baud, // deprecated - remove - k_param_serial1_baud, // deprecated - remove - k_param_serial2_baud, // deprecated - remove - k_param_land_repositioning, - k_param_rangefinder, // rangefinder object - k_param_fs_ekf_thresh, - k_param_terrain, - k_param_acro_rp_expo, - k_param_throttle_deadzone, - k_param_optflow, - k_param_dcmcheck_thresh, // deprecated - remove - k_param_log_bitmask, - k_param_cli_enabled_old, // deprecated - remove - k_param_throttle_filt, - k_param_throttle_behavior, - k_param_pilot_takeoff_alt, // 64 - - // 65: AP_Limits Library - k_param_limits = 65, // deprecated - remove - k_param_gpslock_limit, // deprecated - remove - k_param_geofence_limit, // deprecated - remove - k_param_altitude_limit, // deprecated - remove - k_param_fence, - k_param_gps_glitch, // deprecated - k_param_baro_glitch, // 71 - deprecated - - // AP_ADSB Library - k_param_adsb, // 72 - k_param_notify, // 73 - - // 74: precision landing object - k_param_precland = 74, - - // - // 75: Singlecopter, CoaxCopter - // - k_param_single_servo_1 = 75, // remove - k_param_single_servo_2, // remove - k_param_single_servo_3, // remove - k_param_single_servo_4, // 78 - remove - - // - // 80: Heli - // - k_param_heli_servo_1 = 80, // remove - k_param_heli_servo_2, // remove - k_param_heli_servo_3, // remove - k_param_heli_servo_4, // remove - k_param_heli_pitch_ff, // remove - k_param_heli_roll_ff, // remove - k_param_heli_yaw_ff, // remove - k_param_heli_stab_col_min, // remove - k_param_heli_stab_col_max, // remove - k_param_heli_servo_rsc, // 89 = full! - remove - - // - // 90: misc2 - // - k_param_motors = 90, - k_param_disarm_delay, - k_param_fs_crash_check, - k_param_throw_motor_start, - k_param_terrain_follow, // 94 - k_param_avoid, - k_param_avoidance_adsb, - - // 97: RSSI - k_param_rssi = 97, - - // - // 100: Inertial Nav - // - k_param_inertial_nav = 100, // deprecated - k_param_wp_nav, - k_param_attitude_control, - k_param_pos_control, - k_param_circle_nav, - k_param_loiter_nav, // 105 - - k_param_sysid_board_name_1st =106, //add by @Binsir - k_param_sysid_board_name_2nd=107, //add by @Binsir - k_param_sysid_board_id=108, // modify by @Brown - - // 110: Telemetry control - // - k_param_gcs0 = 110, - k_param_gcs1, - k_param_sysid_this_mav, - k_param_sysid_my_gcs, - k_param_serial1_baud_old, // deprecated - k_param_telem_delay, - k_param_gcs2, - k_param_serial2_baud_old, // deprecated - k_param_serial2_protocol, // deprecated - k_param_serial_manager, - k_param_ch9_option_old, - k_param_ch10_option_old, - k_param_ch11_option_old, - k_param_ch12_option_old, - k_param_takeoff_trigger_dz_old, - k_param_gcs3, - k_param_gcs_pid_mask, // 126 - - // - // 135 : reserved for Solo until features merged with master - // - k_param_rtl_speed_cms = 135, - k_param_fs_batt_curr_rtl, - k_param_rtl_cone_slope, // 137 - - // - // 140: Sensor parameters - // - k_param_imu = 140, // deprecated - can be deleted - k_param_battery_monitoring = 141, // deprecated - can be deleted - k_param_volt_div_ratio, // deprecated - can be deleted - k_param_curr_amp_per_volt, // deprecated - can be deleted - k_param_input_voltage, // deprecated - can be deleted - k_param_pack_capacity, // deprecated - can be deleted - k_param_compass_enabled_deprecated, - k_param_compass, - k_param_rangefinder_enabled_old, // deprecated - k_param_frame_type, - k_param_optflow_enabled, // deprecated - k_param_fs_batt_voltage, // unused - moved to AP_BattMonitor - k_param_ch7_option_old, - k_param_auto_slew_rate, // deprecated - can be deleted - k_param_rangefinder_type_old, // deprecated - k_param_super_simple = 155, - k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change - k_param_copter_leds_mode, // deprecated - remove with next eeprom number change - k_param_ahrs, // AHRS group // 159 - - // - // 160: Navigation parameters - // - k_param_rtl_altitude = 160, - k_param_crosstrack_gain, // deprecated - remove with next eeprom number change - k_param_rtl_loiter_time, - k_param_rtl_alt_final, - k_param_tilt_comp, //164 deprecated - remove with next eeprom number change - - - // - // Camera and mount parameters - // - k_param_camera = 165, - k_param_camera_mount, - k_param_camera_mount2, // deprecated - - // - // Battery monitoring parameters - // - k_param_battery_volt_pin = 168, // deprecated - can be deleted - k_param_battery_curr_pin, // 169 deprecated - can be deleted - - // - // 170: Radio settings - // - k_param_rc_1_old = 170, - k_param_rc_2_old, - k_param_rc_3_old, - k_param_rc_4_old, - k_param_rc_5_old, - k_param_rc_6_old, - k_param_rc_7_old, - k_param_rc_8_old, - k_param_rc_10_old, - k_param_rc_11_old, - k_param_throttle_min, // remove - k_param_throttle_max, // remove - k_param_failsafe_throttle, - k_param_throttle_fs_action, // remove - k_param_failsafe_throttle_value, - k_param_throttle_trim, // remove - k_param_esc_calibrate, - k_param_radio_tuning, - k_param_radio_tuning_high_old, // unused - k_param_radio_tuning_low_old, // unused - k_param_rc_speed = 192, - k_param_failsafe_battery_enabled, // unused - moved to AP_BattMonitor - k_param_throttle_mid, // remove - k_param_failsafe_gps_enabled, // remove - k_param_rc_9_old, - k_param_rc_12_old, - k_param_failsafe_gcs, - k_param_rcmap, // 199 - - // - // 200: flight modes - // - k_param_flight_mode1 = 200, - k_param_flight_mode2, - k_param_flight_mode3, - k_param_flight_mode4, - k_param_flight_mode5, - k_param_flight_mode6, - k_param_simple_modes, - k_param_flight_mode_chan, - - // - // 210: Waypoint data - // - k_param_waypoint_mode = 210, // remove - k_param_command_total, // remove - k_param_command_index, // remove - k_param_command_nav_index, // remove - k_param_waypoint_radius, // remove - k_param_circle_radius, // remove - k_param_waypoint_speed_max, // remove - k_param_land_speed, - k_param_auto_velocity_z_min, // remove - k_param_auto_velocity_z_max, // remove - 219 - k_param_land_speed_high, - - // - // 220: PI/D Controllers - // - k_param_acro_rp_p = 221, - k_param_axis_lock_p, // remove - k_param_pid_rate_roll, // remove - k_param_pid_rate_pitch, // remove - k_param_pid_rate_yaw, // remove - k_param_p_stabilize_roll, // remove - k_param_p_stabilize_pitch, // remove - k_param_p_stabilize_yaw, // remove - k_param_p_pos_xy, // remove - k_param_p_loiter_lon, // remove - k_param_pid_loiter_rate_lat, // remove - k_param_pid_loiter_rate_lon, // remove - k_param_pid_nav_lat, // remove - k_param_pid_nav_lon, // remove - k_param_p_alt_hold, // remove - k_param_p_vel_z, // remove - k_param_pid_optflow_roll, // remove - k_param_pid_optflow_pitch, // remove - k_param_acro_balance_roll_old, // remove - k_param_acro_balance_pitch_old, // remove - k_param_pid_accel_z, // remove - k_param_acro_balance_roll, - k_param_acro_balance_pitch, - k_param_acro_yaw_p, - k_param_autotune_axis_bitmask, // remove - k_param_autotune_aggressiveness, // remove - k_param_pi_vel_xy, // remove - k_param_fs_ekf_action, - k_param_rtl_climb_min, - k_param_rpm_sensor, - k_param_autotune_min_d, // remove - k_param_arming, // 252 - AP_Arming - k_param_logger = 253, // 253 - Logging Group - - // 254,255: reserved - - // the k_param_* space is 9-bits in size - // 511: reserved - }; - - AP_Int16 format_version; - - AP_Int16 sysid_board_id; // modify by @Brown - AP_Int32 sysid_board_name_1st;// modify by @Binsir - AP_Int32 sysid_board_name_2nd;// modify by @Binsir - - // Telemetry control - // - AP_Int16 sysid_this_mav; - AP_Int16 sysid_my_gcs; - AP_Int8 telem_delay; - - AP_Float throttle_filt; - AP_Int16 throttle_behavior; - AP_Float pilot_takeoff_alt; - - AP_Int16 rtl_altitude; - AP_Int16 rtl_speed_cms; - AP_Float rtl_cone_slope; -#if RANGEFINDER_ENABLED == ENABLED - AP_Float rangefinder_gain; -#endif - - AP_Int8 failsafe_gcs; // ground station failsafe behavior - AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position - - AP_Int8 super_simple; - AP_Int16 rtl_alt_final; - AP_Int16 rtl_climb_min; // rtl minimum climb in cm - - AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions - - AP_Int16 poshold_brake_rate; // PosHold flight mode's rotation rate during braking in deg/sec - AP_Int16 poshold_brake_angle_max; // PosHold flight mode's max lean angle during braking in centi-degrees - - // Waypoints - // - AP_Int32 rtl_loiter_time; - AP_Int16 land_speed; - AP_Int16 land_speed_high; - AP_Int16 pilot_speed_up; // maximum vertical ascending velocity the pilot may request - AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request - - // Throttle - // - AP_Int8 failsafe_throttle; - AP_Int16 failsafe_throttle_value; - AP_Int16 throttle_deadzone; - - // Flight modes - // - AP_Int8 flight_mode1; - AP_Int8 flight_mode2; - AP_Int8 flight_mode3; - AP_Int8 flight_mode4; - AP_Int8 flight_mode5; - AP_Int8 flight_mode6; - AP_Int8 simple_modes; - AP_Int8 flight_mode_chan; - - // Misc - // - AP_Int32 log_bitmask; - AP_Int8 esc_calibrate; - AP_Int8 radio_tuning; - AP_Int8 frame_type; - AP_Int8 disarm_delay; - - AP_Int8 land_repositioning; - AP_Int8 fs_ekf_action; - AP_Int8 fs_crash_check; - AP_Float fs_ekf_thresh; - AP_Int16 gcs_pid_mask; - -#if MODE_THROW_ENABLED == ENABLED - AP_Int8 throw_motor_start; -#endif - -#if AP_TERRAIN_AVAILABLE && AC_TERRAIN - AP_Int8 terrain_follow; -#endif - - AP_Int16 rc_speed; // speed of fast RC Channels in Hz - - // Acro parameters - AP_Float acro_rp_p; - AP_Float acro_yaw_p; - AP_Float acro_balance_roll; - AP_Float acro_balance_pitch; - AP_Int8 acro_trainer; - AP_Float acro_rp_expo; - - // Note: keep initializers here in the same order as they are declared - // above. - Parameters() - { - } -}; - -/* - 2nd block of parameters, to avoid going past 256 top level keys - */ -class ParametersG2 { -public: - ParametersG2(void); - - // var_info for holding Parameter information - static const struct AP_Param::GroupInfo var_info[]; - - // altitude at which nav control can start in takeoff - AP_Float wp_navalt_min; - -#if BUTTON_ENABLED == ENABLED - // button checking - AP_Button button; -#endif - -#if STATS_ENABLED == ENABLED - // vehicle statistics - AP_Stats stats; -#endif - -#if GRIPPER_ENABLED - AP_Gripper gripper; -#endif - -#if MODE_THROW_ENABLED == ENABLED - // Throw mode parameters - AP_Int8 throw_nextmode; - AP_Int8 throw_type; -#endif - - // ground effect compensation enable/disable - AP_Int8 gndeffect_comp_enabled; - - // temperature calibration handling - AP_TempCalibration temp_calibration; - -#if BEACON_ENABLED == ENABLED - // beacon (non-GPS positioning) library - AP_Beacon beacon; -#endif - -#if VISUAL_ODOMETRY_ENABLED == ENABLED - // Visual Odometry camera - AP_VisualOdom visual_odom; -#endif - -#if PROXIMITY_ENABLED == ENABLED - // proximity (aka object avoidance) library - AP_Proximity proximity; -#endif - - // whether to enforce acceptance of packets only from sysid_my_gcs - AP_Int8 sysid_enforce; - -#if ADVANCED_FAILSAFE == ENABLED - // advanced failsafe library - AP_AdvancedFailsafe_Copter afs; -#endif - - // developer options - AP_Int32 dev_options; - - // acro exponent parameters - AP_Float acro_y_expo; -#if MODE_ACRO_ENABLED == ENABLED - AP_Float acro_thr_mid; -#endif - - // frame class - AP_Int8 frame_class; - - // RC input channels - RC_Channels_Copter rc_channels; - - // control over servo output ranges - SRV_Channels servo_channels; - -#if MODE_SMARTRTL_ENABLED == ENABLED - // Safe RTL library - AP_SmartRTL smart_rtl; -#endif - - // wheel encoder and winch -#if WINCH_ENABLED == ENABLED - AP_WheelEncoder wheel_encoder; - AP_Winch winch; -#endif - - // Additional pilot velocity items - AP_Int16 pilot_speed_dn; - - // Land alt final stage - AP_Int16 land_alt_low; - -#if TOY_MODE_ENABLED == ENABLED - ToyMode toy_mode; -#endif - -#if OPTFLOW == ENABLED - // we need a pointer to the mode for the G2 table - void *mode_flowhold_ptr; -#endif - -#if MODE_FOLLOW_ENABLED == ENABLED - // follow - AP_Follow follow; -#endif - -#ifdef USER_PARAMS_ENABLED - // User custom parameters - UserParameters user_parameters; -#endif - -#if AUTOTUNE_ENABLED == ENABLED - // we need a pointer to autotune for the G2 table - void *autotune_ptr; -#endif - -#ifdef ENABLE_SCRIPTING - AP_Scripting scripting; -#endif // ENABLE_SCRIPTING - - AP_Float tuning_min; - AP_Float tuning_max; - -#if AC_OAPATHPLANNER_ENABLED == ENABLED - // object avoidance path planning - AP_OAPathPlanner oa; -#endif - -#if MODE_SYSTEMID_ENABLED == ENABLED - // we need a pointer to the mode for the G2 table - void *mode_systemid_ptr; -#endif - - // vibration failsafe enable/disable - AP_Int8 fs_vibe_enabled; - - // Failsafe options bitmask #36 - AP_Int32 fs_options; - -#if MODE_AUTOROTATE_ENABLED == ENABLED - // Autonmous autorotation - AC_Autorotation arot; -#endif -}; - -extern const AP_Param::Info var_info[]; +#pragma once + +#include +#include "RC_Channel.h" + +#if GRIPPER_ENABLED == ENABLED + # include +#endif +#if MODE_FOLLOW_ENABLED == ENABLED + # include +#endif +#if VISUAL_ODOMETRY_ENABLED == ENABLED + # include +#endif + +// Global parameter class. +// +class Parameters { +public: + // The version of the layout as described by the parameter enum. + // + // When changing the parameter enum in an incompatible fashion, this + // value should be incremented by one. + // + // The increment will prevent old parameters from being used incorrectly + // by newer code. + // + static const uint16_t k_format_version = 120; + + // Parameter identities. + // + // The enumeration defined here is used to ensure that every parameter + // or parameter group has a unique ID number. This number is used by + // AP_Param to store and locate parameters in EEPROM. + // + // Note that entries without a number are assigned the next number after + // the entry preceding them. When adding new entries, ensure that they + // don't overlap. + // + // Try to group related variables together, and assign them a set + // range in the enumeration. Place these groups in numerical order + // at the end of the enumeration. + // + // WARNING: Care should be taken when editing this enumeration as the + // AP_Param load/save code depends on the values here to identify + // variables saved in EEPROM. + // + // + enum { + // Layout version number, always key zero. + // + k_param_format_version = 0, + k_param_software_type, // deprecated + k_param_ins_old, // *** Deprecated, remove with next eeprom number change + k_param_ins, // libraries/AP_InertialSensor variables + k_param_NavEKF2_old, // deprecated - remove + k_param_NavEKF2, + k_param_g2, // 2nd block of parameters + k_param_NavEKF3, + k_param_BoardConfig_CAN, + k_param_osd, + + // simulation + k_param_sitl = 10, + + // barometer object (needed for SITL) + k_param_barometer, + + // scheduler object (for debugging) + k_param_scheduler, + + // relay object + k_param_relay, + + // (old) EPM object + k_param_epm_unused, + + // BoardConfig object + k_param_BoardConfig, + + // GPS object + k_param_gps, + + // Parachute object + k_param_parachute, + + // Landing gear object + k_param_landinggear, // 18 + + // Input Management object + k_param_input_manager, // 19 + + // Misc + // + k_param_log_bitmask_old = 20, // Deprecated + k_param_log_last_filenumber, // *** Deprecated - remove + // with next eeprom number + // change + k_param_toy_yaw_rate, // deprecated - remove + k_param_crosstrack_min_distance, // deprecated - remove with next eeprom number change + k_param_rssi_pin, // unused, replaced by rssi_ library parameters + k_param_throttle_accel_enabled, // deprecated - remove + k_param_wp_yaw_behavior, + k_param_acro_trainer, + k_param_pilot_speed_up, // renamed from k_param_pilot_velocity_z_max + k_param_circle_rate, // deprecated - remove + k_param_rangefinder_gain, + k_param_ch8_option_old, // deprecated + k_param_arming_check_old, // deprecated - remove + k_param_sprayer, + k_param_angle_max, + k_param_gps_hdop_good, + k_param_battery, + k_param_fs_batt_mah, // unused - moved to AP_BattMonitor + k_param_angle_rate_max, // remove + k_param_rssi_range, // unused, replaced by rssi_ library parameters + k_param_rc_feel_rp, // deprecated + k_param_NavEKF, // deprecated - remove + k_param_mission, // mission library + k_param_rc_13_old, + k_param_rc_14_old, + k_param_rally, + k_param_poshold_brake_rate, + k_param_poshold_brake_angle_max, + k_param_pilot_accel_z, + k_param_serial0_baud, // deprecated - remove + k_param_serial1_baud, // deprecated - remove + k_param_serial2_baud, // deprecated - remove + k_param_land_repositioning, + k_param_rangefinder, // rangefinder object + k_param_fs_ekf_thresh, + k_param_terrain, + k_param_acro_rp_expo, + k_param_throttle_deadzone, + k_param_optflow, + k_param_dcmcheck_thresh, // deprecated - remove + k_param_log_bitmask, + k_param_cli_enabled_old, // deprecated - remove + k_param_throttle_filt, + k_param_throttle_behavior, + k_param_pilot_takeoff_alt, // 64 + + // 65: AP_Limits Library + k_param_limits = 65, // deprecated - remove + k_param_gpslock_limit, // deprecated - remove + k_param_geofence_limit, // deprecated - remove + k_param_altitude_limit, // deprecated - remove + k_param_fence, + k_param_gps_glitch, // deprecated + k_param_baro_glitch, // 71 - deprecated + + // AP_ADSB Library + k_param_adsb, // 72 + k_param_notify, // 73 + + // 74: precision landing object + k_param_precland = 74, + + // + // 75: Singlecopter, CoaxCopter + // + k_param_single_servo_1 = 75, // remove + k_param_single_servo_2, // remove + k_param_single_servo_3, // remove + k_param_single_servo_4, // 78 - remove + + // + // 80: Heli + // + k_param_heli_servo_1 = 80, // remove + k_param_heli_servo_2, // remove + k_param_heli_servo_3, // remove + k_param_heli_servo_4, // remove + k_param_heli_pitch_ff, // remove + k_param_heli_roll_ff, // remove + k_param_heli_yaw_ff, // remove + k_param_heli_stab_col_min, // remove + k_param_heli_stab_col_max, // remove + k_param_heli_servo_rsc, // 89 = full! - remove + + // + // 90: misc2 + // + k_param_motors = 90, + k_param_disarm_delay, + k_param_fs_crash_check, + k_param_throw_motor_start, + k_param_terrain_follow, // 94 + k_param_avoid, + k_param_avoidance_adsb, + + // 97: RSSI + k_param_rssi = 97, + + // + // 100: Inertial Nav + // + k_param_inertial_nav = 100, // deprecated + k_param_wp_nav, + k_param_attitude_control, + k_param_pos_control, + k_param_circle_nav, + k_param_loiter_nav, // 105 + + k_param_sysid_board_name_1st =106, //add by @Binsir + k_param_sysid_board_name_2nd=107, //add by @Binsir + k_param_sysid_board_id=108, // modify by @Brown + + // 110: Telemetry control + // + k_param_gcs0 = 110, + k_param_gcs1, + k_param_sysid_this_mav, + k_param_sysid_my_gcs, + k_param_serial1_baud_old, // deprecated + k_param_telem_delay, + k_param_gcs2, + k_param_serial2_baud_old, // deprecated + k_param_serial2_protocol, // deprecated + k_param_serial_manager, + k_param_ch9_option_old, + k_param_ch10_option_old, + k_param_ch11_option_old, + k_param_ch12_option_old, + k_param_takeoff_trigger_dz_old, + k_param_gcs3, + k_param_gcs_pid_mask, // 126 + + // + // 135 : reserved for Solo until features merged with master + // + k_param_rtl_speed_cms = 135, + k_param_fs_batt_curr_rtl, + k_param_rtl_cone_slope, // 137 + + // + // 140: Sensor parameters + // + k_param_imu = 140, // deprecated - can be deleted + k_param_battery_monitoring = 141, // deprecated - can be deleted + k_param_volt_div_ratio, // deprecated - can be deleted + k_param_curr_amp_per_volt, // deprecated - can be deleted + k_param_input_voltage, // deprecated - can be deleted + k_param_pack_capacity, // deprecated - can be deleted + k_param_compass_enabled_deprecated, + k_param_compass, + k_param_rangefinder_enabled_old, // deprecated + k_param_frame_type, + k_param_optflow_enabled, // deprecated + k_param_fs_batt_voltage, // unused - moved to AP_BattMonitor + k_param_ch7_option_old, + k_param_auto_slew_rate, // deprecated - can be deleted + k_param_rangefinder_type_old, // deprecated + k_param_super_simple = 155, + k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change + k_param_copter_leds_mode, // deprecated - remove with next eeprom number change + k_param_ahrs, // AHRS group // 159 + + // + // 160: Navigation parameters + // + k_param_rtl_altitude = 160, + k_param_crosstrack_gain, // deprecated - remove with next eeprom number change + k_param_rtl_loiter_time, + k_param_rtl_alt_final, + k_param_tilt_comp, //164 deprecated - remove with next eeprom number change + + + // + // Camera and mount parameters + // + k_param_camera = 165, + k_param_camera_mount, + k_param_camera_mount2, // deprecated + + // + // Battery monitoring parameters + // + k_param_battery_volt_pin = 168, // deprecated - can be deleted + k_param_battery_curr_pin, // 169 deprecated - can be deleted + + // + // 170: Radio settings + // + k_param_rc_1_old = 170, + k_param_rc_2_old, + k_param_rc_3_old, + k_param_rc_4_old, + k_param_rc_5_old, + k_param_rc_6_old, + k_param_rc_7_old, + k_param_rc_8_old, + k_param_rc_10_old, + k_param_rc_11_old, + k_param_throttle_min, // remove + k_param_throttle_max, // remove + k_param_failsafe_throttle, + k_param_throttle_fs_action, // remove + k_param_failsafe_throttle_value, + k_param_throttle_trim, // remove + k_param_esc_calibrate, + k_param_radio_tuning, + k_param_radio_tuning_high_old, // unused + k_param_radio_tuning_low_old, // unused + k_param_rc_speed = 192, + k_param_failsafe_battery_enabled, // unused - moved to AP_BattMonitor + k_param_throttle_mid, // remove + k_param_failsafe_gps_enabled, // remove + k_param_rc_9_old, + k_param_rc_12_old, + k_param_failsafe_gcs, + k_param_rcmap, // 199 + + // + // 200: flight modes + // + k_param_flight_mode1 = 200, + k_param_flight_mode2, + k_param_flight_mode3, + k_param_flight_mode4, + k_param_flight_mode5, + k_param_flight_mode6, + k_param_simple_modes, + k_param_flight_mode_chan, + + // + // 210: Waypoint data + // + k_param_waypoint_mode = 210, // remove + k_param_command_total, // remove + k_param_command_index, // remove + k_param_command_nav_index, // remove + k_param_waypoint_radius, // remove + k_param_circle_radius, // remove + k_param_waypoint_speed_max, // remove + k_param_land_speed, + k_param_auto_velocity_z_min, // remove + k_param_auto_velocity_z_max, // remove - 219 + k_param_land_speed_high, + + // + // 220: PI/D Controllers + // + k_param_acro_rp_p = 221, + k_param_axis_lock_p, // remove + k_param_pid_rate_roll, // remove + k_param_pid_rate_pitch, // remove + k_param_pid_rate_yaw, // remove + k_param_p_stabilize_roll, // remove + k_param_p_stabilize_pitch, // remove + k_param_p_stabilize_yaw, // remove + k_param_p_pos_xy, // remove + k_param_p_loiter_lon, // remove + k_param_pid_loiter_rate_lat, // remove + k_param_pid_loiter_rate_lon, // remove + k_param_pid_nav_lat, // remove + k_param_pid_nav_lon, // remove + k_param_p_alt_hold, // remove + k_param_p_vel_z, // remove + k_param_pid_optflow_roll, // remove + k_param_pid_optflow_pitch, // remove + k_param_acro_balance_roll_old, // remove + k_param_acro_balance_pitch_old, // remove + k_param_pid_accel_z, // remove + k_param_acro_balance_roll, + k_param_acro_balance_pitch, + k_param_acro_yaw_p, + k_param_autotune_axis_bitmask, // remove + k_param_autotune_aggressiveness, // remove + k_param_pi_vel_xy, // remove + k_param_fs_ekf_action, + k_param_rtl_climb_min, + k_param_rpm_sensor, + k_param_autotune_min_d, // remove + k_param_arming, // 252 - AP_Arming + k_param_logger = 253, // 253 - Logging Group + + // 254,255: reserved + k_param_sysid_dead_line = 300, + // the k_param_* space is 9-bits in size + // 511: reserved + }; + + AP_Int16 format_version; + + AP_Int16 sysid_board_id; // modify by @Brown + AP_Int32 sysid_board_name_1st;// modify by @Binsir + AP_Int32 sysid_board_name_2nd;// modify by @Binsir + + // Telemetry control + // + AP_Int16 sysid_this_mav; + AP_Int16 sysid_my_gcs; + AP_Int8 telem_delay; + + AP_Int32 sysid_dead_line; + + AP_Float throttle_filt; + AP_Int16 throttle_behavior; + AP_Float pilot_takeoff_alt; + + AP_Int16 rtl_altitude; + AP_Int16 rtl_speed_cms; + AP_Float rtl_cone_slope; +#if RANGEFINDER_ENABLED == ENABLED + AP_Float rangefinder_gain; +#endif + + AP_Int8 failsafe_gcs; // ground station failsafe behavior + AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position + + AP_Int8 super_simple; + AP_Int16 rtl_alt_final; + AP_Int16 rtl_climb_min; // rtl minimum climb in cm + + AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions + + AP_Int16 poshold_brake_rate; // PosHold flight mode's rotation rate during braking in deg/sec + AP_Int16 poshold_brake_angle_max; // PosHold flight mode's max lean angle during braking in centi-degrees + + // Waypoints + // + AP_Int32 rtl_loiter_time; + AP_Int16 land_speed; + AP_Int16 land_speed_high; + AP_Int16 pilot_speed_up; // maximum vertical ascending velocity the pilot may request + AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request + + // Throttle + // + AP_Int8 failsafe_throttle; + AP_Int16 failsafe_throttle_value; + AP_Int16 throttle_deadzone; + + // Flight modes + // + AP_Int8 flight_mode1; + AP_Int8 flight_mode2; + AP_Int8 flight_mode3; + AP_Int8 flight_mode4; + AP_Int8 flight_mode5; + AP_Int8 flight_mode6; + AP_Int8 simple_modes; + AP_Int8 flight_mode_chan; + + // Misc + // + AP_Int32 log_bitmask; + AP_Int8 esc_calibrate; + AP_Int8 radio_tuning; + AP_Int8 frame_type; + AP_Int8 disarm_delay; + + AP_Int8 land_repositioning; + AP_Int8 fs_ekf_action; + AP_Int8 fs_crash_check; + AP_Float fs_ekf_thresh; + AP_Int16 gcs_pid_mask; + +#if MODE_THROW_ENABLED == ENABLED + AP_Int8 throw_motor_start; +#endif + +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + AP_Int8 terrain_follow; +#endif + + AP_Int16 rc_speed; // speed of fast RC Channels in Hz + + // Acro parameters + AP_Float acro_rp_p; + AP_Float acro_yaw_p; + AP_Float acro_balance_roll; + AP_Float acro_balance_pitch; + AP_Int8 acro_trainer; + AP_Float acro_rp_expo; + + // Note: keep initializers here in the same order as they are declared + // above. + Parameters() + { + } +}; + +/* + 2nd block of parameters, to avoid going past 256 top level keys + */ +class ParametersG2 { +public: + ParametersG2(void); + + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + + // altitude at which nav control can start in takeoff + AP_Float wp_navalt_min; + +#if BUTTON_ENABLED == ENABLED + // button checking + AP_Button button; +#endif + +#if STATS_ENABLED == ENABLED + // vehicle statistics + AP_Stats stats; +#endif + +#if GRIPPER_ENABLED + AP_Gripper gripper; +#endif + +#if MODE_THROW_ENABLED == ENABLED + // Throw mode parameters + AP_Int8 throw_nextmode; + AP_Int8 throw_type; +#endif + + // ground effect compensation enable/disable + AP_Int8 gndeffect_comp_enabled; + + // temperature calibration handling + AP_TempCalibration temp_calibration; + +#if BEACON_ENABLED == ENABLED + // beacon (non-GPS positioning) library + AP_Beacon beacon; +#endif + +#if VISUAL_ODOMETRY_ENABLED == ENABLED + // Visual Odometry camera + AP_VisualOdom visual_odom; +#endif + +#if PROXIMITY_ENABLED == ENABLED + // proximity (aka object avoidance) library + AP_Proximity proximity; +#endif + + // whether to enforce acceptance of packets only from sysid_my_gcs + AP_Int8 sysid_enforce; + +#if ADVANCED_FAILSAFE == ENABLED + // advanced failsafe library + AP_AdvancedFailsafe_Copter afs; +#endif + + // developer options + AP_Int32 dev_options; + + // acro exponent parameters + AP_Float acro_y_expo; +#if MODE_ACRO_ENABLED == ENABLED + AP_Float acro_thr_mid; +#endif + + // frame class + AP_Int8 frame_class; + + // RC input channels + RC_Channels_Copter rc_channels; + + // control over servo output ranges + SRV_Channels servo_channels; + +#if MODE_SMARTRTL_ENABLED == ENABLED + // Safe RTL library + AP_SmartRTL smart_rtl; +#endif + + // wheel encoder and winch +#if WINCH_ENABLED == ENABLED + AP_WheelEncoder wheel_encoder; + AP_Winch winch; +#endif + + // Additional pilot velocity items + AP_Int16 pilot_speed_dn; + + // Land alt final stage + AP_Int16 land_alt_low; + +#if TOY_MODE_ENABLED == ENABLED + ToyMode toy_mode; +#endif + +#if OPTFLOW == ENABLED + // we need a pointer to the mode for the G2 table + void *mode_flowhold_ptr; +#endif + +#if MODE_FOLLOW_ENABLED == ENABLED + // follow + AP_Follow follow; +#endif + +#ifdef USER_PARAMS_ENABLED + // User custom parameters + UserParameters user_parameters; +#endif + +#if AUTOTUNE_ENABLED == ENABLED + // we need a pointer to autotune for the G2 table + void *autotune_ptr; +#endif + +#ifdef ENABLE_SCRIPTING + AP_Scripting scripting; +#endif // ENABLE_SCRIPTING + + AP_Float tuning_min; + AP_Float tuning_max; + +#if AC_OAPATHPLANNER_ENABLED == ENABLED + // object avoidance path planning + AP_OAPathPlanner oa; +#endif + +#if MODE_SYSTEMID_ENABLED == ENABLED + // we need a pointer to the mode for the G2 table + void *mode_systemid_ptr; +#endif + + // vibration failsafe enable/disable + AP_Int8 fs_vibe_enabled; + + // Failsafe options bitmask #36 + AP_Int32 fs_options; + +#if MODE_AUTOROTATE_ENABLED == ENABLED + // Autonmous autorotation + AC_Autorotation arot; +#endif +}; + +extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 811e1dba34..a63fdfd527 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -1,684 +1,701 @@ -#include "Copter.h" -#include - -/***************************************************************************** -* 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 mavlink_delay_cb_static() -{ - copter.mavlink_delay_cb(); -} - - -static void failsafe_check_static() -{ - copter.failsafe_check(); -} - -void Copter::init_ardupilot() -{ - // initialise serial port - serial_manager.init_console(); - - hal.console->printf("\n\nInit %s" - "\n\nFree RAM: %u\n", - AP::fwversion().fw_string, - (unsigned)hal.util->available_memory()); - - // - // Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function) - // - report_version(); - - // load parameters from EEPROM - load_parameters(); - - // time per loop - this gets updated in the main loop() based on - // actual loop rate - G_Dt = 1.0 / scheduler.get_loop_rate_hz(); - -#if STATS_ENABLED == ENABLED - // initialise stats module - 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 mavlink_delay_cb, which will run anytime you have - // more than 5ms remaining in your call to hal.scheduler->delay - hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); - - BoardConfig.init(); -#if HAL_WITH_UAVCAN - BoardConfig_CAN.init(); -#endif - - // init cargo gripper -#if GRIPPER_ENABLED == ENABLED - g2.gripper.init(); -#endif - - fence.init(); - - // init winch and wheel encoder - winch_init(); - - // initialise notify system - notify.init(); - notify_flight_mode(); - - // initialise battery monitor - battery.init(); - - // Init RSSI - rssi.init(); - - barometer.init(); - - // setup telem slots with serial ports - gcs().setup_uarts(); - -#if OSD_ENABLED == ENABLED - osd.init(); -#endif - -#if LOGGING_ENABLED == ENABLED - log_init(); -#endif - - // update motor interlock state - update_using_interlock(); - -#if FRAME_CONFIG == HELI_FRAME - // trad heli specific initialisation - heli_init(); -#endif -#if FRAME_CONFIG == HELI_FRAME - input_manager.set_loop_rate(scheduler.get_loop_rate_hz()); -#endif - - init_rc_in(); // sets up rc channels from radio - - // allocate the motors class - allocate_motors(); - - // initialise rc channels including setting mode - rc().init(); - - // sets up motors and output to escs - init_rc_out(); - - // check if we should enter esc calibration mode - esc_calibration_startup_check(); - - // motors initialised so parameters can be sent - ap.initialised_params = true; - - 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 OPTFLOW == ENABLED - // make optflow available to AHRS - ahrs.set_optflow(&optflow); -#endif - - // init Location class -#if AP_TERRAIN_AVAILABLE && AC_TERRAIN - Location::set_terrain(&terrain); - wp_nav->set_terrain(&terrain); -#endif - -#if AC_OAPATHPLANNER_ENABLED == ENABLED - g2.oa.init(); -#endif - - attitude_control->parameter_sanity_check(); - pos_control->set_dt(scheduler.get_loop_period_s()); - - // init the optical flow sensor - init_optflow(); - -#if MOUNT == ENABLED - // initialise camera mount - camera_mount.init(); -#endif - -#if PRECISION_LANDING == ENABLED - // initialise precision landing - init_precland(); -#endif - - // initialise landing gear position - landinggear.init(); - -#ifdef USERHOOK_INIT - USERHOOK_INIT -#endif - -#if HIL_MODE != HIL_MODE_DISABLED - while (barometer.get_last_update() == 0) { - // the barometer begins updating when we get the first - // HIL_STATE message - gcs().send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message"); - delay(1000); - } - - // set INS to HIL mode - ins.set_hil_mode(); -#endif - - // read Baro pressure at ground - //----------------------------- - barometer.set_log_baro_bit(MASK_LOG_IMU); - barometer.calibrate(); - - // initialise rangefinder - init_rangefinder(); - - // init proximity sensor - init_proximity(); - -#if BEACON_ENABLED == ENABLED - // init beacons used for non-gps position estimation - g2.beacon.init(); -#endif - - // init visual odometry - init_visual_odom(); - -#if RPM_ENABLED == ENABLED - // initialise AP_RPM library - rpm_sensor.init(); -#endif - -#if MODE_AUTO_ENABLED == ENABLED - // initialise mission library - mode_auto.mission.init(); -#endif - -#if MODE_SMARTRTL_ENABLED == ENABLED - // initialize SmartRTL - g2.smart_rtl.init(); -#endif - - // initialise AP_Logger library - logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void)); - - startup_INS_ground(); - -#ifdef ENABLE_SCRIPTING - g2.scripting.init(); -#endif // ENABLE_SCRIPTING - - // set landed flags - set_land_complete(true); - set_land_complete_maybe(true); - - // 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 - failsafe_enable(); - - ins.set_log_raw_bit(MASK_LOG_IMU_RAW); - - // enable output to motors - if (arming.rc_calibration_checks(true)) { - enable_motor_output(); - } - - // disable safety if requested - BoardConfig.init_safety(); - - hal.console->printf("\nReady to FLY "); - - // flag that initialisation has completed - ap.initialised = true; - -#if AP_PARAM_KEY_DUMP - AP_Param::show_all(hal.console, true); -#endif -} - - -//****************************************************************************** -//This function does all the calibrations, etc. that we need during a ground start -//****************************************************************************** -void Copter::startup_INS_ground() -{ - // initialise ahrs (may push imu calibration into the mpu6000 if using that device). - ahrs.init(); - ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER); - - // Warm up and calibrate gyro offsets - ins.init(scheduler.get_loop_rate_hz()); - - // reset ahrs including gyro bias - ahrs.reset(); -} - -// update the harmonic notch filter center frequency dynamically -void Copter::update_dynamic_notch() -{ - const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz(); - const float ref = ins.get_gyro_harmonic_notch_reference(); - - if (is_zero(ref)) { - ins.update_harmonic_notch_freq_hz(ref_freq); - return; - } - - switch (ins.get_gyro_harmonic_notch_tracking_mode()) { - case HarmonicNotch_UpdateThrottle: // throttle based tracking - // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle - ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref))); - break; - -#if RPM_ENABLED == ENABLED - case HarmonicNotch_UpdateRPM: // rpm sensor based tracking - if (rpm_sensor.healthy(0)) { - // set the harmonic notch filter frequency from the main rotor rpm - ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm_sensor.get_rpm(0) * ref / 60.0f)); - } else { - ins.update_harmonic_notch_freq_hz(ref_freq); - } - break; -#endif -#ifdef HAVE_AP_BLHELI_SUPPORT - case HarmonicNotch_UpdateBLHeli: // BLHeli based tracking - ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP_BLHeli::get_singleton()->get_average_motor_frequency_hz() * ref)); - break; -#endif - case HarmonicNotch_Fixed: // static - default: - ins.update_harmonic_notch_freq_hz(ref_freq); - break; - } -} - -// position_ok - returns true if the horizontal absolute position is ok and home position is set -bool Copter::position_ok() const -{ - // 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 Copter::ekf_position_ok() const -{ - 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)); - } else { - // 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 Copter::optflow_position_ok() const -{ -#if OPTFLOW != ENABLED && VISUAL_ODOMETRY_ENABLED != ENABLED - return false; -#else - // 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 OPTFLOW == ENABLED - if (optflow.enabled()) { - enabled = true; - } -#endif -#if VISUAL_ODOMETRY_ENABLED == ENABLED - if (g2.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); - } else { - return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode); - } -#endif -} - -// update_auto_armed - update status of auto_armed flag -void Copter::update_auto_armed() -{ - // disarm checks - if(ap.auto_armed){ - // if motors are disarmed, auto_armed should also be false - if(!motors->armed()) { - set_auto_armed(false); - return; - } - // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false - if(flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio) { - set_auto_armed(false); - } - // if helicopters are on the ground, and the motor is switched off, auto-armed should be false - // so that rotor runup is checked again before attempting to take-off - if(ap.land_complete && motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && ap.using_interlock) { - set_auto_armed(false); - } - }else{ - // arm checks - - // for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true - if(motors->armed() && ap.using_interlock) { - if(!ap.throttle_zero && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { - set_auto_armed(true); - } - // if motors are armed and throttle is above zero auto_armed should be true - // if motors are armed and we are in throw mode, then auto_armed should be true - } else if (motors->armed() && !ap.using_interlock) { - if(!ap.throttle_zero || control_mode == Mode::Number::THROW) { - set_auto_armed(true); - } - } - } -} - -/* - should we log a message type now? - */ -bool Copter::should_log(uint32_t mask) -{ -#if LOGGING_ENABLED == ENABLED - ap.logging_started = logger.logging_started(); - return logger.should_log(mask); -#else - return false; -#endif -} - -// return MAV_TYPE corresponding to frame class -MAV_TYPE Copter::get_frame_mav_type() -{ - switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) { - case AP_Motors::MOTOR_FRAME_QUAD: - case AP_Motors::MOTOR_FRAME_UNDEFINED: - return MAV_TYPE_QUADROTOR; - case AP_Motors::MOTOR_FRAME_HEXA: - case AP_Motors::MOTOR_FRAME_Y6: - return MAV_TYPE_HEXAROTOR; - case AP_Motors::MOTOR_FRAME_OCTA: - case AP_Motors::MOTOR_FRAME_OCTAQUAD: - return MAV_TYPE_OCTOROTOR; - case AP_Motors::MOTOR_FRAME_HELI: - case AP_Motors::MOTOR_FRAME_HELI_DUAL: - case AP_Motors::MOTOR_FRAME_HELI_QUAD: - return MAV_TYPE_HELICOPTER; - case AP_Motors::MOTOR_FRAME_TRI: - return MAV_TYPE_TRICOPTER; - case AP_Motors::MOTOR_FRAME_SINGLE: - case AP_Motors::MOTOR_FRAME_COAX: - case AP_Motors::MOTOR_FRAME_TAILSITTER: - return MAV_TYPE_COAXIAL; - case AP_Motors::MOTOR_FRAME_DODECAHEXA: - return MAV_TYPE_DODECAROTOR; - } - // unknown frame so return generic - return MAV_TYPE_GENERIC; -} - -// return string corresponding to frame_class -const char* Copter::get_frame_string() -{ - switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) { - case AP_Motors::MOTOR_FRAME_QUAD: - return "QUAD"; - case AP_Motors::MOTOR_FRAME_HEXA: - return "HEXA"; - case AP_Motors::MOTOR_FRAME_Y6: - return "Y6"; - case AP_Motors::MOTOR_FRAME_OCTA: - return "OCTA"; - case AP_Motors::MOTOR_FRAME_OCTAQUAD: - return "OCTA_QUAD"; - case AP_Motors::MOTOR_FRAME_HELI: - return "HELI"; - case AP_Motors::MOTOR_FRAME_HELI_DUAL: - return "HELI_DUAL"; - case AP_Motors::MOTOR_FRAME_HELI_QUAD: - return "HELI_QUAD"; - case AP_Motors::MOTOR_FRAME_TRI: - return "TRI"; - case AP_Motors::MOTOR_FRAME_SINGLE: - return "SINGLE"; - case AP_Motors::MOTOR_FRAME_COAX: - return "COAX"; - case AP_Motors::MOTOR_FRAME_TAILSITTER: - return "TAILSITTER"; - case AP_Motors::MOTOR_FRAME_DODECAHEXA: - return "DODECA_HEXA"; - case AP_Motors::MOTOR_FRAME_UNDEFINED: - default: - return "UNKNOWN"; - } -} - -/* - allocate the motors class - */ -void Copter::allocate_motors(void) -{ - switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) { -#if FRAME_CONFIG != HELI_FRAME - case AP_Motors::MOTOR_FRAME_QUAD: - case AP_Motors::MOTOR_FRAME_HEXA: - case AP_Motors::MOTOR_FRAME_Y6: - case AP_Motors::MOTOR_FRAME_OCTA: - case AP_Motors::MOTOR_FRAME_OCTAQUAD: - case AP_Motors::MOTOR_FRAME_DODECAHEXA: - default: - motors = new AP_MotorsMatrix(copter.scheduler.get_loop_rate_hz()); - motors_var_info = AP_MotorsMatrix::var_info; - break; - case AP_Motors::MOTOR_FRAME_TRI: - motors = new AP_MotorsTri(copter.scheduler.get_loop_rate_hz()); - motors_var_info = AP_MotorsTri::var_info; - AP_Param::set_frame_type_flags(AP_PARAM_FRAME_TRICOPTER); - break; - case AP_Motors::MOTOR_FRAME_SINGLE: - motors = new AP_MotorsSingle(copter.scheduler.get_loop_rate_hz()); - motors_var_info = AP_MotorsSingle::var_info; - break; - case AP_Motors::MOTOR_FRAME_COAX: - motors = new AP_MotorsCoax(copter.scheduler.get_loop_rate_hz()); - motors_var_info = AP_MotorsCoax::var_info; - break; - case AP_Motors::MOTOR_FRAME_TAILSITTER: - motors = new AP_MotorsTailsitter(copter.scheduler.get_loop_rate_hz()); - motors_var_info = AP_MotorsTailsitter::var_info; - break; -#else // FRAME_CONFIG == HELI_FRAME - case AP_Motors::MOTOR_FRAME_HELI_DUAL: - motors = new AP_MotorsHeli_Dual(copter.scheduler.get_loop_rate_hz()); - motors_var_info = AP_MotorsHeli_Dual::var_info; - AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI); - break; - - case AP_Motors::MOTOR_FRAME_HELI_QUAD: - motors = new AP_MotorsHeli_Quad(copter.scheduler.get_loop_rate_hz()); - motors_var_info = AP_MotorsHeli_Quad::var_info; - AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI); - break; - - case AP_Motors::MOTOR_FRAME_HELI: - default: - motors = new AP_MotorsHeli_Single(copter.scheduler.get_loop_rate_hz()); - motors_var_info = AP_MotorsHeli_Single::var_info; - AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI); - break; -#endif - } - if (motors == nullptr) { - AP_HAL::panic("Unable to allocate FRAME_CLASS=%u", (unsigned)g2.frame_class.get()); - } - AP_Param::load_object_from_eeprom(motors, motors_var_info); - - ahrs_view = ahrs.create_view(ROTATION_NONE); - if (ahrs_view == nullptr) { - AP_HAL::panic("Unable to allocate AP_AHRS_View"); - } - - const struct AP_Param::GroupInfo *ac_var_info; - -#if FRAME_CONFIG != HELI_FRAME - attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s()); - ac_var_info = AC_AttitudeControl_Multi::var_info; -#else - attitude_control = new AC_AttitudeControl_Heli(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s()); - ac_var_info = AC_AttitudeControl_Heli::var_info; -#endif - if (attitude_control == nullptr) { - AP_HAL::panic("Unable to allocate AttitudeControl"); - } - AP_Param::load_object_from_eeprom(attitude_control, ac_var_info); - - pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control); - if (pos_control == nullptr) { - AP_HAL::panic("Unable to allocate PosControl"); - } - AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info); - -#if AC_OAPATHPLANNER_ENABLED == ENABLED - wp_nav = new AC_WPNav_OA(inertial_nav, *ahrs_view, *pos_control, *attitude_control); -#else - wp_nav = new AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control); -#endif - if (wp_nav == nullptr) { - AP_HAL::panic("Unable to allocate WPNav"); - } - AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info); - - loiter_nav = new AC_Loiter(inertial_nav, *ahrs_view, *pos_control, *attitude_control); - if (loiter_nav == nullptr) { - AP_HAL::panic("Unable to allocate LoiterNav"); - } - AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info); - -#if MODE_CIRCLE_ENABLED == ENABLED - circle_nav = new AC_Circle(inertial_nav, *ahrs_view, *pos_control); - if (circle_nav == nullptr) { - AP_HAL::panic("Unable to allocate CircleNav"); - } - AP_Param::load_object_from_eeprom(circle_nav, circle_nav->var_info); -#endif - - // reload lines from the defaults file that may now be accessible - AP_Param::reload_defaults_file(true); - - // now setup some frame-class specific defaults - switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) { - case AP_Motors::MOTOR_FRAME_Y6: - attitude_control->get_rate_roll_pid().kP().set_default(0.1); - attitude_control->get_rate_roll_pid().kD().set_default(0.006); - attitude_control->get_rate_pitch_pid().kP().set_default(0.1); - attitude_control->get_rate_pitch_pid().kD().set_default(0.006); - attitude_control->get_rate_yaw_pid().kP().set_default(0.15); - attitude_control->get_rate_yaw_pid().kI().set_default(0.015); - break; - case AP_Motors::MOTOR_FRAME_TRI: - attitude_control->get_rate_yaw_pid().filt_D_hz().set_default(100); - break; - default: - break; - } - - // brushed 16kHz defaults to 16kHz pulses - if (motors->get_pwm_type() == AP_Motors::PWM_TYPE_BRUSHED) { - g.rc_speed.set_default(16000); - } - - if (upgrading_frame_params) { - // do frame specific upgrade. This is only done the first time we run the new firmware -#if FRAME_CONFIG == HELI_FRAME - SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 12, CH_1); - SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 13, CH_2); - SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 14, CH_3); - SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 15, CH_4); -#else - if (g2.frame_class == AP_Motors::MOTOR_FRAME_TRI) { - const AP_Param::ConversionInfo tri_conversion_info[] = { - { Parameters::k_param_motors, 32, AP_PARAM_INT16, "SERVO7_TRIM" }, - { Parameters::k_param_motors, 33, AP_PARAM_INT16, "SERVO7_MIN" }, - { Parameters::k_param_motors, 34, AP_PARAM_INT16, "SERVO7_MAX" }, - { Parameters::k_param_motors, 35, AP_PARAM_FLOAT, "MOT_YAW_SV_ANGLE" }, - }; - // we need to use CONVERT_FLAG_FORCE as the SERVO7_* parameters will already be set from RC7_* - AP_Param::convert_old_parameters(tri_conversion_info, ARRAY_SIZE(tri_conversion_info), AP_Param::CONVERT_FLAG_FORCE); - const AP_Param::ConversionInfo tri_conversion_info_rev { Parameters::k_param_motors, 31, AP_PARAM_INT8, "SERVO7_REVERSED" }; - AP_Param::convert_old_parameter(&tri_conversion_info_rev, 1, AP_Param::CONVERT_FLAG_REVERSE | AP_Param::CONVERT_FLAG_FORCE); - // AP_MotorsTri was converted from having nested var_info to one level - AP_Param::convert_parent_class(Parameters::k_param_motors, motors, motors->var_info); - } -#endif - } - - // upgrade parameters. This must be done after allocating the objects - convert_pid_parameters(); -#if FRAME_CONFIG == HELI_FRAME - convert_tradheli_parameters(); -#endif -} - -bool Copter::is_tradheli() const -{ -#if FRAME_CONFIG == HELI_FRAME - return true; -#else - return false; -#endif -} +#include "Copter.h" +#include + +/***************************************************************************** +* 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 mavlink_delay_cb_static() +{ + copter.mavlink_delay_cb(); +} + + +static void failsafe_check_static() +{ + copter.failsafe_check(); +} + +void Copter::init_ardupilot() +{ + // initialise serial port + serial_manager.init_console(); + + hal.console->printf("\n\nInit %s" + "\n\nFree RAM: %u\n", + AP::fwversion().fw_string, + (unsigned)hal.util->available_memory()); + + // + // Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function) + // + report_version(); + + // load parameters from EEPROM + load_parameters(); + + // time per loop - this gets updated in the main loop() based on + // actual loop rate + G_Dt = 1.0 / scheduler.get_loop_rate_hz(); + +#if STATS_ENABLED == ENABLED + // initialise stats module + 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 mavlink_delay_cb, which will run anytime you have + // more than 5ms remaining in your call to hal.scheduler->delay + hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); + + BoardConfig.init(); +#if HAL_WITH_UAVCAN + BoardConfig_CAN.init(); +#endif + + // init cargo gripper +#if GRIPPER_ENABLED == ENABLED + g2.gripper.init(); +#endif + + fence.init(); + + // init winch and wheel encoder + winch_init(); + + // initialise notify system + notify.init(); + notify_flight_mode(); + + // initialise battery monitor + battery.init(); + + // Init RSSI + rssi.init(); + + barometer.init(); + + // setup telem slots with serial ports + gcs().setup_uarts(); + +#if OSD_ENABLED == ENABLED + osd.init(); +#endif + +#if LOGGING_ENABLED == ENABLED + log_init(); +#endif + + // update motor interlock state + update_using_interlock(); + +#if FRAME_CONFIG == HELI_FRAME + // trad heli specific initialisation + heli_init(); +#endif +#if FRAME_CONFIG == HELI_FRAME + input_manager.set_loop_rate(scheduler.get_loop_rate_hz()); +#endif + + init_rc_in(); // sets up rc channels from radio + + // allocate the motors class + allocate_motors(); + + // initialise rc channels including setting mode + rc().init(); + + // sets up motors and output to escs + init_rc_out(); + + // check if we should enter esc calibration mode + esc_calibration_startup_check(); + + // motors initialised so parameters can be sent + ap.initialised_params = true; + + 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 OPTFLOW == ENABLED + // make optflow available to AHRS + ahrs.set_optflow(&optflow); +#endif + + // init Location class +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + Location::set_terrain(&terrain); + wp_nav->set_terrain(&terrain); +#endif + +#if AC_OAPATHPLANNER_ENABLED == ENABLED + g2.oa.init(); +#endif + + attitude_control->parameter_sanity_check(); + pos_control->set_dt(scheduler.get_loop_period_s()); + + // init the optical flow sensor + init_optflow(); + +#if MOUNT == ENABLED + // initialise camera mount + camera_mount.init(); +#endif + +#if PRECISION_LANDING == ENABLED + // initialise precision landing + init_precland(); +#endif + + // initialise landing gear position + landinggear.init(); + +#ifdef USERHOOK_INIT + USERHOOK_INIT +#endif + +#if HIL_MODE != HIL_MODE_DISABLED + while (barometer.get_last_update() == 0) { + // the barometer begins updating when we get the first + // HIL_STATE message + gcs().send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message"); + delay(1000); + } + + // set INS to HIL mode + ins.set_hil_mode(); +#endif + + // read Baro pressure at ground + //----------------------------- + barometer.set_log_baro_bit(MASK_LOG_IMU); + barometer.calibrate(); + + // initialise rangefinder + init_rangefinder(); + + // init proximity sensor + init_proximity(); + +#if BEACON_ENABLED == ENABLED + // init beacons used for non-gps position estimation + g2.beacon.init(); +#endif + + // init visual odometry + init_visual_odom(); + +#if RPM_ENABLED == ENABLED + // initialise AP_RPM library + rpm_sensor.init(); +#endif + +#if MODE_AUTO_ENABLED == ENABLED + // initialise mission library + mode_auto.mission.init(); +#endif + +#if MODE_SMARTRTL_ENABLED == ENABLED + // initialize SmartRTL + g2.smart_rtl.init(); +#endif + + // initialise AP_Logger library + logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void)); + + startup_INS_ground(); + +#ifdef ENABLE_SCRIPTING + g2.scripting.init(); +#endif // ENABLE_SCRIPTING + + // set landed flags + set_land_complete(true); + set_land_complete_maybe(true); + + // 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 + failsafe_enable(); + + ins.set_log_raw_bit(MASK_LOG_IMU_RAW); + + // enable output to motors + if (arming.rc_calibration_checks(true)) { + enable_motor_output(); + } + + // disable safety if requested + BoardConfig.init_safety(); + + hal.console->printf("\nReady to FLY "); + + // flag that initialisation has completed + ap.initialised = true; + +#if AP_PARAM_KEY_DUMP + AP_Param::show_all(hal.console, true); +#endif +} + + +//****************************************************************************** +//This function does all the calibrations, etc. that we need during a ground start +//****************************************************************************** +void Copter::startup_INS_ground() +{ + // initialise ahrs (may push imu calibration into the mpu6000 if using that device). + ahrs.init(); + ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER); + + // Warm up and calibrate gyro offsets + ins.init(scheduler.get_loop_rate_hz()); + + // reset ahrs including gyro bias + ahrs.reset(); +} + +// update the harmonic notch filter center frequency dynamically +void Copter::update_dynamic_notch() +{ + const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz(); + const float ref = ins.get_gyro_harmonic_notch_reference(); + + if (is_zero(ref)) { + ins.update_harmonic_notch_freq_hz(ref_freq); + return; + } + + switch (ins.get_gyro_harmonic_notch_tracking_mode()) { + case HarmonicNotch_UpdateThrottle: // throttle based tracking + // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle + ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref))); + break; + +#if RPM_ENABLED == ENABLED + case HarmonicNotch_UpdateRPM: // rpm sensor based tracking + if (rpm_sensor.healthy(0)) { + // set the harmonic notch filter frequency from the main rotor rpm + ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm_sensor.get_rpm(0) * ref / 60.0f)); + } else { + ins.update_harmonic_notch_freq_hz(ref_freq); + } + break; +#endif +#ifdef HAVE_AP_BLHELI_SUPPORT + case HarmonicNotch_UpdateBLHeli: // BLHeli based tracking + ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP_BLHeli::get_singleton()->get_average_motor_frequency_hz() * ref)); + break; +#endif + case HarmonicNotch_Fixed: // static + default: + ins.update_harmonic_notch_freq_hz(ref_freq); + break; + } +} + +// position_ok - returns true if the horizontal absolute position is ok and home position is set +bool Copter::position_ok() const +{ + // return false if ekf failsafe has triggered + if (failsafe.ekf) { + return false; + } + + // check ekf position estimate + return (ekf_position_ok() || optflow_position_ok()); +} + +bool Copter::deadline_ok() +{ + + int32_t deadline =0; + copter.get_deadline_params(deadline); + int32_t gps_date =0; + AP::gps().get_gps_headline_value(gps_date); + if ((deadline - gps_date) > 0) + { + return true; + } + else + { + return false; + } +} + +// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set +bool Copter::ekf_position_ok() const +{ + 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)); + } else { + // 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 Copter::optflow_position_ok() const +{ +#if OPTFLOW != ENABLED && VISUAL_ODOMETRY_ENABLED != ENABLED + return false; +#else + // 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 OPTFLOW == ENABLED + if (optflow.enabled()) { + enabled = true; + } +#endif +#if VISUAL_ODOMETRY_ENABLED == ENABLED + if (g2.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); + } else { + return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode); + } +#endif +} + +// update_auto_armed - update status of auto_armed flag +void Copter::update_auto_armed() +{ + // disarm checks + if(ap.auto_armed){ + // if motors are disarmed, auto_armed should also be false + if(!motors->armed()) { + set_auto_armed(false); + return; + } + // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false + if(flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio) { + set_auto_armed(false); + } + // if helicopters are on the ground, and the motor is switched off, auto-armed should be false + // so that rotor runup is checked again before attempting to take-off + if(ap.land_complete && motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && ap.using_interlock) { + set_auto_armed(false); + } + }else{ + // arm checks + + // for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true + if(motors->armed() && ap.using_interlock) { + if(!ap.throttle_zero && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { + set_auto_armed(true); + } + // if motors are armed and throttle is above zero auto_armed should be true + // if motors are armed and we are in throw mode, then auto_armed should be true + } else if (motors->armed() && !ap.using_interlock) { + if(!ap.throttle_zero || control_mode == Mode::Number::THROW) { + set_auto_armed(true); + } + } + } +} + +/* + should we log a message type now? + */ +bool Copter::should_log(uint32_t mask) +{ +#if LOGGING_ENABLED == ENABLED + ap.logging_started = logger.logging_started(); + return logger.should_log(mask); +#else + return false; +#endif +} + +// return MAV_TYPE corresponding to frame class +MAV_TYPE Copter::get_frame_mav_type() +{ + switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) { + case AP_Motors::MOTOR_FRAME_QUAD: + case AP_Motors::MOTOR_FRAME_UNDEFINED: + return MAV_TYPE_QUADROTOR; + case AP_Motors::MOTOR_FRAME_HEXA: + case AP_Motors::MOTOR_FRAME_Y6: + return MAV_TYPE_HEXAROTOR; + case AP_Motors::MOTOR_FRAME_OCTA: + case AP_Motors::MOTOR_FRAME_OCTAQUAD: + return MAV_TYPE_OCTOROTOR; + case AP_Motors::MOTOR_FRAME_HELI: + case AP_Motors::MOTOR_FRAME_HELI_DUAL: + case AP_Motors::MOTOR_FRAME_HELI_QUAD: + return MAV_TYPE_HELICOPTER; + case AP_Motors::MOTOR_FRAME_TRI: + return MAV_TYPE_TRICOPTER; + case AP_Motors::MOTOR_FRAME_SINGLE: + case AP_Motors::MOTOR_FRAME_COAX: + case AP_Motors::MOTOR_FRAME_TAILSITTER: + return MAV_TYPE_COAXIAL; + case AP_Motors::MOTOR_FRAME_DODECAHEXA: + return MAV_TYPE_DODECAROTOR; + } + // unknown frame so return generic + return MAV_TYPE_GENERIC; +} + +// return string corresponding to frame_class +const char* Copter::get_frame_string() +{ + switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) { + case AP_Motors::MOTOR_FRAME_QUAD: + return "QUAD"; + case AP_Motors::MOTOR_FRAME_HEXA: + return "HEXA"; + case AP_Motors::MOTOR_FRAME_Y6: + return "Y6"; + case AP_Motors::MOTOR_FRAME_OCTA: + return "OCTA"; + case AP_Motors::MOTOR_FRAME_OCTAQUAD: + return "OCTA_QUAD"; + case AP_Motors::MOTOR_FRAME_HELI: + return "HELI"; + case AP_Motors::MOTOR_FRAME_HELI_DUAL: + return "HELI_DUAL"; + case AP_Motors::MOTOR_FRAME_HELI_QUAD: + return "HELI_QUAD"; + case AP_Motors::MOTOR_FRAME_TRI: + return "TRI"; + case AP_Motors::MOTOR_FRAME_SINGLE: + return "SINGLE"; + case AP_Motors::MOTOR_FRAME_COAX: + return "COAX"; + case AP_Motors::MOTOR_FRAME_TAILSITTER: + return "TAILSITTER"; + case AP_Motors::MOTOR_FRAME_DODECAHEXA: + return "DODECA_HEXA"; + case AP_Motors::MOTOR_FRAME_UNDEFINED: + default: + return "UNKNOWN"; + } +} + +/* + allocate the motors class + */ +void Copter::allocate_motors(void) +{ + switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) { +#if FRAME_CONFIG != HELI_FRAME + case AP_Motors::MOTOR_FRAME_QUAD: + case AP_Motors::MOTOR_FRAME_HEXA: + case AP_Motors::MOTOR_FRAME_Y6: + case AP_Motors::MOTOR_FRAME_OCTA: + case AP_Motors::MOTOR_FRAME_OCTAQUAD: + case AP_Motors::MOTOR_FRAME_DODECAHEXA: + default: + motors = new AP_MotorsMatrix(copter.scheduler.get_loop_rate_hz()); + motors_var_info = AP_MotorsMatrix::var_info; + break; + case AP_Motors::MOTOR_FRAME_TRI: + motors = new AP_MotorsTri(copter.scheduler.get_loop_rate_hz()); + motors_var_info = AP_MotorsTri::var_info; + AP_Param::set_frame_type_flags(AP_PARAM_FRAME_TRICOPTER); + break; + case AP_Motors::MOTOR_FRAME_SINGLE: + motors = new AP_MotorsSingle(copter.scheduler.get_loop_rate_hz()); + motors_var_info = AP_MotorsSingle::var_info; + break; + case AP_Motors::MOTOR_FRAME_COAX: + motors = new AP_MotorsCoax(copter.scheduler.get_loop_rate_hz()); + motors_var_info = AP_MotorsCoax::var_info; + break; + case AP_Motors::MOTOR_FRAME_TAILSITTER: + motors = new AP_MotorsTailsitter(copter.scheduler.get_loop_rate_hz()); + motors_var_info = AP_MotorsTailsitter::var_info; + break; +#else // FRAME_CONFIG == HELI_FRAME + case AP_Motors::MOTOR_FRAME_HELI_DUAL: + motors = new AP_MotorsHeli_Dual(copter.scheduler.get_loop_rate_hz()); + motors_var_info = AP_MotorsHeli_Dual::var_info; + AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI); + break; + + case AP_Motors::MOTOR_FRAME_HELI_QUAD: + motors = new AP_MotorsHeli_Quad(copter.scheduler.get_loop_rate_hz()); + motors_var_info = AP_MotorsHeli_Quad::var_info; + AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI); + break; + + case AP_Motors::MOTOR_FRAME_HELI: + default: + motors = new AP_MotorsHeli_Single(copter.scheduler.get_loop_rate_hz()); + motors_var_info = AP_MotorsHeli_Single::var_info; + AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI); + break; +#endif + } + if (motors == nullptr) { + AP_HAL::panic("Unable to allocate FRAME_CLASS=%u", (unsigned)g2.frame_class.get()); + } + AP_Param::load_object_from_eeprom(motors, motors_var_info); + + ahrs_view = ahrs.create_view(ROTATION_NONE); + if (ahrs_view == nullptr) { + AP_HAL::panic("Unable to allocate AP_AHRS_View"); + } + + const struct AP_Param::GroupInfo *ac_var_info; + +#if FRAME_CONFIG != HELI_FRAME + attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s()); + ac_var_info = AC_AttitudeControl_Multi::var_info; +#else + attitude_control = new AC_AttitudeControl_Heli(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s()); + ac_var_info = AC_AttitudeControl_Heli::var_info; +#endif + if (attitude_control == nullptr) { + AP_HAL::panic("Unable to allocate AttitudeControl"); + } + AP_Param::load_object_from_eeprom(attitude_control, ac_var_info); + + pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control); + if (pos_control == nullptr) { + AP_HAL::panic("Unable to allocate PosControl"); + } + AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info); + +#if AC_OAPATHPLANNER_ENABLED == ENABLED + wp_nav = new AC_WPNav_OA(inertial_nav, *ahrs_view, *pos_control, *attitude_control); +#else + wp_nav = new AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control); +#endif + if (wp_nav == nullptr) { + AP_HAL::panic("Unable to allocate WPNav"); + } + AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info); + + loiter_nav = new AC_Loiter(inertial_nav, *ahrs_view, *pos_control, *attitude_control); + if (loiter_nav == nullptr) { + AP_HAL::panic("Unable to allocate LoiterNav"); + } + AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info); + +#if MODE_CIRCLE_ENABLED == ENABLED + circle_nav = new AC_Circle(inertial_nav, *ahrs_view, *pos_control); + if (circle_nav == nullptr) { + AP_HAL::panic("Unable to allocate CircleNav"); + } + AP_Param::load_object_from_eeprom(circle_nav, circle_nav->var_info); +#endif + + // reload lines from the defaults file that may now be accessible + AP_Param::reload_defaults_file(true); + + // now setup some frame-class specific defaults + switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) { + case AP_Motors::MOTOR_FRAME_Y6: + attitude_control->get_rate_roll_pid().kP().set_default(0.1); + attitude_control->get_rate_roll_pid().kD().set_default(0.006); + attitude_control->get_rate_pitch_pid().kP().set_default(0.1); + attitude_control->get_rate_pitch_pid().kD().set_default(0.006); + attitude_control->get_rate_yaw_pid().kP().set_default(0.15); + attitude_control->get_rate_yaw_pid().kI().set_default(0.015); + break; + case AP_Motors::MOTOR_FRAME_TRI: + attitude_control->get_rate_yaw_pid().filt_D_hz().set_default(100); + break; + default: + break; + } + + // brushed 16kHz defaults to 16kHz pulses + if (motors->get_pwm_type() == AP_Motors::PWM_TYPE_BRUSHED) { + g.rc_speed.set_default(16000); + } + + if (upgrading_frame_params) { + // do frame specific upgrade. This is only done the first time we run the new firmware +#if FRAME_CONFIG == HELI_FRAME + SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 12, CH_1); + SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 13, CH_2); + SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 14, CH_3); + SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 15, CH_4); +#else + if (g2.frame_class == AP_Motors::MOTOR_FRAME_TRI) { + const AP_Param::ConversionInfo tri_conversion_info[] = { + { Parameters::k_param_motors, 32, AP_PARAM_INT16, "SERVO7_TRIM" }, + { Parameters::k_param_motors, 33, AP_PARAM_INT16, "SERVO7_MIN" }, + { Parameters::k_param_motors, 34, AP_PARAM_INT16, "SERVO7_MAX" }, + { Parameters::k_param_motors, 35, AP_PARAM_FLOAT, "MOT_YAW_SV_ANGLE" }, + }; + // we need to use CONVERT_FLAG_FORCE as the SERVO7_* parameters will already be set from RC7_* + AP_Param::convert_old_parameters(tri_conversion_info, ARRAY_SIZE(tri_conversion_info), AP_Param::CONVERT_FLAG_FORCE); + const AP_Param::ConversionInfo tri_conversion_info_rev { Parameters::k_param_motors, 31, AP_PARAM_INT8, "SERVO7_REVERSED" }; + AP_Param::convert_old_parameter(&tri_conversion_info_rev, 1, AP_Param::CONVERT_FLAG_REVERSE | AP_Param::CONVERT_FLAG_FORCE); + // AP_MotorsTri was converted from having nested var_info to one level + AP_Param::convert_parent_class(Parameters::k_param_motors, motors, motors->var_info); + } +#endif + } + + // upgrade parameters. This must be done after allocating the objects + convert_pid_parameters(); +#if FRAME_CONFIG == HELI_FRAME + convert_tradheli_parameters(); +#endif +} + +bool Copter::is_tradheli() const +{ +#if FRAME_CONFIG == HELI_FRAME + return true; +#else + return false; +#endif +} diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index bbb58285f0..451c50c1a6 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1,1666 +1,1673 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ -#include "AP_GPS.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "AP_GPS_NOVA.h" -#include "AP_GPS_ERB.h" -#include "AP_GPS_GSOF.h" -#include "AP_GPS_MTK.h" -#include "AP_GPS_MTK19.h" -#include "AP_GPS_NMEA.h" -#include "AP_GPS_SBF.h" -#include "AP_GPS_SBP.h" -#include "AP_GPS_SBP2.h" -#include "AP_GPS_SIRF.h" -#include "AP_GPS_UBLOX.h" -#include "AP_GPS_MAV.h" -#include "GPS_Backend.h" - -#if HAL_WITH_UAVCAN -#include -#include -#include "AP_GPS_UAVCAN.h" -#endif - -#include -#include - -#define GPS_RTK_INJECT_TO_ALL 127 -#define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms -#define GPS_BAUD_TIME_MS 1200 -#define GPS_TIMEOUT_MS 4000u - -// defines used to specify the mask position for use of different accuracy metrics in the blending algorithm -#define BLEND_MASK_USE_HPOS_ACC 1 -#define BLEND_MASK_USE_VPOS_ACC 2 -#define BLEND_MASK_USE_SPD_ACC 4 -#define BLEND_COUNTER_FAILURE_INCREMENT 10 - -extern const AP_HAL::HAL &hal; - -// baudrates to try to detect GPSes with -const uint32_t AP_GPS::_baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U}; - -// initialisation blobs to send to the GPS to try to get it into the -// right mode -const char AP_GPS::_initialisation_blob[] = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY; - -AP_GPS *AP_GPS::_singleton; - -// table of user settable parameters -const AP_Param::GroupInfo AP_GPS::var_info[] = { - // @Param: TYPE - // @DisplayName: GPS type - // @Description: GPS type - // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA - // @RebootRequired: True - // @User: Advanced - AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT), - -#if GPS_MAX_RECEIVERS > 1 - // @Param: TYPE2 - // @DisplayName: 2nd GPS type - // @Description: GPS type of 2nd GPS - // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA - // @RebootRequired: True - // @User: Advanced - AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0), -#endif - - // @Param: NAVFILTER - // @DisplayName: Navigation filter setting - // @Description: Navigation filter engine setting - // @Values: 0:Portable,2:Stationary,3:Pedestrian,4:Automotive,5:Sea,6:Airborne1G,7:Airborne2G,8:Airborne4G - // @User: Advanced - AP_GROUPINFO("NAVFILTER", 2, AP_GPS, _navfilter, GPS_ENGINE_AIRBORNE_4G), - -#if GPS_MAX_RECEIVERS > 1 - // @Param: AUTO_SWITCH - // @DisplayName: Automatic Switchover Setting - // @Description: Automatic switchover to GPS reporting best lock - // @Values: 0:Disabled,1:UseBest,2:Blend,3:UseSecond - // @User: Advanced - AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, 1), -#endif - - // @Param: MIN_DGPS - // @DisplayName: Minimum Lock Type Accepted for DGPS - // @Description: Sets the minimum type of differential GPS corrections required before allowing to switch into DGPS mode. - // @Values: 0:Any,50:FloatRTK,100:IntegerRTK - // @User: Advanced - // @RebootRequired: True - AP_GROUPINFO("MIN_DGPS", 4, AP_GPS, _min_dgps, 100), - - // @Param: SBAS_MODE - // @DisplayName: SBAS Mode - // @Description: This sets the SBAS (satellite based augmentation system) mode if available on this GPS. If set to 2 then the SBAS mode is not changed in the GPS. Otherwise the GPS will be reconfigured to enable/disable SBAS. Disabling SBAS may be worthwhile in some parts of the world where an SBAS signal is available but the baseline is too long to be useful. - // @Values: 0:Disabled,1:Enabled,2:NoChange - // @User: Advanced - AP_GROUPINFO("SBAS_MODE", 5, AP_GPS, _sbas_mode, 2), - - // @Param: MIN_ELEV - // @DisplayName: Minimum elevation - // @Description: This sets the minimum elevation of satellites above the horizon for them to be used for navigation. Setting this to -100 leaves the minimum elevation set to the GPS modules default. - // @Range: -100 90 - // @Units: deg - // @User: Advanced - AP_GROUPINFO("MIN_ELEV", 6, AP_GPS, _min_elevation, -100), - - // @Param: INJECT_TO - // @DisplayName: Destination for GPS_INJECT_DATA MAVLink packets - // @Description: The GGS can send raw serial packets to inject data to multiple GPSes. - // @Values: 0:send to first GPS,1:send to 2nd GPS,127:send to all - // @User: Advanced - AP_GROUPINFO("INJECT_TO", 7, AP_GPS, _inject_to, GPS_RTK_INJECT_TO_ALL), - - // @Param: SBP_LOGMASK - // @DisplayName: Swift Binary Protocol Logging Mask - // @Description: Masked with the SBP msg_type field to determine whether SBR1/SBR2 data is logged - // @Values: 0:None (0x0000),-1:All (0xFFFF),-256:External only (0xFF00) - // @User: Advanced - AP_GROUPINFO("SBP_LOGMASK", 8, AP_GPS, _sbp_logmask, -256), - - // @Param: RAW_DATA - // @DisplayName: Raw data logging - // @Description: Handles logging raw data; on uBlox chips that support raw data this will log RXM messages into logger; on Septentrio this will log on the equipment's SD card and when set to 2, the autopilot will try to stop logging after disarming and restart after arming - // @Values: 0:Ignore,1:Always log,2:Stop logging when disarmed (SBF only),5:Only log every five samples (uBlox only) - // @RebootRequired: True - // @User: Advanced - AP_GROUPINFO("RAW_DATA", 9, AP_GPS, _raw_data, 0), - - // @Param: GNSS_MODE - // @DisplayName: GNSS system configuration - // @Description: Bitmask for what GNSS system to use on the first GPS (all unchecked or zero to leave GPS as configured) - // @Values: 0:Leave as currently configured, 1:GPS-NoSBAS, 3:GPS+SBAS, 4:Galileo-NoSBAS, 6:Galileo+SBAS, 8:Beidou, 51:GPS+IMES+QZSS+SBAS (Japan Only), 64:GLONASS, 66:GLONASS+SBAS, 67:GPS+GLONASS+SBAS - // @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS - // @User: Advanced - AP_GROUPINFO("GNSS_MODE", 10, AP_GPS, _gnss_mode[0], 0), - - // @Param: SAVE_CFG - // @DisplayName: Save GPS configuration - // @Description: Determines whether the configuration for this GPS should be written to non-volatile memory on the GPS. Currently working for UBlox 6 series and above. - // @Values: 0:Do not save config,1:Save config,2:Save only when needed - // @User: Advanced - AP_GROUPINFO("SAVE_CFG", 11, AP_GPS, _save_config, 2), - -#if GPS_MAX_RECEIVERS > 1 - // @Param: GNSS_MODE2 - // @DisplayName: GNSS system configuration - // @Description: Bitmask for what GNSS system to use on the second GPS (all unchecked or zero to leave GPS as configured) - // @Values: 0:Leave as currently configured, 1:GPS-NoSBAS, 3:GPS+SBAS, 4:Galileo-NoSBAS, 6:Galileo+SBAS, 8:Beidou, 51:GPS+IMES+QZSS+SBAS (Japan Only), 64:GLONASS, 66:GLONASS+SBAS, 67:GPS+GLONASS+SBAS - // @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS - // @User: Advanced - AP_GROUPINFO("GNSS_MODE2", 12, AP_GPS, _gnss_mode[1], 0), -#endif - - // @Param: AUTO_CONFIG - // @DisplayName: Automatic GPS configuration - // @Description: Controls if the autopilot should automatically configure the GPS based on the parameters and default settings - // @Values: 0:Disables automatic configuration,1:Enable automatic configuration - // @User: Advanced - AP_GROUPINFO("AUTO_CONFIG", 13, AP_GPS, _auto_config, 1), - - // @Param: RATE_MS - // @DisplayName: GPS update rate in milliseconds - // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed - // @Units: ms - // @Values: 100:10Hz,125:8Hz,200:5Hz - // @Range: 50 200 - // @User: Advanced - AP_GROUPINFO("RATE_MS", 14, AP_GPS, _rate_ms[0], 200), - -#if GPS_MAX_RECEIVERS > 1 - // @Param: RATE_MS2 - // @DisplayName: GPS 2 update rate in milliseconds - // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed - // @Units: ms - // @Values: 100:10Hz,125:8Hz,200:5Hz - // @Range: 50 200 - // @User: Advanced - AP_GROUPINFO("RATE_MS2", 15, AP_GPS, _rate_ms[1], 200), -#endif - - // @Param: POS1_X - // @DisplayName: Antenna X position offset - // @Description: X position of the first GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer. - // @Units: m - // @Range: -10 10 - // @User: Advanced - - // @Param: POS1_Y - // @DisplayName: Antenna Y position offset - // @Description: Y position of the first GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer. - // @Units: m - // @Range: -10 10 - // @User: Advanced - - // @Param: POS1_Z - // @DisplayName: Antenna Z position offset - // @Description: Z position of the first GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer. - // @Units: m - // @Range: -10 10 - // @User: Advanced - AP_GROUPINFO("POS1", 16, AP_GPS, _antenna_offset[0], 0.0f), - - // @Param: POS2_X - // @DisplayName: Antenna X position offset - // @Description: X position of the second GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer. - // @Units: m - // @Range: -10 10 - // @User: Advanced - - // @Param: POS2_Y - // @DisplayName: Antenna Y position offset - // @Description: Y position of the second GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer. - // @Units: m - // @Range: -10 10 - // @User: Advanced - -#if GPS_MAX_RECEIVERS > 1 - // @Param: POS2_Z - // @DisplayName: Antenna Z position offset - // @Description: Z position of the second GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer. - // @Units: m - // @Range: -10 10 - // @User: Advanced - AP_GROUPINFO("POS2", 17, AP_GPS, _antenna_offset[1], 0.0f), -#endif - - // @Param: DELAY_MS - // @DisplayName: GPS delay in milliseconds - // @Description: Controls the amount of GPS measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type. - // @Units: ms - // @Range: 0 250 - // @User: Advanced - // @RebootRequired: True - AP_GROUPINFO("DELAY_MS", 18, AP_GPS, _delay_ms[0], 0), - -#if GPS_MAX_RECEIVERS > 1 - // @Param: DELAY_MS2 - // @DisplayName: GPS 2 delay in milliseconds - // @Description: Controls the amount of GPS measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type. - // @Units: ms - // @Range: 0 250 - // @User: Advanced - // @RebootRequired: True - AP_GROUPINFO("DELAY_MS2", 19, AP_GPS, _delay_ms[1], 0), -#endif - -#if defined(GPS_BLENDED_INSTANCE) - // @Param: BLEND_MASK - // @DisplayName: Multi GPS Blending Mask - // @Description: Determines which of the accuracy measures Horizontal position, Vertical Position and Speed are used to calculate the weighting on each GPS receiver when soft switching has been selected by setting GPS_AUTO_SWITCH to 2 - // @Bitmask: 0:Horiz Pos,1:Vert Pos,2:Speed - // @User: Advanced - AP_GROUPINFO("BLEND_MASK", 20, AP_GPS, _blend_mask, 5), - - // @Param: BLEND_TC - // @DisplayName: Blending time constant - // @Description: Controls the slowest time constant applied to the calculation of GPS position and height offsets used to adjust different GPS receivers for steady state position differences. - // @Units: s - // @Range: 5.0 30.0 - // @User: Advanced - AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f), -#endif - - AP_GROUPEND -}; - -// constructor -AP_GPS::AP_GPS() -{ - static_assert((sizeof(_initialisation_blob) * (CHAR_BIT + 2)) < (4800 * GPS_BAUD_TIME_MS * 1e-3), - "GPS initilisation blob is too large to be completely sent before the baud rate changes"); - - AP_Param::setup_object_defaults(this, var_info); - - if (_singleton != nullptr) { - AP_HAL::panic("AP_GPS must be singleton"); - } - _singleton = this; -} - -// return true if a specific type of GPS uses a UART -bool AP_GPS::needs_uart(GPS_Type type) const -{ - switch (type) { - case GPS_TYPE_NONE: - case GPS_TYPE_HIL: - case GPS_TYPE_UAVCAN: - case GPS_TYPE_MAV: - return false; - default: - break; - } - return true; -} - -/// Startup initialisation. -void AP_GPS::init(const AP_SerialManager& serial_manager) -{ - primary_instance = 0; - - // search for serial ports with gps protocol - uint8_t uart_idx = 0; - for (uint8_t i=0; i GPS_MAX_RATE_MS) { - _rate_ms[i] = GPS_MAX_RATE_MS; - } - } -} - -// return number of active GPS sensors. Note that if the first GPS -// is not present but the 2nd is then we return 2. Note that a blended -// GPS solution is treated as an additional sensor. -uint8_t AP_GPS::num_sensors(void) const -{ - if (!_output_is_blended) { - return num_instances; - } else { - return num_instances+1; - } -} - -bool AP_GPS::speed_accuracy(uint8_t instance, float &sacc) const -{ - if (state[instance].have_speed_accuracy) { - sacc = state[instance].speed_accuracy; - return true; - } - return false; -} - -bool AP_GPS::horizontal_accuracy(uint8_t instance, float &hacc) const -{ - if (state[instance].have_horizontal_accuracy) { - hacc = state[instance].horizontal_accuracy; - return true; - } - return false; -} - -bool AP_GPS::vertical_accuracy(uint8_t instance, float &vacc) const -{ - if (state[instance].have_vertical_accuracy) { - vacc = state[instance].vertical_accuracy; - return true; - } - return false; -} - - -/** - convert GPS week and milliseconds to unix epoch in milliseconds - */ -uint64_t AP_GPS::time_epoch_convert(uint16_t gps_week, uint32_t gps_ms) -{ - uint64_t fix_time_ms = UNIX_OFFSET_MSEC + gps_week * AP_MSEC_PER_WEEK + gps_ms; - return fix_time_ms; -} - -/** - calculate current time since the unix epoch in microseconds - */ -uint64_t AP_GPS::time_epoch_usec(uint8_t instance) const -{ - const GPS_State &istate = state[instance]; - if (istate.last_gps_time_ms == 0 || istate.time_week == 0) { - return 0; - } - uint64_t fix_time_ms = time_epoch_convert(istate.time_week, istate.time_week_ms); - // add in the milliseconds since the last fix - return (fix_time_ms + (AP_HAL::millis() - istate.last_gps_time_ms)) * 1000ULL; -} - -/* - send some more initialisation string bytes if there is room in the - UART transmit buffer - */ -void AP_GPS::send_blob_start(uint8_t instance, const char *_blob, uint16_t size) -{ - initblob_state[instance].blob = _blob; - initblob_state[instance].remaining = size; -} - -/* - send some more initialisation string bytes if there is room in the - UART transmit buffer - */ -void AP_GPS::send_blob_update(uint8_t instance) -{ - // exit immediately if no uart for this instance - if (_port[instance] == nullptr) { - return; - } - - // see if we can write some more of the initialisation blob - if (initblob_state[instance].remaining > 0) { - int16_t space = _port[instance]->txspace(); - if (space > (int16_t)initblob_state[instance].remaining) { - space = initblob_state[instance].remaining; - } - while (space > 0) { - _port[instance]->write(*initblob_state[instance].blob); - initblob_state[instance].blob++; - space--; - initblob_state[instance].remaining--; - } - } -} - -/* - run detection step for one GPS instance. If this finds a GPS then it - will fill in drivers[instance] and change state[instance].status - from NO_GPS to NO_FIX. - */ -void AP_GPS::detect_instance(uint8_t instance) -{ - AP_GPS_Backend *new_gps = nullptr; - struct detect_state *dstate = &detect_state[instance]; - const uint32_t now = AP_HAL::millis(); - - state[instance].status = NO_GPS; - state[instance].hdop = GPS_UNKNOWN_DOP; - state[instance].vdop = GPS_UNKNOWN_DOP; - - switch (_type[instance]) { - // user has to explicitly set the MAV type, do not use AUTO - // do not try to detect the MAV type, assume it's there - case GPS_TYPE_MAV: -#ifndef HAL_BUILD_AP_PERIPH - dstate->auto_detected_baud = false; // specified, not detected - new_gps = new AP_GPS_MAV(*this, state[instance], nullptr); - goto found_gps; -#endif - break; - - // user has to explicitly set the UAVCAN type, do not use AUTO - case GPS_TYPE_UAVCAN: -#if HAL_WITH_UAVCAN - dstate->auto_detected_baud = false; // specified, not detected - new_gps = AP_GPS_UAVCAN::probe(*this, state[instance]); - goto found_gps; -#endif - return; // We don't do anything here if UAVCAN is not supported - default: - break; - } - - if (_port[instance] == nullptr) { - // UART not available - return; - } - - // all remaining drivers automatically cycle through baud rates to detect - // the correct baud rate, and should have the selected baud broadcast - dstate->auto_detected_baud = true; - -#ifndef HAL_BUILD_AP_PERIPH - switch (_type[instance]) { - // by default the sbf/trimble gps outputs no data on its port, until configured. - case GPS_TYPE_SBF: - new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]); - break; - - case GPS_TYPE_GSOF: - new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]); - break; - - case GPS_TYPE_NOVA: - new_gps = new AP_GPS_NOVA(*this, state[instance], _port[instance]); - break; - - default: - break; - } -#endif // HAL_BUILD_AP_PERIPH - - if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) { - // try the next baud rate - // incrementing like this will skip the first element in array of bauds - // this is okay, and relied upon - dstate->current_baud++; - if (dstate->current_baud == ARRAY_SIZE(_baudrates)) { - dstate->current_baud = 0; - } - uint32_t baudrate = _baudrates[dstate->current_baud]; - _port[instance]->begin(baudrate); - _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); - dstate->last_baud_change_ms = now; - - if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) { - if (_type[instance] == GPS_TYPE_HEMI) { - send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); - } else { - send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); - } - } - } - - if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) { - send_blob_update(instance); - } - - while (initblob_state[instance].remaining == 0 && _port[instance]->available() > 0 - && new_gps == nullptr) { - uint8_t data = _port[instance]->read(); - /* - running a uBlox at less than 38400 will lead to packet - corruption, as we can't receive the packets in the 200ms - window for 5Hz fixes. The NMEA startup message should force - the uBlox into 115200 no matter what rate it is configured - for. - */ - if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) && - ((!_auto_config && _baudrates[dstate->current_baud] >= 38400) || - _baudrates[dstate->current_baud] == 115200) && - AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) { - new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance]); - } -#ifndef HAL_BUILD_AP_PERIPH -#if !HAL_MINIMIZE_FEATURES - // we drop the MTK drivers when building a small build as they are so rarely used - // and are surprisingly large - else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK19) && - AP_GPS_MTK19::_detect(dstate->mtk19_detect_state, data)) { - new_gps = new AP_GPS_MTK19(*this, state[instance], _port[instance]); - } else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK) && - AP_GPS_MTK::_detect(dstate->mtk_detect_state, data)) { - new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]); - } -#endif - else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && - AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) { - new_gps = new AP_GPS_SBP2(*this, state[instance], _port[instance]); - } - else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && - AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) { - new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]); - } -#if !HAL_MINIMIZE_FEATURES - else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) && - AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) { - new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]); - } -#endif - else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) && - AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) { - new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]); - } else if ((_type[instance] == GPS_TYPE_NMEA || - _type[instance] == GPS_TYPE_HEMI) && - AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) { - new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]); - } -#endif // HAL_BUILD_AP_PERIPH - if (new_gps) { - goto found_gps; - } - } - -found_gps: - if (new_gps != nullptr) { - state[instance].status = NO_FIX; - drivers[instance] = new_gps; - timing[instance].last_message_time_ms = now; - timing[instance].delta_time_ms = GPS_TIMEOUT_MS; - new_gps->broadcast_gps_type(); - } -} - -AP_GPS::GPS_Status AP_GPS::highest_supported_status(uint8_t instance) const -{ - if (instance < GPS_MAX_RECEIVERS && drivers[instance] != nullptr) { - return drivers[instance]->highest_supported_status(); - } - return AP_GPS::GPS_OK_FIX_3D; -} - -bool AP_GPS::should_log() const -{ -#ifndef HAL_BUILD_AP_PERIPH - AP_Logger *logger = AP_Logger::get_singleton(); - if (logger == nullptr) { - return false; - } - if (_log_gps_bit == (uint32_t)-1) { - return false; - } - if (!logger->should_log(_log_gps_bit)) { - return false; - } - return true; -#else - return false; -#endif -} - - -/* - update one GPS instance. This should be called at 10Hz or greater - */ -void AP_GPS::update_instance(uint8_t instance) -{ - if (_type[instance] == GPS_TYPE_HIL) { - // in HIL, leave info alone - return; - } - if (_type[instance] == GPS_TYPE_NONE) { - // not enabled - state[instance].status = NO_GPS; - state[instance].hdop = GPS_UNKNOWN_DOP; - state[instance].vdop = GPS_UNKNOWN_DOP; - return; - } - if (locked_ports & (1U<read(); - uint32_t tnow = AP_HAL::millis(); - - // if we did not get a message, and the idle timer of 2 seconds - // has expired, re-initialise the GPS. This will cause GPS - // detection to run again - bool data_should_be_logged = false; - if (!result) { - if (tnow - timing[instance].last_message_time_ms > GPS_TIMEOUT_MS) { - memset((void *)&state[instance], 0, sizeof(state[instance])); - state[instance].instance = instance; - state[instance].hdop = GPS_UNKNOWN_DOP; - state[instance].vdop = GPS_UNKNOWN_DOP; - timing[instance].last_message_time_ms = tnow; - timing[instance].delta_time_ms = GPS_TIMEOUT_MS; - // do not try to detect again if type is MAV - if (_type[instance] == GPS_TYPE_MAV) { - state[instance].status = NO_FIX; - } else { - // free the driver before we run the next detection, so we - // don't end up with two allocated at any time - delete drivers[instance]; - drivers[instance] = nullptr; - state[instance].status = NO_GPS; - } - // log this data as a "flag" that the GPS is no longer - // valid (see PR#8144) - data_should_be_logged = true; - } - } else { - if (state[instance].uart_timestamp_ms != 0) { - // set the timestamp for this messages based on - // set_uart_timestamp() in backend, if available - tnow = state[instance].uart_timestamp_ms; - state[instance].uart_timestamp_ms = 0; - } - // delta will only be correct after parsing two messages - timing[instance].delta_time_ms = tnow - timing[instance].last_message_time_ms; - timing[instance].last_message_time_ms = tnow; - if (state[instance].status >= GPS_OK_FIX_2D) { - timing[instance].last_fix_time_ms = tnow; - } - - data_should_be_logged = true; - } - -#ifndef HAL_BUILD_AP_PERIPH - if (data_should_be_logged && - (should_log() || AP::ahrs().have_ekf_logging())) { - AP::logger().Write_GPS(instance); - } - - if (state[instance].status >= GPS_OK_FIX_3D) { - const uint64_t now = time_epoch_usec(instance); - if (now != 0) { - AP::rtc().set_utc_usec(now, AP_RTC::SOURCE_GPS); - } - } -#else - (void)data_should_be_logged; -#endif -} - -/* - update all GPS instances - */ -void AP_GPS::update(void) -{ - for (uint8_t i=0; i 0) { - _blend_health_counter--; - } - // stop blending if unhealthy - if (_blend_health_counter >= 50) { - _output_is_blended = false; - } - } else { - _output_is_blended = false; - _blend_health_counter = 0; - } - - if (_output_is_blended) { - // Use the weighting to calculate blended GPS states - calc_blended_state(); - // set primary to the virtual instance - primary_instance = GPS_BLENDED_INSTANCE; - } else { - // use switch logic to find best GPS - uint32_t now = AP_HAL::millis(); - if (_auto_switch == 3) { - // select the second GPS instance - primary_instance = 1; - } else if (_auto_switch >= 1) { - // handling switching away from blended GPS - if (primary_instance == GPS_BLENDED_INSTANCE) { - primary_instance = 0; - for (uint8_t i=1; i state[primary_instance].status) || - ((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) { - primary_instance = i; - _last_instance_swap_ms = now; - } - } - } else { - // handle switch between real GPSs - for (uint8_t i=0; i state[primary_instance].status) { - // we have a higher status lock, or primary is set to the blended GPS, change GPS - primary_instance = i; - _last_instance_swap_ms = now; - continue; - } - - bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1); - - if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) { - - bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2); - - if ((another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) || - (another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000)) { - // this GPS has more satellites than the - // current primary, switch primary. Once we switch we will - // then tend to stick to the new GPS as primary. We don't - // want to switch too often as it will look like a - // position shift to the controllers. - primary_instance = i; - _last_instance_swap_ms = now; - } - } - } - } - } else { - // AUTO_SWITCH is 0 so no switching of GPSs - primary_instance = 0; - } - - // copy the primary instance to the blended instance in case it is enabled later - state[GPS_BLENDED_INSTANCE] = state[primary_instance]; - _blended_antenna_offset = _antenna_offset[primary_instance]; - } -#endif // GPS_BLENDED_INSTANCE - -#ifndef HAL_BUILD_AP_PERIPH - // update notify with gps status. We always base this on the primary_instance - AP_Notify::flags.gps_status = state[primary_instance].status; - AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats; -#endif -} - -void AP_GPS::handle_gps_inject(const mavlink_message_t &msg) -{ - mavlink_gps_inject_data_t packet; - mavlink_msg_gps_inject_data_decode(&msg, &packet); - - if (packet.len > sizeof(packet.data)) { - // invalid packet - return; - } - - handle_gps_rtcm_fragment(0, packet.data, packet.len); -} - -/* - pass along a mavlink message (for MAV type) - */ -void AP_GPS::handle_msg(const mavlink_message_t &msg) -{ - switch (msg.msgid) { - case MAVLINK_MSG_ID_GPS_RTCM_DATA: - // pass data to de-fragmenter - handle_gps_rtcm_data(msg); - break; - case MAVLINK_MSG_ID_GPS_INJECT_DATA: - handle_gps_inject(msg); - break; - default: { - uint8_t i; - for (i=0; ihandle_msg(msg); - } - } - break; - } - } -} - -/* - set HIL (hardware in the loop) status for a GPS instance - */ -void AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms, - const Location &_location, const Vector3f &_velocity, uint8_t _num_sats, - uint16_t hdop) -{ - if (instance >= GPS_MAX_RECEIVERS) { - return; - } - const uint32_t tnow = AP_HAL::millis(); - GPS_State &istate = state[instance]; - istate.status = _status; - istate.location = _location; - istate.velocity = _velocity; - istate.ground_speed = norm(istate.velocity.x, istate.velocity.y); - istate.ground_course = wrap_360(degrees(atan2f(istate.velocity.y, istate.velocity.x))); - istate.hdop = hdop; - istate.num_sats = _num_sats; - istate.last_gps_time_ms = tnow; - uint64_t gps_time_ms = time_epoch_ms - UNIX_OFFSET_MSEC; - istate.time_week = gps_time_ms / AP_MSEC_PER_WEEK; - istate.time_week_ms = gps_time_ms - istate.time_week * AP_MSEC_PER_WEEK; - timing[instance].last_message_time_ms = tnow; - timing[instance].last_fix_time_ms = tnow; - _type[instance].set(GPS_TYPE_HIL); -} - -// set accuracy for HIL -void AP_GPS::setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc, bool _have_vertical_velocity, uint32_t sample_ms) -{ - if (instance >= GPS_MAX_RECEIVERS) { - return; - } - GPS_State &istate = state[instance]; - istate.vdop = vdop * 100; - istate.horizontal_accuracy = hacc; - istate.vertical_accuracy = vacc; - istate.speed_accuracy = sacc; - istate.have_horizontal_accuracy = true; - istate.have_vertical_accuracy = true; - istate.have_speed_accuracy = true; - istate.have_vertical_velocity |= _have_vertical_velocity; - if (sample_ms != 0) { - timing[instance].last_message_time_ms = sample_ms; - timing[instance].last_fix_time_ms = sample_ms; - } -} - -/** - Lock a GPS port, preventing the GPS driver from using it. This can - be used to allow a user to control a GPS port via the - SERIAL_CONTROL protocol - */ -void AP_GPS::lock_port(uint8_t instance, bool lock) -{ - - if (instance >= GPS_MAX_RECEIVERS) { - return; - } - if (lock) { - locked_ports |= (1U<inject_data(data, len); - } -} - -void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) -{ - static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS]; - if (status(0) > AP_GPS::NO_GPS) { - // when we have a GPS then only send new data - if (last_send_time_ms[chan] == last_message_time_ms(0)) { - return; - } - last_send_time_ms[chan] = last_message_time_ms(0); - } else { - // when we don't have a GPS then send at 1Hz - uint32_t now = AP_HAL::millis(); - if (now - last_send_time_ms[chan] < 1000) { - return; - } - last_send_time_ms[chan] = now; - } - const Location &loc = location(0); - float hacc = 0.0f; - float vacc = 0.0f; - float sacc = 0.0f; - horizontal_accuracy(0, hacc); - vertical_accuracy(0, vacc); - speed_accuracy(0, sacc); - mavlink_msg_gps_raw_int_send( - chan, - last_fix_time_ms(0)*(uint64_t)1000, - status(0), - loc.lat, // in 1E7 degrees - loc.lng, // in 1E7 degrees - loc.alt * 10UL, // in mm - get_hdop(0), - get_vdop(0), - ground_speed(0)*100, // cm/s - ground_course(0)*100, // 1/100 degrees, - num_sats(0), - 0, // TODO: Elipsoid height in mm - hacc * 1000, // one-sigma standard deviation in mm - vacc * 1000, // one-sigma standard deviation in mm - sacc * 1000, // one-sigma standard deviation in mm/s - 0); // TODO one-sigma heading accuracy standard deviation -} - -#if GPS_MAX_RECEIVERS > 1 -void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) -{ - static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS]; - if (num_instances < 2 || status(1) <= AP_GPS::NO_GPS) { - return; - } - // when we have a GPS then only send new data - if (last_send_time_ms[chan] == last_message_time_ms(1)) { - return; - } - last_send_time_ms[chan] = last_message_time_ms(1); - - const Location &loc = location(1); - mavlink_msg_gps2_raw_send( - chan, - last_fix_time_ms(1)*(uint64_t)1000, - status(1), - loc.lat, - loc.lng, - loc.alt * 10UL, - get_hdop(1), - get_vdop(1), - ground_speed(1)*100, // cm/s - ground_course(1)*100, // 1/100 degrees, - num_sats(1), - state[1].rtk_num_sats, - state[1].rtk_age_ms); -} -#endif // GPS_MAX_RECEIVERS - -void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst) -{ - if (inst >= GPS_MAX_RECEIVERS) { - return; - } - if (drivers[inst] != nullptr && drivers[inst]->supports_mavlink_gps_rtk_message()) { - drivers[inst]->send_mavlink_gps_rtk(chan); - } -} - -bool AP_GPS::first_unconfigured_gps(uint8_t &instance) const -{ - for (int i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (_type[i] != GPS_TYPE_NONE && (drivers[i] == nullptr || !drivers[i]->is_configured())) { - instance = i; - return true; - } - } - return false; -} - -void AP_GPS::broadcast_first_configuration_failure_reason(void) const -{ - uint8_t unconfigured; - if (first_unconfigured_gps(unconfigured)) { - if (drivers[unconfigured] == nullptr) { - gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: was not found", unconfigured + 1); - } else { - drivers[unconfigured]->broadcast_configuration_failure_reason(); - } - } -} - -// pre-arm check that all GPSs are close to each other. farthest distance between GPSs (in meters) is returned -bool AP_GPS::all_consistent(float &distance) const -{ - // return true immediately if only one valid receiver - if (num_instances <= 1 || - drivers[0] == nullptr || _type[0] == GPS_TYPE_NONE) { - distance = 0; - return true; - } - - // calculate distance - distance = state[0].location.get_distance_NED(state[1].location).length(); - // success if distance is within 50m - return (distance < 50); -} - -// pre-arm check of GPS blending. True means healthy or that blending is not being used -bool AP_GPS::blend_health_check() const -{ - return (_blend_health_counter < 50); -} - -/* - re-assemble fragmented RTCM data - */ -void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len) -{ - if ((flags & 1) == 0) { - // it is not fragmented, pass direct - inject_data(data, len); - return; - } - - // see if we need to allocate re-assembly buffer - if (rtcm_buffer == nullptr) { - rtcm_buffer = (struct rtcm_buffer *)calloc(1, sizeof(*rtcm_buffer)); - if (rtcm_buffer == nullptr) { - // nothing to do but discard the data - return; - } - } - - uint8_t fragment = (flags >> 1U) & 0x03; - uint8_t sequence = (flags >> 3U) & 0x1F; - - // see if this fragment is consistent with existing fragments - if (rtcm_buffer->fragments_received && - (rtcm_buffer->sequence != sequence || - (rtcm_buffer->fragments_received & (1U<sequence = sequence; - rtcm_buffer->fragments_received |= (1U << fragment); - - // copy the data - memcpy(&rtcm_buffer->buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*(uint16_t)fragment], data, len); - - // when we get a fragment of less than max size then we know the - // number of fragments. Note that this means if you want to send a - // block of RTCM data of an exact multiple of the buffer size you - // need to send a final packet of zero length - if (len < MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN) { - rtcm_buffer->fragment_count = fragment+1; - rtcm_buffer->total_length = (MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*fragment) + len; - } else if (rtcm_buffer->fragments_received == 0x0F) { - // special case of 4 full fragments - rtcm_buffer->fragment_count = 4; - rtcm_buffer->total_length = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*4; - } - - - // see if we have all fragments - if (rtcm_buffer->fragment_count != 0 && - rtcm_buffer->fragments_received == (1U << rtcm_buffer->fragment_count) - 1) { - // we have them all, inject - inject_data(rtcm_buffer->buffer, rtcm_buffer->total_length); - memset(rtcm_buffer, 0, sizeof(*rtcm_buffer)); - } -} - -/* - re-assemble GPS_RTCM_DATA message - */ -void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg) -{ - mavlink_gps_rtcm_data_t packet; - mavlink_msg_gps_rtcm_data_decode(&msg, &packet); - - if (packet.len > sizeof(packet.data)) { - // invalid packet - return; - } - - handle_gps_rtcm_fragment(packet.flags, packet.data, packet.len); -} - -void AP_GPS::Write_AP_Logger_Log_Startup_messages() -{ - for (uint8_t instance=0; instanceWrite_AP_Logger_Log_Startup_messages(); - } -} - -/* - return the expected lag (in seconds) in the position and velocity readings from the gps - return true if the GPS hardware configuration is known or the delay parameter has been set - */ -bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const -{ - // always enusre a lag is provided - lag_sec = 0.22f; - - if (instance >= GPS_MAX_INSTANCES) { - return false; - } - -#if defined(GPS_BLENDED_INSTANCE) - // return lag of blended GPS - if (instance == GPS_BLENDED_INSTANCE) { - lag_sec = _blended_lag_sec; - // auto switching uses all GPS receivers, so all must be configured - uint8_t inst; // we don't actually care what the number is, but must pass it - return first_unconfigured_gps(inst); - } -#endif - - if (_delay_ms[instance] > 0) { - // if the user has specified a non zero time delay, always return that value - lag_sec = 0.001f * (float)_delay_ms[instance]; - // the user is always right !! - return true; - } else if (drivers[instance] == nullptr || state[instance].status == NO_GPS) { - // no GPS was detected in this instance so return the worst possible lag term - if (_type[instance] == GPS_TYPE_NONE) { - lag_sec = 0.0f; - return true; - } - return _type[instance] == GPS_TYPE_AUTO; - } else { - // the user has not specified a delay so we determine it from the GPS type - return drivers[instance]->get_lag(lag_sec); - } -} - -// return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin -const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const -{ - if (instance >= GPS_MAX_INSTANCES) { - // we have to return a reference so use instance 0 - return _antenna_offset[0]; - } - -#if defined(GPS_BLENDED_INSTANCE) - if (instance == GPS_BLENDED_INSTANCE) { - // return an offset for the blended GPS solution - return _blended_antenna_offset; - } -#endif - - return _antenna_offset[instance]; -} - -/* - returns the desired gps update rate in milliseconds - this does not provide any guarantee that the GPS is updating at the requested - rate it is simply a helper for use in the backends for determining what rate - they should be configuring the GPS to run at -*/ -uint16_t AP_GPS::get_rate_ms(uint8_t instance) const -{ - // sanity check - if (instance >= num_instances || _rate_ms[instance] <= 0) { - return GPS_MAX_RATE_MS; - } - return MIN(_rate_ms[instance], GPS_MAX_RATE_MS); -} - -#if defined(GPS_BLENDED_INSTANCE) -/* - calculate the weightings used to blend GPSs location and velocity data -*/ -bool AP_GPS::calc_blend_weights(void) -{ - // zero the blend weights - memset(&_blend_weights, 0, sizeof(_blend_weights)); - - // exit immediately if not enough receivers to do blending - if (state[0].status <= NO_FIX || state[1].status <= NO_FIX) { - return false; - } - - // Use the oldest non-zero time, but if time difference is excessive, use newest to prevent a disconnected receiver from blocking updates - uint32_t max_ms = 0; // newest non-zero system time of arrival of a GPS message - uint32_t min_ms = -1; // oldest non-zero system time of arrival of a GPS message - int16_t max_rate_ms = 0; // largest update interval of a GPS receiver - for (uint8_t i=0; i max_ms) { - max_ms = state[i].last_gps_time_ms; - } - if ((state[i].last_gps_time_ms < min_ms) && (state[i].last_gps_time_ms > 0)) { - min_ms = state[i].last_gps_time_ms; - } - if (get_rate_ms(i) > max_rate_ms) { - max_rate_ms = get_rate_ms(i); - } - } - if ((int32_t)(max_ms - min_ms) < (int32_t)(2 * max_rate_ms)) { - // data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated - state[GPS_BLENDED_INSTANCE].last_gps_time_ms = min_ms; - } else { - // receiver data has timed out so fail out of blending - return false; - } - - // calculate the sum squared speed accuracy across all GPS sensors - float speed_accuracy_sum_sq = 0.0f; - if (_blend_mask & BLEND_MASK_USE_SPD_ACC) { - for (uint8_t i=0; i= GPS_OK_FIX_3D) { - if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f) { - speed_accuracy_sum_sq += state[i].speed_accuracy * state[i].speed_accuracy; - } else { - // not all receivers support this metric so set it to zero and don't use it - speed_accuracy_sum_sq = 0.0f; - break; - } - } - } - } - - // calculate the sum squared horizontal position accuracy across all GPS sensors - float horizontal_accuracy_sum_sq = 0.0f; - if (_blend_mask & BLEND_MASK_USE_HPOS_ACC) { - for (uint8_t i=0; i= GPS_OK_FIX_2D) { - if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f) { - horizontal_accuracy_sum_sq += state[i].horizontal_accuracy * state[i].horizontal_accuracy; - } else { - // not all receivers support this metric so set it to zero and don't use it - horizontal_accuracy_sum_sq = 0.0f; - break; - } - } - } - } - - // calculate the sum squared vertical position accuracy across all GPS sensors - float vertical_accuracy_sum_sq = 0.0f; - if (_blend_mask & BLEND_MASK_USE_VPOS_ACC) { - for (uint8_t i=0; i= GPS_OK_FIX_3D) { - if (state[i].have_vertical_accuracy && state[i].vertical_accuracy > 0.0f) { - vertical_accuracy_sum_sq += state[i].vertical_accuracy * state[i].vertical_accuracy; - } else { - // not all receivers support this metric so set it to zero and don't use it - vertical_accuracy_sum_sq = 0.0f; - break; - } - } - } - } - // Check if we can do blending using reported accuracy - bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f || speed_accuracy_sum_sq > 0.0f); - - // if we can't do blending using reported accuracy, return false and hard switch logic will be used instead - if (!can_do_blending) { - return false; - } - - float sum_of_all_weights = 0.0f; - - // calculate a weighting using the reported horizontal position - float hpos_blend_weights[GPS_MAX_RECEIVERS] = {}; - if (horizontal_accuracy_sum_sq > 0.0f && (_blend_mask & BLEND_MASK_USE_HPOS_ACC)) { - // calculate the weights using the inverse of the variances - float sum_of_hpos_weights = 0.0f; - for (uint8_t i=0; i= GPS_OK_FIX_2D && state[i].horizontal_accuracy >= 0.001f) { - hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (state[i].horizontal_accuracy * state[i].horizontal_accuracy); - sum_of_hpos_weights += hpos_blend_weights[i]; - } - } - // normalise the weights - if (sum_of_hpos_weights > 0.0f) { - for (uint8_t i=0; i 0.0f && (_blend_mask & BLEND_MASK_USE_VPOS_ACC)) { - // calculate the weights using the inverse of the variances - float sum_of_vpos_weights = 0.0f; - for (uint8_t i=0; i= GPS_OK_FIX_3D && state[i].vertical_accuracy >= 0.001f) { - vpos_blend_weights[i] = vertical_accuracy_sum_sq / (state[i].vertical_accuracy * state[i].vertical_accuracy); - sum_of_vpos_weights += vpos_blend_weights[i]; - } - } - // normalise the weights - if (sum_of_vpos_weights > 0.0f) { - for (uint8_t i=0; i 0.0f && (_blend_mask & BLEND_MASK_USE_SPD_ACC)) { - // calculate the weights using the inverse of the variances - float sum_of_spd_weights = 0.0f; - for (uint8_t i=0; i= GPS_OK_FIX_3D && state[i].speed_accuracy >= 0.001f) { - spd_blend_weights[i] = speed_accuracy_sum_sq / (state[i].speed_accuracy * state[i].speed_accuracy); - sum_of_spd_weights += spd_blend_weights[i]; - } - } - // normalise the weights - if (sum_of_spd_weights > 0.0f) { - for (uint8_t i=0; i state[GPS_BLENDED_INSTANCE].status) { - state[GPS_BLENDED_INSTANCE].status = state[i].status; - } - - // calculate a blended average velocity - state[GPS_BLENDED_INSTANCE].velocity += state[i].velocity * _blend_weights[i]; - - // report the best valid accuracies and DOP metrics - - if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f && state[i].horizontal_accuracy < state[GPS_BLENDED_INSTANCE].horizontal_accuracy) { - state[GPS_BLENDED_INSTANCE].have_horizontal_accuracy = true; - state[GPS_BLENDED_INSTANCE].horizontal_accuracy = state[i].horizontal_accuracy; - } - - if (state[i].have_vertical_accuracy && state[i].vertical_accuracy > 0.0f && state[i].vertical_accuracy < state[GPS_BLENDED_INSTANCE].vertical_accuracy) { - state[GPS_BLENDED_INSTANCE].have_vertical_accuracy = true; - state[GPS_BLENDED_INSTANCE].vertical_accuracy = state[i].vertical_accuracy; - } - - if (state[i].have_vertical_velocity) { - state[GPS_BLENDED_INSTANCE].have_vertical_velocity = true; - } - - if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f && state[i].speed_accuracy < state[GPS_BLENDED_INSTANCE].speed_accuracy) { - state[GPS_BLENDED_INSTANCE].have_speed_accuracy = true; - state[GPS_BLENDED_INSTANCE].speed_accuracy = state[i].speed_accuracy; - } - - if (state[i].hdop > 0 && state[i].hdop < state[GPS_BLENDED_INSTANCE].hdop) { - state[GPS_BLENDED_INSTANCE].hdop = state[i].hdop; - } - - if (state[i].vdop > 0 && state[i].vdop < state[GPS_BLENDED_INSTANCE].vdop) { - state[GPS_BLENDED_INSTANCE].vdop = state[i].vdop; - } - - if (state[i].num_sats > 0 && state[i].num_sats > state[GPS_BLENDED_INSTANCE].num_sats) { - state[GPS_BLENDED_INSTANCE].num_sats = state[i].num_sats; - } - - // report a blended average GPS antenna position - Vector3f temp_antenna_offset = _antenna_offset[i]; - temp_antenna_offset *= _blend_weights[i]; - _blended_antenna_offset += temp_antenna_offset; - - // blend the timing data - if (timing[i].last_fix_time_ms > timing[GPS_BLENDED_INSTANCE].last_fix_time_ms) { - timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = timing[i].last_fix_time_ms; - } - if (timing[i].last_message_time_ms > timing[GPS_BLENDED_INSTANCE].last_message_time_ms) { - timing[GPS_BLENDED_INSTANCE].last_message_time_ms = timing[i].last_message_time_ms; - } - - } - - /* - * Calculate an instantaneous weighted/blended average location from the available GPS instances and store in the _output_state. - * This will be statistically the most likely location, but will be not stable enough for direct use by the autopilot. - */ - - // Use the GPS with the highest weighting as the reference position - float best_weight = 0.0f; - uint8_t best_index = 0; - for (uint8_t i=0; i best_weight) { - best_weight = _blend_weights[i]; - best_index = i; - state[GPS_BLENDED_INSTANCE].location = state[i].location; - } - } - - // Calculate the weighted sum of horizontal and vertical position offsets relative to the reference position - Vector2f blended_NE_offset_m; - float blended_alt_offset_cm = 0.0f; - blended_NE_offset_m.zero(); - for (uint8_t i=0; i 0.0f && i != best_index) { - blended_NE_offset_m += state[GPS_BLENDED_INSTANCE].location.get_distance_NE(state[i].location) * _blend_weights[i]; - blended_alt_offset_cm += (float)(state[i].location.alt - state[GPS_BLENDED_INSTANCE].location.alt) * _blend_weights[i]; - } - } - - // Add the sum of weighted offsets to the reference location to obtain the blended location - state[GPS_BLENDED_INSTANCE].location.offset(blended_NE_offset_m.x, blended_NE_offset_m.y); - state[GPS_BLENDED_INSTANCE].location.alt += (int)blended_alt_offset_cm; - - // Calculate ground speed and course from blended velocity vector - state[GPS_BLENDED_INSTANCE].ground_speed = norm(state[GPS_BLENDED_INSTANCE].velocity.x, state[GPS_BLENDED_INSTANCE].velocity.y); - state[GPS_BLENDED_INSTANCE].ground_course = wrap_360(degrees(atan2f(state[GPS_BLENDED_INSTANCE].velocity.y, state[GPS_BLENDED_INSTANCE].velocity.x))); - - /* - * The blended location in _output_state.location is not stable enough to be used by the autopilot - * as it will move around as the relative accuracy changes. To mitigate this effect a low-pass filtered - * offset from each GPS location to the blended location is calculated and the filtered offset is applied - * to each receiver. - */ - - // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset - // A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering - float alpha[GPS_MAX_RECEIVERS] = {}; - for (uint8_t i=0; i 0) { - float min_alpha = constrain_float(_omega_lpf * 0.001f * (float)(state[i].last_gps_time_ms - _last_time_updated[i]), 0.0f, 1.0f); - if (_blend_weights[i] > min_alpha) { - alpha[i] = min_alpha / _blend_weights[i]; - } else { - alpha[i] = 1.0f; - } - _last_time_updated[i] = state[i].last_gps_time_ms; - } - } - - // Calculate the offset from each GPS solution to the blended solution - for (uint8_t i=0; i 0) { - // this is our first valid sensor week data - last_week_instance = state[i].time_week; - } else if (last_week_instance != 0 && _blend_weights[i] > 0 && last_week_instance != state[i].time_week) { - // there is valid sensor week data that is inconsistent - weeks_consistent = false; - } - } - // calculate output - if (!weeks_consistent) { - // use data from highest weighted sensor - state[GPS_BLENDED_INSTANCE].time_week = state[best_index].time_week; - state[GPS_BLENDED_INSTANCE].time_week_ms = state[best_index].time_week_ms; - } else { - // use week number from highest weighting GPS (they should all have the same week number) - state[GPS_BLENDED_INSTANCE].time_week = state[best_index].time_week; - // calculate a blended value for the number of ms lapsed in the week - double temp_time_0 = 0.0; - for (uint8_t i=0; i 0.0f) { - temp_time_0 += (double)state[i].time_week_ms * (double)_blend_weights[i]; - } - } - state[GPS_BLENDED_INSTANCE].time_week_ms = (uint32_t)temp_time_0; - } - - // calculate a blended value for the timing data and lag - double temp_time_1 = 0.0; - double temp_time_2 = 0.0; - for (uint8_t i=0; i 0.0f) { - temp_time_1 += (double)timing[i].last_fix_time_ms * (double) _blend_weights[i]; - temp_time_2 += (double)timing[i].last_message_time_ms * (double)_blend_weights[i]; - float gps_lag_sec = 0; - get_lag(i, gps_lag_sec); - _blended_lag_sec += gps_lag_sec * _blend_weights[i]; - } - } - timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = (uint32_t)temp_time_1; - timing[GPS_BLENDED_INSTANCE].last_message_time_ms = (uint32_t)temp_time_2; -} -#endif // GPS_BLENDED_INSTANCE - -bool AP_GPS::is_healthy(uint8_t instance) const -{ - if (instance >= GPS_MAX_INSTANCES) { - return false; - } - - const uint16_t gps_max_delta_ms = 245; // 200 ms (5Hz) + 45 ms buffer - - bool last_msg_valid = last_message_delta_time_ms(instance) < gps_max_delta_ms; - -#if defined(GPS_BLENDED_INSTANCE) - if (instance == GPS_BLENDED_INSTANCE) { - return last_msg_valid && blend_health_check(); - } -#endif - - return last_msg_valid && - drivers[instance] != nullptr && - drivers[instance]->is_healthy(); -} - -bool AP_GPS::prepare_for_arming(void) { - bool all_passed = true; - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (drivers[i] != nullptr) { - all_passed &= drivers[i]->prepare_for_arming(); - } - } - return all_passed; -} - -namespace AP { - -AP_GPS &gps() -{ - return *AP_GPS::get_singleton(); -} - -}; +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#include "AP_GPS.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "AP_GPS_NOVA.h" +#include "AP_GPS_ERB.h" +#include "AP_GPS_GSOF.h" +#include "AP_GPS_MTK.h" +#include "AP_GPS_MTK19.h" +#include "AP_GPS_NMEA.h" +#include "AP_GPS_SBF.h" +#include "AP_GPS_SBP.h" +#include "AP_GPS_SBP2.h" +#include "AP_GPS_SIRF.h" +#include "AP_GPS_UBLOX.h" +#include "AP_GPS_MAV.h" +#include "GPS_Backend.h" + +#if HAL_WITH_UAVCAN +#include +#include +#include "AP_GPS_UAVCAN.h" +#endif + +#include +#include + +#define GPS_RTK_INJECT_TO_ALL 127 +#define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms +#define GPS_BAUD_TIME_MS 1200 +#define GPS_TIMEOUT_MS 4000u + +// defines used to specify the mask position for use of different accuracy metrics in the blending algorithm +#define BLEND_MASK_USE_HPOS_ACC 1 +#define BLEND_MASK_USE_VPOS_ACC 2 +#define BLEND_MASK_USE_SPD_ACC 4 +#define BLEND_COUNTER_FAILURE_INCREMENT 10 + +extern const AP_HAL::HAL &hal; + +// baudrates to try to detect GPSes with +const uint32_t AP_GPS::_baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U}; + +// initialisation blobs to send to the GPS to try to get it into the +// right mode +const char AP_GPS::_initialisation_blob[] = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY; + +AP_GPS *AP_GPS::_singleton; + +// table of user settable parameters +const AP_Param::GroupInfo AP_GPS::var_info[] = { + // @Param: TYPE + // @DisplayName: GPS type + // @Description: GPS type + // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT), + +#if GPS_MAX_RECEIVERS > 1 + // @Param: TYPE2 + // @DisplayName: 2nd GPS type + // @Description: GPS type of 2nd GPS + // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0), +#endif + + // @Param: NAVFILTER + // @DisplayName: Navigation filter setting + // @Description: Navigation filter engine setting + // @Values: 0:Portable,2:Stationary,3:Pedestrian,4:Automotive,5:Sea,6:Airborne1G,7:Airborne2G,8:Airborne4G + // @User: Advanced + AP_GROUPINFO("NAVFILTER", 2, AP_GPS, _navfilter, GPS_ENGINE_AIRBORNE_4G), + +#if GPS_MAX_RECEIVERS > 1 + // @Param: AUTO_SWITCH + // @DisplayName: Automatic Switchover Setting + // @Description: Automatic switchover to GPS reporting best lock + // @Values: 0:Disabled,1:UseBest,2:Blend,3:UseSecond + // @User: Advanced + AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, 1), +#endif + + // @Param: MIN_DGPS + // @DisplayName: Minimum Lock Type Accepted for DGPS + // @Description: Sets the minimum type of differential GPS corrections required before allowing to switch into DGPS mode. + // @Values: 0:Any,50:FloatRTK,100:IntegerRTK + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("MIN_DGPS", 4, AP_GPS, _min_dgps, 100), + + // @Param: SBAS_MODE + // @DisplayName: SBAS Mode + // @Description: This sets the SBAS (satellite based augmentation system) mode if available on this GPS. If set to 2 then the SBAS mode is not changed in the GPS. Otherwise the GPS will be reconfigured to enable/disable SBAS. Disabling SBAS may be worthwhile in some parts of the world where an SBAS signal is available but the baseline is too long to be useful. + // @Values: 0:Disabled,1:Enabled,2:NoChange + // @User: Advanced + AP_GROUPINFO("SBAS_MODE", 5, AP_GPS, _sbas_mode, 2), + + // @Param: MIN_ELEV + // @DisplayName: Minimum elevation + // @Description: This sets the minimum elevation of satellites above the horizon for them to be used for navigation. Setting this to -100 leaves the minimum elevation set to the GPS modules default. + // @Range: -100 90 + // @Units: deg + // @User: Advanced + AP_GROUPINFO("MIN_ELEV", 6, AP_GPS, _min_elevation, -100), + + // @Param: INJECT_TO + // @DisplayName: Destination for GPS_INJECT_DATA MAVLink packets + // @Description: The GGS can send raw serial packets to inject data to multiple GPSes. + // @Values: 0:send to first GPS,1:send to 2nd GPS,127:send to all + // @User: Advanced + AP_GROUPINFO("INJECT_TO", 7, AP_GPS, _inject_to, GPS_RTK_INJECT_TO_ALL), + + // @Param: SBP_LOGMASK + // @DisplayName: Swift Binary Protocol Logging Mask + // @Description: Masked with the SBP msg_type field to determine whether SBR1/SBR2 data is logged + // @Values: 0:None (0x0000),-1:All (0xFFFF),-256:External only (0xFF00) + // @User: Advanced + AP_GROUPINFO("SBP_LOGMASK", 8, AP_GPS, _sbp_logmask, -256), + + // @Param: RAW_DATA + // @DisplayName: Raw data logging + // @Description: Handles logging raw data; on uBlox chips that support raw data this will log RXM messages into logger; on Septentrio this will log on the equipment's SD card and when set to 2, the autopilot will try to stop logging after disarming and restart after arming + // @Values: 0:Ignore,1:Always log,2:Stop logging when disarmed (SBF only),5:Only log every five samples (uBlox only) + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("RAW_DATA", 9, AP_GPS, _raw_data, 0), + + // @Param: GNSS_MODE + // @DisplayName: GNSS system configuration + // @Description: Bitmask for what GNSS system to use on the first GPS (all unchecked or zero to leave GPS as configured) + // @Values: 0:Leave as currently configured, 1:GPS-NoSBAS, 3:GPS+SBAS, 4:Galileo-NoSBAS, 6:Galileo+SBAS, 8:Beidou, 51:GPS+IMES+QZSS+SBAS (Japan Only), 64:GLONASS, 66:GLONASS+SBAS, 67:GPS+GLONASS+SBAS + // @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS + // @User: Advanced + AP_GROUPINFO("GNSS_MODE", 10, AP_GPS, _gnss_mode[0], 0), + + // @Param: SAVE_CFG + // @DisplayName: Save GPS configuration + // @Description: Determines whether the configuration for this GPS should be written to non-volatile memory on the GPS. Currently working for UBlox 6 series and above. + // @Values: 0:Do not save config,1:Save config,2:Save only when needed + // @User: Advanced + AP_GROUPINFO("SAVE_CFG", 11, AP_GPS, _save_config, 2), + +#if GPS_MAX_RECEIVERS > 1 + // @Param: GNSS_MODE2 + // @DisplayName: GNSS system configuration + // @Description: Bitmask for what GNSS system to use on the second GPS (all unchecked or zero to leave GPS as configured) + // @Values: 0:Leave as currently configured, 1:GPS-NoSBAS, 3:GPS+SBAS, 4:Galileo-NoSBAS, 6:Galileo+SBAS, 8:Beidou, 51:GPS+IMES+QZSS+SBAS (Japan Only), 64:GLONASS, 66:GLONASS+SBAS, 67:GPS+GLONASS+SBAS + // @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS + // @User: Advanced + AP_GROUPINFO("GNSS_MODE2", 12, AP_GPS, _gnss_mode[1], 0), +#endif + + // @Param: AUTO_CONFIG + // @DisplayName: Automatic GPS configuration + // @Description: Controls if the autopilot should automatically configure the GPS based on the parameters and default settings + // @Values: 0:Disables automatic configuration,1:Enable automatic configuration + // @User: Advanced + AP_GROUPINFO("AUTO_CONFIG", 13, AP_GPS, _auto_config, 1), + + // @Param: RATE_MS + // @DisplayName: GPS update rate in milliseconds + // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed + // @Units: ms + // @Values: 100:10Hz,125:8Hz,200:5Hz + // @Range: 50 200 + // @User: Advanced + AP_GROUPINFO("RATE_MS", 14, AP_GPS, _rate_ms[0], 200), + +#if GPS_MAX_RECEIVERS > 1 + // @Param: RATE_MS2 + // @DisplayName: GPS 2 update rate in milliseconds + // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed + // @Units: ms + // @Values: 100:10Hz,125:8Hz,200:5Hz + // @Range: 50 200 + // @User: Advanced + AP_GROUPINFO("RATE_MS2", 15, AP_GPS, _rate_ms[1], 200), +#endif + + // @Param: POS1_X + // @DisplayName: Antenna X position offset + // @Description: X position of the first GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer. + // @Units: m + // @Range: -10 10 + // @User: Advanced + + // @Param: POS1_Y + // @DisplayName: Antenna Y position offset + // @Description: Y position of the first GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer. + // @Units: m + // @Range: -10 10 + // @User: Advanced + + // @Param: POS1_Z + // @DisplayName: Antenna Z position offset + // @Description: Z position of the first GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer. + // @Units: m + // @Range: -10 10 + // @User: Advanced + AP_GROUPINFO("POS1", 16, AP_GPS, _antenna_offset[0], 0.0f), + + // @Param: POS2_X + // @DisplayName: Antenna X position offset + // @Description: X position of the second GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer. + // @Units: m + // @Range: -10 10 + // @User: Advanced + + // @Param: POS2_Y + // @DisplayName: Antenna Y position offset + // @Description: Y position of the second GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer. + // @Units: m + // @Range: -10 10 + // @User: Advanced + +#if GPS_MAX_RECEIVERS > 1 + // @Param: POS2_Z + // @DisplayName: Antenna Z position offset + // @Description: Z position of the second GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer. + // @Units: m + // @Range: -10 10 + // @User: Advanced + AP_GROUPINFO("POS2", 17, AP_GPS, _antenna_offset[1], 0.0f), +#endif + + // @Param: DELAY_MS + // @DisplayName: GPS delay in milliseconds + // @Description: Controls the amount of GPS measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type. + // @Units: ms + // @Range: 0 250 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("DELAY_MS", 18, AP_GPS, _delay_ms[0], 0), + +#if GPS_MAX_RECEIVERS > 1 + // @Param: DELAY_MS2 + // @DisplayName: GPS 2 delay in milliseconds + // @Description: Controls the amount of GPS measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type. + // @Units: ms + // @Range: 0 250 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("DELAY_MS2", 19, AP_GPS, _delay_ms[1], 0), +#endif + +#if defined(GPS_BLENDED_INSTANCE) + // @Param: BLEND_MASK + // @DisplayName: Multi GPS Blending Mask + // @Description: Determines which of the accuracy measures Horizontal position, Vertical Position and Speed are used to calculate the weighting on each GPS receiver when soft switching has been selected by setting GPS_AUTO_SWITCH to 2 + // @Bitmask: 0:Horiz Pos,1:Vert Pos,2:Speed + // @User: Advanced + AP_GROUPINFO("BLEND_MASK", 20, AP_GPS, _blend_mask, 5), + + // @Param: BLEND_TC + // @DisplayName: Blending time constant + // @Description: Controls the slowest time constant applied to the calculation of GPS position and height offsets used to adjust different GPS receivers for steady state position differences. + // @Units: s + // @Range: 5.0 30.0 + // @User: Advanced + AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f), +#endif + + AP_GROUPEND +}; + +// constructor +AP_GPS::AP_GPS() +{ + static_assert((sizeof(_initialisation_blob) * (CHAR_BIT + 2)) < (4800 * GPS_BAUD_TIME_MS * 1e-3), + "GPS initilisation blob is too large to be completely sent before the baud rate changes"); + + AP_Param::setup_object_defaults(this, var_info); + + if (_singleton != nullptr) { + AP_HAL::panic("AP_GPS must be singleton"); + } + _singleton = this; +} + +// return true if a specific type of GPS uses a UART +bool AP_GPS::needs_uart(GPS_Type type) const +{ + switch (type) { + case GPS_TYPE_NONE: + case GPS_TYPE_HIL: + case GPS_TYPE_UAVCAN: + case GPS_TYPE_MAV: + return false; + default: + break; + } + return true; +} + +/// Startup initialisation. +void AP_GPS::init(const AP_SerialManager& serial_manager) +{ + primary_instance = 0; + + // search for serial ports with gps protocol + uint8_t uart_idx = 0; + for (uint8_t i=0; i GPS_MAX_RATE_MS) { + _rate_ms[i] = GPS_MAX_RATE_MS; + } + } +} + +// return number of active GPS sensors. Note that if the first GPS +// is not present but the 2nd is then we return 2. Note that a blended +// GPS solution is treated as an additional sensor. +uint8_t AP_GPS::num_sensors(void) const +{ + if (!_output_is_blended) { + return num_instances; + } else { + return num_instances+1; + } +} + +bool AP_GPS::speed_accuracy(uint8_t instance, float &sacc) const +{ + if (state[instance].have_speed_accuracy) { + sacc = state[instance].speed_accuracy; + return true; + } + return false; +} + +bool AP_GPS::horizontal_accuracy(uint8_t instance, float &hacc) const +{ + if (state[instance].have_horizontal_accuracy) { + hacc = state[instance].horizontal_accuracy; + return true; + } + return false; +} + +bool AP_GPS::vertical_accuracy(uint8_t instance, float &vacc) const +{ + if (state[instance].have_vertical_accuracy) { + vacc = state[instance].vertical_accuracy; + return true; + } + return false; +} + + +/** + convert GPS week and milliseconds to unix epoch in milliseconds + */ +uint64_t AP_GPS::time_epoch_convert(uint16_t gps_week, uint32_t gps_ms) +{ + uint64_t fix_time_ms = UNIX_OFFSET_MSEC + gps_week * AP_MSEC_PER_WEEK + gps_ms; + return fix_time_ms; +} + +/** + calculate current time since the unix epoch in microseconds + */ +uint64_t AP_GPS::time_epoch_usec(uint8_t instance) const +{ + const GPS_State &istate = state[instance]; + if (istate.last_gps_time_ms == 0 || istate.time_week == 0) { + return 0; + } + uint64_t fix_time_ms = time_epoch_convert(istate.time_week, istate.time_week_ms); + // add in the milliseconds since the last fix + return (fix_time_ms + (AP_HAL::millis() - istate.last_gps_time_ms)) * 1000ULL; +} + +/* + send some more initialisation string bytes if there is room in the + UART transmit buffer + */ +void AP_GPS::send_blob_start(uint8_t instance, const char *_blob, uint16_t size) +{ + initblob_state[instance].blob = _blob; + initblob_state[instance].remaining = size; +} + +/* + send some more initialisation string bytes if there is room in the + UART transmit buffer + */ +void AP_GPS::send_blob_update(uint8_t instance) +{ + // exit immediately if no uart for this instance + if (_port[instance] == nullptr) { + return; + } + + // see if we can write some more of the initialisation blob + if (initblob_state[instance].remaining > 0) { + int16_t space = _port[instance]->txspace(); + if (space > (int16_t)initblob_state[instance].remaining) { + space = initblob_state[instance].remaining; + } + while (space > 0) { + _port[instance]->write(*initblob_state[instance].blob); + initblob_state[instance].blob++; + space--; + initblob_state[instance].remaining--; + } + } +} + +/* + run detection step for one GPS instance. If this finds a GPS then it + will fill in drivers[instance] and change state[instance].status + from NO_GPS to NO_FIX. + */ +void AP_GPS::detect_instance(uint8_t instance) +{ + AP_GPS_Backend *new_gps = nullptr; + struct detect_state *dstate = &detect_state[instance]; + const uint32_t now = AP_HAL::millis(); + + state[instance].status = NO_GPS; + state[instance].hdop = GPS_UNKNOWN_DOP; + state[instance].vdop = GPS_UNKNOWN_DOP; + + switch (_type[instance]) { + // user has to explicitly set the MAV type, do not use AUTO + // do not try to detect the MAV type, assume it's there + case GPS_TYPE_MAV: +#ifndef HAL_BUILD_AP_PERIPH + dstate->auto_detected_baud = false; // specified, not detected + new_gps = new AP_GPS_MAV(*this, state[instance], nullptr); + goto found_gps; +#endif + break; + + // user has to explicitly set the UAVCAN type, do not use AUTO + case GPS_TYPE_UAVCAN: +#if HAL_WITH_UAVCAN + dstate->auto_detected_baud = false; // specified, not detected + new_gps = AP_GPS_UAVCAN::probe(*this, state[instance]); + goto found_gps; +#endif + return; // We don't do anything here if UAVCAN is not supported + default: + break; + } + + if (_port[instance] == nullptr) { + // UART not available + return; + } + + // all remaining drivers automatically cycle through baud rates to detect + // the correct baud rate, and should have the selected baud broadcast + dstate->auto_detected_baud = true; + +#ifndef HAL_BUILD_AP_PERIPH + switch (_type[instance]) { + // by default the sbf/trimble gps outputs no data on its port, until configured. + case GPS_TYPE_SBF: + new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]); + break; + + case GPS_TYPE_GSOF: + new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]); + break; + + case GPS_TYPE_NOVA: + new_gps = new AP_GPS_NOVA(*this, state[instance], _port[instance]); + break; + + default: + break; + } +#endif // HAL_BUILD_AP_PERIPH + + if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) { + // try the next baud rate + // incrementing like this will skip the first element in array of bauds + // this is okay, and relied upon + dstate->current_baud++; + if (dstate->current_baud == ARRAY_SIZE(_baudrates)) { + dstate->current_baud = 0; + } + uint32_t baudrate = _baudrates[dstate->current_baud]; + _port[instance]->begin(baudrate); + _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); + dstate->last_baud_change_ms = now; + + if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) { + if (_type[instance] == GPS_TYPE_HEMI) { + send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); + } else { + send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); + } + } + } + + if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) { + send_blob_update(instance); + } + + while (initblob_state[instance].remaining == 0 && _port[instance]->available() > 0 + && new_gps == nullptr) { + uint8_t data = _port[instance]->read(); + /* + running a uBlox at less than 38400 will lead to packet + corruption, as we can't receive the packets in the 200ms + window for 5Hz fixes. The NMEA startup message should force + the uBlox into 115200 no matter what rate it is configured + for. + */ + if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) && + ((!_auto_config && _baudrates[dstate->current_baud] >= 38400) || + _baudrates[dstate->current_baud] == 115200) && + AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) { + new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance]); + } +#ifndef HAL_BUILD_AP_PERIPH +#if !HAL_MINIMIZE_FEATURES + // we drop the MTK drivers when building a small build as they are so rarely used + // and are surprisingly large + else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK19) && + AP_GPS_MTK19::_detect(dstate->mtk19_detect_state, data)) { + new_gps = new AP_GPS_MTK19(*this, state[instance], _port[instance]); + } else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK) && + AP_GPS_MTK::_detect(dstate->mtk_detect_state, data)) { + new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]); + } +#endif + else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && + AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) { + new_gps = new AP_GPS_SBP2(*this, state[instance], _port[instance]); + } + else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && + AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) { + new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]); + } +#if !HAL_MINIMIZE_FEATURES + else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) && + AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) { + new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]); + } +#endif + else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) && + AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) { + new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]); + } else if ((_type[instance] == GPS_TYPE_NMEA || + _type[instance] == GPS_TYPE_HEMI) && + AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) { + new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]); + } +#endif // HAL_BUILD_AP_PERIPH + if (new_gps) { + goto found_gps; + } + } + +found_gps: + if (new_gps != nullptr) { + state[instance].status = NO_FIX; + drivers[instance] = new_gps; + timing[instance].last_message_time_ms = now; + timing[instance].delta_time_ms = GPS_TIMEOUT_MS; + new_gps->broadcast_gps_type(); + } +} + +AP_GPS::GPS_Status AP_GPS::highest_supported_status(uint8_t instance) const +{ + if (instance < GPS_MAX_RECEIVERS && drivers[instance] != nullptr) { + return drivers[instance]->highest_supported_status(); + } + return AP_GPS::GPS_OK_FIX_3D; +} + +bool AP_GPS::should_log() const +{ +#ifndef HAL_BUILD_AP_PERIPH + AP_Logger *logger = AP_Logger::get_singleton(); + if (logger == nullptr) { + return false; + } + if (_log_gps_bit == (uint32_t)-1) { + return false; + } + if (!logger->should_log(_log_gps_bit)) { + return false; + } + return true; +#else + return false; +#endif +} + + +/* + update one GPS instance. This should be called at 10Hz or greater + */ +void AP_GPS::update_instance(uint8_t instance) +{ + if (_type[instance] == GPS_TYPE_HIL) { + // in HIL, leave info alone + return; + } + if (_type[instance] == GPS_TYPE_NONE) { + // not enabled + state[instance].status = NO_GPS; + state[instance].hdop = GPS_UNKNOWN_DOP; + state[instance].vdop = GPS_UNKNOWN_DOP; + return; + } + if (locked_ports & (1U<read(); + uint32_t tnow = AP_HAL::millis(); + + // if we did not get a message, and the idle timer of 2 seconds + // has expired, re-initialise the GPS. This will cause GPS + // detection to run again + bool data_should_be_logged = false; + if (!result) { + if (tnow - timing[instance].last_message_time_ms > GPS_TIMEOUT_MS) { + memset((void *)&state[instance], 0, sizeof(state[instance])); + state[instance].instance = instance; + state[instance].hdop = GPS_UNKNOWN_DOP; + state[instance].vdop = GPS_UNKNOWN_DOP; + timing[instance].last_message_time_ms = tnow; + timing[instance].delta_time_ms = GPS_TIMEOUT_MS; + // do not try to detect again if type is MAV + if (_type[instance] == GPS_TYPE_MAV) { + state[instance].status = NO_FIX; + } else { + // free the driver before we run the next detection, so we + // don't end up with two allocated at any time + delete drivers[instance]; + drivers[instance] = nullptr; + state[instance].status = NO_GPS; + } + // log this data as a "flag" that the GPS is no longer + // valid (see PR#8144) + data_should_be_logged = true; + } + } else { + if (state[instance].uart_timestamp_ms != 0) { + // set the timestamp for this messages based on + // set_uart_timestamp() in backend, if available + tnow = state[instance].uart_timestamp_ms; + state[instance].uart_timestamp_ms = 0; + } + // delta will only be correct after parsing two messages + timing[instance].delta_time_ms = tnow - timing[instance].last_message_time_ms; + timing[instance].last_message_time_ms = tnow; + if (state[instance].status >= GPS_OK_FIX_2D) { + timing[instance].last_fix_time_ms = tnow; + } + + data_should_be_logged = true; + } + +#ifndef HAL_BUILD_AP_PERIPH + if (data_should_be_logged && + (should_log() || AP::ahrs().have_ekf_logging())) { + AP::logger().Write_GPS(instance); + } + + if (state[instance].status >= GPS_OK_FIX_3D) { + const uint64_t now = time_epoch_usec(instance); + if (now != 0) { + AP::rtc().set_utc_usec(now, AP_RTC::SOURCE_GPS); + } + } +#else + (void)data_should_be_logged; +#endif +} + +/* + update all GPS instances + */ +void AP_GPS::update(void) +{ + for (uint8_t i=0; i 0) { + _blend_health_counter--; + } + // stop blending if unhealthy + if (_blend_health_counter >= 50) { + _output_is_blended = false; + } + } else { + _output_is_blended = false; + _blend_health_counter = 0; + } + + if (_output_is_blended) { + // Use the weighting to calculate blended GPS states + calc_blended_state(); + // set primary to the virtual instance + primary_instance = GPS_BLENDED_INSTANCE; + } else { + // use switch logic to find best GPS + uint32_t now = AP_HAL::millis(); + if (_auto_switch == 3) { + // select the second GPS instance + primary_instance = 1; + } else if (_auto_switch >= 1) { + // handling switching away from blended GPS + if (primary_instance == GPS_BLENDED_INSTANCE) { + primary_instance = 0; + for (uint8_t i=1; i state[primary_instance].status) || + ((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) { + primary_instance = i; + _last_instance_swap_ms = now; + } + } + } else { + // handle switch between real GPSs + for (uint8_t i=0; i state[primary_instance].status) { + // we have a higher status lock, or primary is set to the blended GPS, change GPS + primary_instance = i; + _last_instance_swap_ms = now; + continue; + } + + bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1); + + if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) { + + bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2); + + if ((another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) || + (another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000)) { + // this GPS has more satellites than the + // current primary, switch primary. Once we switch we will + // then tend to stick to the new GPS as primary. We don't + // want to switch too often as it will look like a + // position shift to the controllers. + primary_instance = i; + _last_instance_swap_ms = now; + } + } + } + } + } else { + // AUTO_SWITCH is 0 so no switching of GPSs + primary_instance = 0; + } + + // copy the primary instance to the blended instance in case it is enabled later + state[GPS_BLENDED_INSTANCE] = state[primary_instance]; + _blended_antenna_offset = _antenna_offset[primary_instance]; + } +#endif // GPS_BLENDED_INSTANCE + +#ifndef HAL_BUILD_AP_PERIPH + // update notify with gps status. We always base this on the primary_instance + AP_Notify::flags.gps_status = state[primary_instance].status; + AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats; +#endif +} + +void AP_GPS::handle_gps_inject(const mavlink_message_t &msg) +{ + mavlink_gps_inject_data_t packet; + mavlink_msg_gps_inject_data_decode(&msg, &packet); + + if (packet.len > sizeof(packet.data)) { + // invalid packet + return; + } + + handle_gps_rtcm_fragment(0, packet.data, packet.len); +} + +/* + pass along a mavlink message (for MAV type) + */ +void AP_GPS::handle_msg(const mavlink_message_t &msg) +{ + switch (msg.msgid) { + case MAVLINK_MSG_ID_GPS_RTCM_DATA: + // pass data to de-fragmenter + handle_gps_rtcm_data(msg); + break; + case MAVLINK_MSG_ID_GPS_INJECT_DATA: + handle_gps_inject(msg); + break; + default: { + uint8_t i; + for (i=0; ihandle_msg(msg); + } + } + break; + } + } +} + +/* + set HIL (hardware in the loop) status for a GPS instance + */ +void AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms, + const Location &_location, const Vector3f &_velocity, uint8_t _num_sats, + uint16_t hdop) +{ + if (instance >= GPS_MAX_RECEIVERS) { + return; + } + const uint32_t tnow = AP_HAL::millis(); + GPS_State &istate = state[instance]; + istate.status = _status; + istate.location = _location; + istate.velocity = _velocity; + istate.ground_speed = norm(istate.velocity.x, istate.velocity.y); + istate.ground_course = wrap_360(degrees(atan2f(istate.velocity.y, istate.velocity.x))); + istate.hdop = hdop; + istate.num_sats = _num_sats; + istate.last_gps_time_ms = tnow; + uint64_t gps_time_ms = time_epoch_ms - UNIX_OFFSET_MSEC; + istate.time_week = gps_time_ms / AP_MSEC_PER_WEEK; + istate.time_week_ms = gps_time_ms - istate.time_week * AP_MSEC_PER_WEEK; + timing[instance].last_message_time_ms = tnow; + timing[instance].last_fix_time_ms = tnow; + _type[instance].set(GPS_TYPE_HIL); +} + +// set accuracy for HIL +void AP_GPS::setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc, bool _have_vertical_velocity, uint32_t sample_ms) +{ + if (instance >= GPS_MAX_RECEIVERS) { + return; + } + GPS_State &istate = state[instance]; + istate.vdop = vdop * 100; + istate.horizontal_accuracy = hacc; + istate.vertical_accuracy = vacc; + istate.speed_accuracy = sacc; + istate.have_horizontal_accuracy = true; + istate.have_vertical_accuracy = true; + istate.have_speed_accuracy = true; + istate.have_vertical_velocity |= _have_vertical_velocity; + if (sample_ms != 0) { + timing[instance].last_message_time_ms = sample_ms; + timing[instance].last_fix_time_ms = sample_ms; + } +} + +/** + Lock a GPS port, preventing the GPS driver from using it. This can + be used to allow a user to control a GPS port via the + SERIAL_CONTROL protocol + */ +void AP_GPS::lock_port(uint8_t instance, bool lock) +{ + + if (instance >= GPS_MAX_RECEIVERS) { + return; + } + if (lock) { + locked_ports |= (1U<inject_data(data, len); + } +} + +void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) +{ + static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS]; + if (status(0) > AP_GPS::NO_GPS) { + // when we have a GPS then only send new data + if (last_send_time_ms[chan] == last_message_time_ms(0)) { + return; + } + last_send_time_ms[chan] = last_message_time_ms(0); + } else { + // when we don't have a GPS then send at 1Hz + uint32_t now = AP_HAL::millis(); + if (now - last_send_time_ms[chan] < 1000) { + return; + } + last_send_time_ms[chan] = now; + } + const Location &loc = location(0); + float hacc = 0.0f; + float vacc = 0.0f; + float sacc = 0.0f; + horizontal_accuracy(0, hacc); + vertical_accuracy(0, vacc); + speed_accuracy(0, sacc); + mavlink_msg_gps_raw_int_send( + chan, + last_fix_time_ms(0)*(uint64_t)1000, + status(0), + loc.lat, // in 1E7 degrees + loc.lng, // in 1E7 degrees + loc.alt * 10UL, // in mm + get_hdop(0), + get_vdop(0), + ground_speed(0)*100, // cm/s + ground_course(0)*100, // 1/100 degrees, + num_sats(0), + 0, // TODO: Elipsoid height in mm + hacc * 1000, // one-sigma standard deviation in mm + vacc * 1000, // one-sigma standard deviation in mm + sacc * 1000, // one-sigma standard deviation in mm/s + 0); // TODO one-sigma heading accuracy standard deviation +} + +#if GPS_MAX_RECEIVERS > 1 +void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) +{ + static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS]; + if (num_instances < 2 || status(1) <= AP_GPS::NO_GPS) { + return; + } + // when we have a GPS then only send new data + if (last_send_time_ms[chan] == last_message_time_ms(1)) { + return; + } + last_send_time_ms[chan] = last_message_time_ms(1); + + const Location &loc = location(1); + mavlink_msg_gps2_raw_send( + chan, + last_fix_time_ms(1)*(uint64_t)1000, + status(1), + loc.lat, + loc.lng, + loc.alt * 10UL, + get_hdop(1), + get_vdop(1), + ground_speed(1)*100, // cm/s + ground_course(1)*100, // 1/100 degrees, + num_sats(1), + state[1].rtk_num_sats, + state[1].rtk_age_ms); +} +#endif // GPS_MAX_RECEIVERS + +void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst) +{ + if (inst >= GPS_MAX_RECEIVERS) { + return; + } + if (drivers[inst] != nullptr && drivers[inst]->supports_mavlink_gps_rtk_message()) { + drivers[inst]->send_mavlink_gps_rtk(chan); + } +} + +bool AP_GPS::first_unconfigured_gps(uint8_t &instance) const +{ + for (int i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_type[i] != GPS_TYPE_NONE && (drivers[i] == nullptr || !drivers[i]->is_configured())) { + instance = i; + return true; + } + } + return false; +} + +void AP_GPS::broadcast_first_configuration_failure_reason(void) const +{ + uint8_t unconfigured; + if (first_unconfigured_gps(unconfigured)) { + if (drivers[unconfigured] == nullptr) { + gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: was not found", unconfigured + 1); + } else { + drivers[unconfigured]->broadcast_configuration_failure_reason(); + } + } +} + +// pre-arm check that all GPSs are close to each other. farthest distance between GPSs (in meters) is returned +bool AP_GPS::all_consistent(float &distance) const +{ + // return true immediately if only one valid receiver + if (num_instances <= 1 || + drivers[0] == nullptr || _type[0] == GPS_TYPE_NONE) { + distance = 0; + return true; + } + + // calculate distance + distance = state[0].location.get_distance_NED(state[1].location).length(); + // success if distance is within 50m + return (distance < 50); +} + +// pre-arm check of GPS blending. True means healthy or that blending is not being used +bool AP_GPS::blend_health_check() const +{ + return (_blend_health_counter < 50); +} + +/* + re-assemble fragmented RTCM data + */ +void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len) +{ + if ((flags & 1) == 0) { + // it is not fragmented, pass direct + inject_data(data, len); + return; + } + + // see if we need to allocate re-assembly buffer + if (rtcm_buffer == nullptr) { + rtcm_buffer = (struct rtcm_buffer *)calloc(1, sizeof(*rtcm_buffer)); + if (rtcm_buffer == nullptr) { + // nothing to do but discard the data + return; + } + } + + uint8_t fragment = (flags >> 1U) & 0x03; + uint8_t sequence = (flags >> 3U) & 0x1F; + + // see if this fragment is consistent with existing fragments + if (rtcm_buffer->fragments_received && + (rtcm_buffer->sequence != sequence || + (rtcm_buffer->fragments_received & (1U<sequence = sequence; + rtcm_buffer->fragments_received |= (1U << fragment); + + // copy the data + memcpy(&rtcm_buffer->buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*(uint16_t)fragment], data, len); + + // when we get a fragment of less than max size then we know the + // number of fragments. Note that this means if you want to send a + // block of RTCM data of an exact multiple of the buffer size you + // need to send a final packet of zero length + if (len < MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN) { + rtcm_buffer->fragment_count = fragment+1; + rtcm_buffer->total_length = (MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*fragment) + len; + } else if (rtcm_buffer->fragments_received == 0x0F) { + // special case of 4 full fragments + rtcm_buffer->fragment_count = 4; + rtcm_buffer->total_length = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*4; + } + + + // see if we have all fragments + if (rtcm_buffer->fragment_count != 0 && + rtcm_buffer->fragments_received == (1U << rtcm_buffer->fragment_count) - 1) { + // we have them all, inject + inject_data(rtcm_buffer->buffer, rtcm_buffer->total_length); + memset(rtcm_buffer, 0, sizeof(*rtcm_buffer)); + } +} + +/* + re-assemble GPS_RTCM_DATA message + */ +void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg) +{ + mavlink_gps_rtcm_data_t packet; + mavlink_msg_gps_rtcm_data_decode(&msg, &packet); + + if (packet.len > sizeof(packet.data)) { + // invalid packet + return; + } + + handle_gps_rtcm_fragment(packet.flags, packet.data, packet.len); +} + +void AP_GPS::Write_AP_Logger_Log_Startup_messages() +{ + for (uint8_t instance=0; instanceWrite_AP_Logger_Log_Startup_messages(); + } +} + +void AP_GPS::get_gps_headline_value(int32_t & gps_date_time){ + + gps_date_time = state[GPS_BLENDED_INSTANCE].zr_date_timestamp; + +} +/* + return the expected lag (in seconds) in the position and velocity readings from the gps + return true if the GPS hardware configuration is known or the delay parameter has been set + */ +bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const +{ + // always enusre a lag is provided + lag_sec = 0.22f; + + if (instance >= GPS_MAX_INSTANCES) { + return false; + } + +#if defined(GPS_BLENDED_INSTANCE) + // return lag of blended GPS + if (instance == GPS_BLENDED_INSTANCE) { + lag_sec = _blended_lag_sec; + // auto switching uses all GPS receivers, so all must be configured + uint8_t inst; // we don't actually care what the number is, but must pass it + return first_unconfigured_gps(inst); + } +#endif + + if (_delay_ms[instance] > 0) { + // if the user has specified a non zero time delay, always return that value + lag_sec = 0.001f * (float)_delay_ms[instance]; + // the user is always right !! + return true; + } else if (drivers[instance] == nullptr || state[instance].status == NO_GPS) { + // no GPS was detected in this instance so return the worst possible lag term + if (_type[instance] == GPS_TYPE_NONE) { + lag_sec = 0.0f; + return true; + } + return _type[instance] == GPS_TYPE_AUTO; + } else { + // the user has not specified a delay so we determine it from the GPS type + return drivers[instance]->get_lag(lag_sec); + } +} + +// return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin +const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const +{ + if (instance >= GPS_MAX_INSTANCES) { + // we have to return a reference so use instance 0 + return _antenna_offset[0]; + } + +#if defined(GPS_BLENDED_INSTANCE) + if (instance == GPS_BLENDED_INSTANCE) { + // return an offset for the blended GPS solution + return _blended_antenna_offset; + } +#endif + + return _antenna_offset[instance]; +} + +/* + returns the desired gps update rate in milliseconds + this does not provide any guarantee that the GPS is updating at the requested + rate it is simply a helper for use in the backends for determining what rate + they should be configuring the GPS to run at +*/ +uint16_t AP_GPS::get_rate_ms(uint8_t instance) const +{ + // sanity check + if (instance >= num_instances || _rate_ms[instance] <= 0) { + return GPS_MAX_RATE_MS; + } + return MIN(_rate_ms[instance], GPS_MAX_RATE_MS); +} + +#if defined(GPS_BLENDED_INSTANCE) +/* + calculate the weightings used to blend GPSs location and velocity data +*/ +bool AP_GPS::calc_blend_weights(void) +{ + // zero the blend weights + memset(&_blend_weights, 0, sizeof(_blend_weights)); + + // exit immediately if not enough receivers to do blending + if (state[0].status <= NO_FIX || state[1].status <= NO_FIX) { + return false; + } + + // Use the oldest non-zero time, but if time difference is excessive, use newest to prevent a disconnected receiver from blocking updates + uint32_t max_ms = 0; // newest non-zero system time of arrival of a GPS message + uint32_t min_ms = -1; // oldest non-zero system time of arrival of a GPS message + int16_t max_rate_ms = 0; // largest update interval of a GPS receiver + for (uint8_t i=0; i max_ms) { + max_ms = state[i].last_gps_time_ms; + } + if ((state[i].last_gps_time_ms < min_ms) && (state[i].last_gps_time_ms > 0)) { + min_ms = state[i].last_gps_time_ms; + } + if (get_rate_ms(i) > max_rate_ms) { + max_rate_ms = get_rate_ms(i); + } + } + if ((int32_t)(max_ms - min_ms) < (int32_t)(2 * max_rate_ms)) { + // data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated + state[GPS_BLENDED_INSTANCE].last_gps_time_ms = min_ms; + } else { + // receiver data has timed out so fail out of blending + return false; + } + + // calculate the sum squared speed accuracy across all GPS sensors + float speed_accuracy_sum_sq = 0.0f; + if (_blend_mask & BLEND_MASK_USE_SPD_ACC) { + for (uint8_t i=0; i= GPS_OK_FIX_3D) { + if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f) { + speed_accuracy_sum_sq += state[i].speed_accuracy * state[i].speed_accuracy; + } else { + // not all receivers support this metric so set it to zero and don't use it + speed_accuracy_sum_sq = 0.0f; + break; + } + } + } + } + + // calculate the sum squared horizontal position accuracy across all GPS sensors + float horizontal_accuracy_sum_sq = 0.0f; + if (_blend_mask & BLEND_MASK_USE_HPOS_ACC) { + for (uint8_t i=0; i= GPS_OK_FIX_2D) { + if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f) { + horizontal_accuracy_sum_sq += state[i].horizontal_accuracy * state[i].horizontal_accuracy; + } else { + // not all receivers support this metric so set it to zero and don't use it + horizontal_accuracy_sum_sq = 0.0f; + break; + } + } + } + } + + // calculate the sum squared vertical position accuracy across all GPS sensors + float vertical_accuracy_sum_sq = 0.0f; + if (_blend_mask & BLEND_MASK_USE_VPOS_ACC) { + for (uint8_t i=0; i= GPS_OK_FIX_3D) { + if (state[i].have_vertical_accuracy && state[i].vertical_accuracy > 0.0f) { + vertical_accuracy_sum_sq += state[i].vertical_accuracy * state[i].vertical_accuracy; + } else { + // not all receivers support this metric so set it to zero and don't use it + vertical_accuracy_sum_sq = 0.0f; + break; + } + } + } + } + // Check if we can do blending using reported accuracy + bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f || speed_accuracy_sum_sq > 0.0f); + + // if we can't do blending using reported accuracy, return false and hard switch logic will be used instead + if (!can_do_blending) { + return false; + } + + float sum_of_all_weights = 0.0f; + + // calculate a weighting using the reported horizontal position + float hpos_blend_weights[GPS_MAX_RECEIVERS] = {}; + if (horizontal_accuracy_sum_sq > 0.0f && (_blend_mask & BLEND_MASK_USE_HPOS_ACC)) { + // calculate the weights using the inverse of the variances + float sum_of_hpos_weights = 0.0f; + for (uint8_t i=0; i= GPS_OK_FIX_2D && state[i].horizontal_accuracy >= 0.001f) { + hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (state[i].horizontal_accuracy * state[i].horizontal_accuracy); + sum_of_hpos_weights += hpos_blend_weights[i]; + } + } + // normalise the weights + if (sum_of_hpos_weights > 0.0f) { + for (uint8_t i=0; i 0.0f && (_blend_mask & BLEND_MASK_USE_VPOS_ACC)) { + // calculate the weights using the inverse of the variances + float sum_of_vpos_weights = 0.0f; + for (uint8_t i=0; i= GPS_OK_FIX_3D && state[i].vertical_accuracy >= 0.001f) { + vpos_blend_weights[i] = vertical_accuracy_sum_sq / (state[i].vertical_accuracy * state[i].vertical_accuracy); + sum_of_vpos_weights += vpos_blend_weights[i]; + } + } + // normalise the weights + if (sum_of_vpos_weights > 0.0f) { + for (uint8_t i=0; i 0.0f && (_blend_mask & BLEND_MASK_USE_SPD_ACC)) { + // calculate the weights using the inverse of the variances + float sum_of_spd_weights = 0.0f; + for (uint8_t i=0; i= GPS_OK_FIX_3D && state[i].speed_accuracy >= 0.001f) { + spd_blend_weights[i] = speed_accuracy_sum_sq / (state[i].speed_accuracy * state[i].speed_accuracy); + sum_of_spd_weights += spd_blend_weights[i]; + } + } + // normalise the weights + if (sum_of_spd_weights > 0.0f) { + for (uint8_t i=0; i state[GPS_BLENDED_INSTANCE].status) { + state[GPS_BLENDED_INSTANCE].status = state[i].status; + } + + // calculate a blended average velocity + state[GPS_BLENDED_INSTANCE].velocity += state[i].velocity * _blend_weights[i]; + + // report the best valid accuracies and DOP metrics + + if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f && state[i].horizontal_accuracy < state[GPS_BLENDED_INSTANCE].horizontal_accuracy) { + state[GPS_BLENDED_INSTANCE].have_horizontal_accuracy = true; + state[GPS_BLENDED_INSTANCE].horizontal_accuracy = state[i].horizontal_accuracy; + } + + if (state[i].have_vertical_accuracy && state[i].vertical_accuracy > 0.0f && state[i].vertical_accuracy < state[GPS_BLENDED_INSTANCE].vertical_accuracy) { + state[GPS_BLENDED_INSTANCE].have_vertical_accuracy = true; + state[GPS_BLENDED_INSTANCE].vertical_accuracy = state[i].vertical_accuracy; + } + + if (state[i].have_vertical_velocity) { + state[GPS_BLENDED_INSTANCE].have_vertical_velocity = true; + } + + if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f && state[i].speed_accuracy < state[GPS_BLENDED_INSTANCE].speed_accuracy) { + state[GPS_BLENDED_INSTANCE].have_speed_accuracy = true; + state[GPS_BLENDED_INSTANCE].speed_accuracy = state[i].speed_accuracy; + } + + if (state[i].hdop > 0 && state[i].hdop < state[GPS_BLENDED_INSTANCE].hdop) { + state[GPS_BLENDED_INSTANCE].hdop = state[i].hdop; + } + + if (state[i].vdop > 0 && state[i].vdop < state[GPS_BLENDED_INSTANCE].vdop) { + state[GPS_BLENDED_INSTANCE].vdop = state[i].vdop; + } + + if (state[i].num_sats > 0 && state[i].num_sats > state[GPS_BLENDED_INSTANCE].num_sats) { + state[GPS_BLENDED_INSTANCE].num_sats = state[i].num_sats; + } + + // report a blended average GPS antenna position + Vector3f temp_antenna_offset = _antenna_offset[i]; + temp_antenna_offset *= _blend_weights[i]; + _blended_antenna_offset += temp_antenna_offset; + + // blend the timing data + if (timing[i].last_fix_time_ms > timing[GPS_BLENDED_INSTANCE].last_fix_time_ms) { + timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = timing[i].last_fix_time_ms; + } + if (timing[i].last_message_time_ms > timing[GPS_BLENDED_INSTANCE].last_message_time_ms) { + timing[GPS_BLENDED_INSTANCE].last_message_time_ms = timing[i].last_message_time_ms; + } + + } + + /* + * Calculate an instantaneous weighted/blended average location from the available GPS instances and store in the _output_state. + * This will be statistically the most likely location, but will be not stable enough for direct use by the autopilot. + */ + + // Use the GPS with the highest weighting as the reference position + float best_weight = 0.0f; + uint8_t best_index = 0; + for (uint8_t i=0; i best_weight) { + best_weight = _blend_weights[i]; + best_index = i; + state[GPS_BLENDED_INSTANCE].location = state[i].location; + } + } + + // Calculate the weighted sum of horizontal and vertical position offsets relative to the reference position + Vector2f blended_NE_offset_m; + float blended_alt_offset_cm = 0.0f; + blended_NE_offset_m.zero(); + for (uint8_t i=0; i 0.0f && i != best_index) { + blended_NE_offset_m += state[GPS_BLENDED_INSTANCE].location.get_distance_NE(state[i].location) * _blend_weights[i]; + blended_alt_offset_cm += (float)(state[i].location.alt - state[GPS_BLENDED_INSTANCE].location.alt) * _blend_weights[i]; + } + } + + // Add the sum of weighted offsets to the reference location to obtain the blended location + state[GPS_BLENDED_INSTANCE].location.offset(blended_NE_offset_m.x, blended_NE_offset_m.y); + state[GPS_BLENDED_INSTANCE].location.alt += (int)blended_alt_offset_cm; + + // Calculate ground speed and course from blended velocity vector + state[GPS_BLENDED_INSTANCE].ground_speed = norm(state[GPS_BLENDED_INSTANCE].velocity.x, state[GPS_BLENDED_INSTANCE].velocity.y); + state[GPS_BLENDED_INSTANCE].ground_course = wrap_360(degrees(atan2f(state[GPS_BLENDED_INSTANCE].velocity.y, state[GPS_BLENDED_INSTANCE].velocity.x))); + + /* + * The blended location in _output_state.location is not stable enough to be used by the autopilot + * as it will move around as the relative accuracy changes. To mitigate this effect a low-pass filtered + * offset from each GPS location to the blended location is calculated and the filtered offset is applied + * to each receiver. + */ + + // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset + // A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering + float alpha[GPS_MAX_RECEIVERS] = {}; + for (uint8_t i=0; i 0) { + float min_alpha = constrain_float(_omega_lpf * 0.001f * (float)(state[i].last_gps_time_ms - _last_time_updated[i]), 0.0f, 1.0f); + if (_blend_weights[i] > min_alpha) { + alpha[i] = min_alpha / _blend_weights[i]; + } else { + alpha[i] = 1.0f; + } + _last_time_updated[i] = state[i].last_gps_time_ms; + } + } + + // Calculate the offset from each GPS solution to the blended solution + for (uint8_t i=0; i 0) { + // this is our first valid sensor week data + last_week_instance = state[i].time_week; + } else if (last_week_instance != 0 && _blend_weights[i] > 0 && last_week_instance != state[i].time_week) { + // there is valid sensor week data that is inconsistent + weeks_consistent = false; + } + } + // calculate output + if (!weeks_consistent) { + // use data from highest weighted sensor + state[GPS_BLENDED_INSTANCE].time_week = state[best_index].time_week; + state[GPS_BLENDED_INSTANCE].time_week_ms = state[best_index].time_week_ms; + state[GPS_BLENDED_INSTANCE].zr_date_timestamp = state[best_index].zr_date_timestamp; + } else { + // use week number from highest weighting GPS (they should all have the same week number) + state[GPS_BLENDED_INSTANCE].time_week = state[best_index].time_week; + // calculate a blended value for the number of ms lapsed in the week + double temp_time_0 = 0.0; + for (uint8_t i=0; i 0.0f) { + temp_time_0 += (double)state[i].time_week_ms * (double)_blend_weights[i]; + } + } + state[GPS_BLENDED_INSTANCE].time_week_ms = (uint32_t)temp_time_0; + state[GPS_BLENDED_INSTANCE].zr_date_timestamp = state[best_index].zr_date_timestamp; + } + + // calculate a blended value for the timing data and lag + double temp_time_1 = 0.0; + double temp_time_2 = 0.0; + for (uint8_t i=0; i 0.0f) { + temp_time_1 += (double)timing[i].last_fix_time_ms * (double) _blend_weights[i]; + temp_time_2 += (double)timing[i].last_message_time_ms * (double)_blend_weights[i]; + float gps_lag_sec = 0; + get_lag(i, gps_lag_sec); + _blended_lag_sec += gps_lag_sec * _blend_weights[i]; + } + } + timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = (uint32_t)temp_time_1; + timing[GPS_BLENDED_INSTANCE].last_message_time_ms = (uint32_t)temp_time_2; +} +#endif // GPS_BLENDED_INSTANCE + +bool AP_GPS::is_healthy(uint8_t instance) const +{ + if (instance >= GPS_MAX_INSTANCES) { + return false; + } + + const uint16_t gps_max_delta_ms = 245; // 200 ms (5Hz) + 45 ms buffer + + bool last_msg_valid = last_message_delta_time_ms(instance) < gps_max_delta_ms; + +#if defined(GPS_BLENDED_INSTANCE) + if (instance == GPS_BLENDED_INSTANCE) { + return last_msg_valid && blend_health_check(); + } +#endif + + return last_msg_valid && + drivers[instance] != nullptr && + drivers[instance]->is_healthy(); +} + +bool AP_GPS::prepare_for_arming(void) { + bool all_passed = true; + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (drivers[i] != nullptr) { + all_passed &= drivers[i]->prepare_for_arming(); + } + } + return all_passed; +} + +namespace AP { + +AP_GPS &gps() +{ + return *AP_GPS::get_singleton(); +} + +}; diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 50481ef234..03dfe975b2 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -1,588 +1,588 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ -#pragma once - -#include -#include -#include -#include -#include -#include "GPS_detect_state.h" -#include - -/** - maximum number of GPS instances available on this platform. If more - than 1 then redundant sensors may be available - */ -#ifndef GPS_MAX_RECEIVERS -#define GPS_MAX_RECEIVERS 2 // maximum number of physical GPS sensors allowed - does not include virtual GPS created by blending receiver data -#endif -#ifndef GPS_MAX_INSTANCES -#define GPS_MAX_INSTANCES (GPS_MAX_RECEIVERS + 1) // maximum number of GPS instances including the 'virtual' GPS created by blending receiver data -#endif -#if GPS_MAX_INSTANCES > GPS_MAX_RECEIVERS -#define GPS_BLENDED_INSTANCE GPS_MAX_RECEIVERS // the virtual blended GPS is always the highest instance (2) -#endif -#define GPS_UNKNOWN_DOP UINT16_MAX // set unknown DOP's to maximum value, which is also correct for MAVLink - -// the number of GPS leap seconds -#define GPS_LEAPSECONDS_MILLIS 18000ULL - -#define UNIX_OFFSET_MSEC (17000ULL * 86400ULL + 52ULL * 10ULL * AP_MSEC_PER_WEEK - GPS_LEAPSECONDS_MILLIS) - -class AP_GPS_Backend; - -/// @class AP_GPS -/// GPS driver main class -class AP_GPS -{ - friend class AP_GPS_ERB; - friend class AP_GPS_GSOF; - friend class AP_GPS_MAV; - friend class AP_GPS_MTK; - friend class AP_GPS_MTK19; - friend class AP_GPS_NMEA; - friend class AP_GPS_NOVA; - friend class AP_GPS_PX4; - friend class AP_GPS_SBF; - friend class AP_GPS_SBP; - friend class AP_GPS_SBP2; - friend class AP_GPS_SIRF; - friend class AP_GPS_UBLOX; - friend class AP_GPS_Backend; - -public: - AP_GPS(); - - /* Do not allow copies */ - AP_GPS(const AP_GPS &other) = delete; - AP_GPS &operator=(const AP_GPS&) = delete; - - static AP_GPS *get_singleton() { - return _singleton; - } - - // GPS driver types - enum GPS_Type { - GPS_TYPE_NONE = 0, - GPS_TYPE_AUTO = 1, - GPS_TYPE_UBLOX = 2, - GPS_TYPE_MTK = 3, - GPS_TYPE_MTK19 = 4, - GPS_TYPE_NMEA = 5, - GPS_TYPE_SIRF = 6, - GPS_TYPE_HIL = 7, - GPS_TYPE_SBP = 8, - GPS_TYPE_UAVCAN = 9, - GPS_TYPE_SBF = 10, - GPS_TYPE_GSOF = 11, - GPS_TYPE_ERB = 13, - GPS_TYPE_MAV = 14, - GPS_TYPE_NOVA = 15, - GPS_TYPE_HEMI = 16, // hemisphere NMEA - }; - - /// GPS status codes - enum GPS_Status { - NO_GPS = GPS_FIX_TYPE_NO_GPS, ///< No GPS connected/detected - NO_FIX = GPS_FIX_TYPE_NO_FIX, ///< Receiving valid GPS messages but no lock - GPS_OK_FIX_2D = GPS_FIX_TYPE_2D_FIX, ///< Receiving valid messages and 2D lock - GPS_OK_FIX_3D = GPS_FIX_TYPE_3D_FIX, ///< Receiving valid messages and 3D lock - GPS_OK_FIX_3D_DGPS = GPS_FIX_TYPE_DGPS, ///< Receiving valid messages and 3D lock with differential improvements - GPS_OK_FIX_3D_RTK_FLOAT = GPS_FIX_TYPE_RTK_FLOAT, ///< Receiving valid messages and 3D RTK Float - GPS_OK_FIX_3D_RTK_FIXED = GPS_FIX_TYPE_RTK_FIXED, ///< Receiving valid messages and 3D RTK Fixed - }; - - // GPS navigation engine settings. Not all GPS receivers support - // this - enum GPS_Engine_Setting { - GPS_ENGINE_NONE = -1, - GPS_ENGINE_PORTABLE = 0, - GPS_ENGINE_STATIONARY = 2, - GPS_ENGINE_PEDESTRIAN = 3, - GPS_ENGINE_AUTOMOTIVE = 4, - GPS_ENGINE_SEA = 5, - GPS_ENGINE_AIRBORNE_1G = 6, - GPS_ENGINE_AIRBORNE_2G = 7, - GPS_ENGINE_AIRBORNE_4G = 8 - }; - - enum GPS_Config { - GPS_ALL_CONFIGURED = 255 - }; - - /* - The GPS_State structure is filled in by the backend driver as it - parses each message from the GPS. - */ - struct GPS_State { - uint8_t instance; // the instance number of this GPS - - // all the following fields must all be filled by the backend driver - GPS_Status status; ///< driver fix status - uint32_t time_week_ms; ///< GPS time (milliseconds from start of GPS week) - uint16_t time_week; ///< GPS week number - Location location; ///< last fix location - float ground_speed; ///< ground speed in m/sec - float ground_course; ///< ground course in degrees - float gps_yaw; ///< GPS derived yaw information, if available (degrees) - uint16_t hdop; ///< horizontal dilution of precision in cm - uint16_t vdop; ///< vertical dilution of precision in cm - uint8_t num_sats; ///< Number of visible satellites - Vector3f velocity; ///< 3D velocity in m/s, in NED format - float speed_accuracy; ///< 3D velocity RMS accuracy estimate in m/s - float horizontal_accuracy; ///< horizontal RMS accuracy estimate in m - float vertical_accuracy; ///< vertical RMS accuracy estimate in m - float gps_yaw_accuracy; ///< heading accuracy of the GPS in degrees - bool have_vertical_velocity; ///< does GPS give vertical velocity? Set to true only once available. - bool have_speed_accuracy; ///< does GPS give speed accuracy? Set to true only once available. - bool have_horizontal_accuracy; ///< does GPS give horizontal position accuracy? Set to true only once available. - bool have_vertical_accuracy; ///< does GPS give vertical position accuracy? Set to true only once available. - bool have_gps_yaw; ///< does GPS give yaw? Set to true only once available. - bool have_gps_yaw_accuracy; ///< does the GPS give a heading accuracy estimate? Set to true only once available - uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds - uint32_t uart_timestamp_ms; ///< optional timestamp from set_uart_timestamp() - - // all the following fields must only all be filled by RTK capable backend drivers - uint32_t rtk_time_week_ms; ///< GPS Time of Week of last baseline in milliseconds - uint16_t rtk_week_number; ///< GPS Week Number of last baseline - uint32_t rtk_age_ms; ///< GPS age of last baseline correction in milliseconds (0 when no corrections, 0xFFFFFFFF indicates overflow) - uint8_t rtk_num_sats; ///< Current number of satellites used for RTK calculation - uint8_t rtk_baseline_coords_type; ///< Coordinate system of baseline. 0 == ECEF, 1 == NED - int32_t rtk_baseline_x_mm; ///< Current baseline in ECEF x or NED north component in mm - int32_t rtk_baseline_y_mm; ///< Current baseline in ECEF y or NED east component in mm - int32_t rtk_baseline_z_mm; ///< Current baseline in ECEF z or NED down component in mm - uint32_t rtk_accuracy; ///< Current estimate of 3D baseline accuracy (receiver dependent, typical 0 to 9999) - int32_t rtk_iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses - }; - - /// Startup initialisation. - void init(const AP_SerialManager& serial_manager); - - /// Update GPS state based on possible bytes received from the module. - /// This routine must be called periodically (typically at 10Hz or - /// more) to process incoming data. - void update(void); - - // Pass mavlink data to message handlers (for MAV type) - void handle_msg(const mavlink_message_t &msg); - - // Accessor functions - - // return number of active GPS sensors. Note that if the first GPS - // is not present but the 2nd is then we return 2. Note that a blended - // GPS solution is treated as an additional sensor. - uint8_t num_sensors(void) const; - - // Return the index of the primary sensor which is the index of the sensor contributing to - // the output. A blended solution is available as an additional instance - uint8_t primary_sensor(void) const { - return primary_instance; - } - - /// Query GPS status - GPS_Status status(uint8_t instance) const { - if (_force_disable_gps && state[instance].status > NO_FIX) { - return NO_FIX; - } - return state[instance].status; - } - GPS_Status status(void) const { - return status(primary_instance); - } - - // Query the highest status this GPS supports (always reports GPS_OK_FIX_3D for the blended GPS) - GPS_Status highest_supported_status(uint8_t instance) const; - - // location of last fix - const Location &location(uint8_t instance) const { - return state[instance].location; - } - const Location &location() const { - return location(primary_instance); - } - - // report speed accuracy - bool speed_accuracy(uint8_t instance, float &sacc) const; - bool speed_accuracy(float &sacc) const { - return speed_accuracy(primary_instance, sacc); - } - - bool horizontal_accuracy(uint8_t instance, float &hacc) const; - bool horizontal_accuracy(float &hacc) const { - return horizontal_accuracy(primary_instance, hacc); - } - - bool vertical_accuracy(uint8_t instance, float &vacc) const; - bool vertical_accuracy(float &vacc) const { - return vertical_accuracy(primary_instance, vacc); - } - - // 3D velocity in NED format - const Vector3f &velocity(uint8_t instance) const { - return state[instance].velocity; - } - const Vector3f &velocity() const { - return velocity(primary_instance); - } - - // ground speed in m/s - float ground_speed(uint8_t instance) const { - return state[instance].ground_speed; - } - float ground_speed() const { - return ground_speed(primary_instance); - } - - // ground speed in cm/s - uint32_t ground_speed_cm(void) { - return ground_speed() * 100; - } - - // ground course in degrees - float ground_course(uint8_t instance) const { - return state[instance].ground_course; - } - float ground_course() const { - return ground_course(primary_instance); - } - // ground course in centi-degrees - int32_t ground_course_cd(uint8_t instance) const { - return ground_course(instance) * 100; - } - int32_t ground_course_cd() const { - return ground_course_cd(primary_instance); - } - - // yaw in degrees if available - bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg) const { - if (!have_gps_yaw(instance)) { - return false; - } - yaw_deg = state[instance].gps_yaw; - if (state[instance].have_gps_yaw_accuracy) { - accuracy_deg = state[instance].gps_yaw_accuracy; - } else { - // fall back to 10 degrees as a generic default - accuracy_deg = 10; - } - return true; - } - bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg) const { - return gps_yaw_deg(primary_instance, yaw_deg, accuracy_deg); - } - - // number of locked satellites - uint8_t num_sats(uint8_t instance) const { - return state[instance].num_sats; - } - uint8_t num_sats() const { - return num_sats(primary_instance); - } - - // GPS time of week in milliseconds - uint32_t time_week_ms(uint8_t instance) const { - return state[instance].time_week_ms; - } - uint32_t time_week_ms() const { - return time_week_ms(primary_instance); - } - - // GPS week - uint16_t time_week(uint8_t instance) const { - return state[instance].time_week; - } - uint16_t time_week() const { - return time_week(primary_instance); - } - - // horizontal dilution of precision - uint16_t get_hdop(uint8_t instance) const { - return state[instance].hdop; - } - uint16_t get_hdop() const { - return get_hdop(primary_instance); - } - - // vertical dilution of precision - uint16_t get_vdop(uint8_t instance) const { - return state[instance].vdop; - } - uint16_t get_vdop() const { - return get_vdop(primary_instance); - } - - // the time we got our last fix in system milliseconds. This is - // used when calculating how far we might have moved since that fix - uint32_t last_fix_time_ms(uint8_t instance) const { - return timing[instance].last_fix_time_ms; - } - uint32_t last_fix_time_ms(void) const { - return last_fix_time_ms(primary_instance); - } - - // the time we last processed a message in milliseconds. This is - // used to indicate that we have new GPS data to process - uint32_t last_message_time_ms(uint8_t instance) const { - return timing[instance].last_message_time_ms; - } - uint32_t last_message_time_ms(void) const { - return last_message_time_ms(primary_instance); - } - - // system time delta between the last two reported positions - uint16_t last_message_delta_time_ms(uint8_t instance) const { - return timing[instance].delta_time_ms; - } - uint16_t last_message_delta_time_ms(void) const { - return last_message_delta_time_ms(primary_instance); - } - - // return true if the GPS supports vertical velocity values - bool have_vertical_velocity(uint8_t instance) const { - return state[instance].have_vertical_velocity; - } - bool have_vertical_velocity(void) const { - return have_vertical_velocity(primary_instance); - } - - // return true if the GPS supports yaw - bool have_gps_yaw(uint8_t instance) const { - return state[instance].have_gps_yaw; - } - bool have_gps_yaw(void) const { - return have_gps_yaw(primary_instance); - } - - // the expected lag (in seconds) in the position and velocity readings from the gps - // return true if the GPS hardware configuration is known or the lag parameter has been set manually - bool get_lag(uint8_t instance, float &lag_sec) const; - bool get_lag(float &lag_sec) const { - return get_lag(primary_instance, lag_sec); - } - - // return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin - const Vector3f &get_antenna_offset(uint8_t instance) const; - - // set position for HIL - void setHIL(uint8_t instance, GPS_Status status, uint64_t time_epoch_ms, - const Location &location, const Vector3f &velocity, uint8_t num_sats, - uint16_t hdop); - - // set accuracy for HIL - void setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc, bool _have_vertical_velocity, uint32_t sample_ms); - - // lock out a GPS port, allowing another application to use the port - void lock_port(uint8_t instance, bool locked); - - //MAVLink Status Sending - void send_mavlink_gps_raw(mavlink_channel_t chan); - void send_mavlink_gps2_raw(mavlink_channel_t chan); - - void send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst); - - // Returns true if there is an unconfigured GPS, and provides the instance number of the first non configured GPS - bool first_unconfigured_gps(uint8_t &instance) const WARN_IF_UNUSED; - void broadcast_first_configuration_failure_reason(void) const; - - // pre-arm check that all GPSs are close to each other. farthest distance between GPSs (in meters) is returned - bool all_consistent(float &distance) const; - - // pre-arm check of GPS blending. False if blending is unhealthy, True if healthy or blending is not being used - bool blend_health_check() const; - - // handle sending of initialisation strings to the GPS - only used by backends - void send_blob_start(uint8_t instance, const char *_blob, uint16_t size); - void send_blob_update(uint8_t instance); - - // return last fix time since the 1/1/1970 in microseconds - uint64_t time_epoch_usec(uint8_t instance) const; - uint64_t time_epoch_usec(void) const { - return time_epoch_usec(primary_instance); - } - - // convert GPS week and millis to unix epoch in ms - static uint64_t time_epoch_convert(uint16_t gps_week, uint32_t gps_ms); - - static const struct AP_Param::GroupInfo var_info[]; - - void Write_AP_Logger_Log_Startup_messages(); - - // indicate which bit in LOG_BITMASK indicates gps logging enabled - void set_log_gps_bit(uint32_t bit) { _log_gps_bit = bit; } - - // report if the gps is healthy (this is defined as existing, an update at a rate greater than 4Hz, - // as well as any driver specific behaviour) - bool is_healthy(uint8_t instance) const; - bool is_healthy(void) const { return is_healthy(primary_instance); } - - // returns true if all GPS instances have passed all final arming checks/state changes - bool prepare_for_arming(void); - - // used to disable GPS for GPS failure testing in flight - void force_disable(bool disable) { - _force_disable_gps = disable; - } - - // handle possibly fragmented RTCM injection data - void handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len); - -protected: - - // configuration parameters - AP_Int8 _type[GPS_MAX_RECEIVERS]; - AP_Int8 _navfilter; - AP_Int8 _auto_switch; - AP_Int8 _min_dgps; - AP_Int16 _sbp_logmask; - AP_Int8 _inject_to; - uint32_t _last_instance_swap_ms; - AP_Int8 _sbas_mode; - AP_Int8 _min_elevation; - AP_Int8 _raw_data; - AP_Int8 _gnss_mode[GPS_MAX_RECEIVERS]; - AP_Int16 _rate_ms[GPS_MAX_RECEIVERS]; // this parameter should always be accessed using get_rate_ms() - AP_Int8 _save_config; - AP_Int8 _auto_config; - AP_Vector3f _antenna_offset[GPS_MAX_RECEIVERS]; - AP_Int16 _delay_ms[GPS_MAX_RECEIVERS]; - AP_Int8 _blend_mask; - AP_Float _blend_tc; - - uint32_t _log_gps_bit = -1; - -private: - static AP_GPS *_singleton; - - // returns the desired gps update rate in milliseconds - // this does not provide any guarantee that the GPS is updating at the requested - // rate it is simply a helper for use in the backends for determining what rate - // they should be configuring the GPS to run at - uint16_t get_rate_ms(uint8_t instance) const; - - struct GPS_timing { - // the time we got our last fix in system milliseconds - uint32_t last_fix_time_ms; - - // the time we got our last message in system milliseconds - uint32_t last_message_time_ms; - - // delta time between the last pair of GPS updates in system milliseconds - uint16_t delta_time_ms; - }; - // Note allowance for an additional instance to contain blended data - GPS_timing timing[GPS_MAX_INSTANCES]; - GPS_State state[GPS_MAX_INSTANCES]; - AP_GPS_Backend *drivers[GPS_MAX_RECEIVERS]; - AP_HAL::UARTDriver *_port[GPS_MAX_RECEIVERS]; - - /// primary GPS instance - uint8_t primary_instance; - - /// number of GPS instances present - uint8_t num_instances; - - // which ports are locked - uint8_t locked_ports; - - // state of auto-detection process, per instance - struct detect_state { - uint32_t last_baud_change_ms; - uint8_t current_baud; - bool auto_detected_baud; - struct UBLOX_detect_state ublox_detect_state; - struct MTK_detect_state mtk_detect_state; - struct MTK19_detect_state mtk19_detect_state; - struct SIRF_detect_state sirf_detect_state; - struct NMEA_detect_state nmea_detect_state; - struct SBP_detect_state sbp_detect_state; - struct SBP2_detect_state sbp2_detect_state; - struct ERB_detect_state erb_detect_state; - } detect_state[GPS_MAX_RECEIVERS]; - - struct { - const char *blob; - uint16_t remaining; - } initblob_state[GPS_MAX_RECEIVERS]; - - static const uint32_t _baudrates[]; - static const char _initialisation_blob[]; - static const char _initialisation_raw_blob[]; - - void detect_instance(uint8_t instance); - void update_instance(uint8_t instance); - - /* - buffer for re-assembling RTCM data for GPS injection. - The 8 bit flags field in GPS_RTCM_DATA is interpreted as: - 1 bit for "is fragmented" - 2 bits for fragment number - 5 bits for sequence number - - The rtcm_buffer is allocated on first use. Once a block of data - is successfully reassembled it is injected into all active GPS - backends. This assumes we don't want more than 4*180=720 bytes - in a RTCM data block - */ - struct rtcm_buffer { - uint8_t fragments_received; - uint8_t sequence; - uint8_t fragment_count; - uint16_t total_length; - uint8_t buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*4]; - } *rtcm_buffer; - - // re-assemble GPS_RTCM_DATA message - void handle_gps_rtcm_data(const mavlink_message_t &msg); - void handle_gps_inject(const mavlink_message_t &msg); - - //Inject a packet of raw binary to a GPS - void inject_data(const uint8_t *data, uint16_t len); - void inject_data(uint8_t instance, const uint8_t *data, uint16_t len); - - // GPS blending and switching - Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS]; // Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m) - float _hgt_offset_cm[GPS_MAX_RECEIVERS]; // Filtered height offset from GPS instance relative to blended solution in _output_state.location (cm) - Vector3f _blended_antenna_offset; // blended antenna offset - float _blended_lag_sec; // blended receiver lag in seconds - float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances. - uint32_t _last_time_updated[GPS_MAX_RECEIVERS]; // the last value of state.last_gps_time_ms read for that GPS instance - used to detect new data. - float _omega_lpf; // cutoff frequency in rad/sec of LPF applied to position offsets - bool _output_is_blended; // true when a blended GPS solution being output - uint8_t _blend_health_counter; // 0 = perfectly health, 100 = very unhealthy - - // calculate the blend weight. Returns true if blend could be calculated, false if not - bool calc_blend_weights(void); - - // calculate the blended state - void calc_blended_state(void); - - bool should_log() const; - - bool needs_uart(GPS_Type type) const; - - // Auto configure types - enum GPS_AUTO_CONFIG { - GPS_AUTO_CONFIG_DISABLE = 0, - GPS_AUTO_CONFIG_ENABLE = 1 - }; - - // used for flight testing with GPS loss - bool _force_disable_gps; -}; - -namespace AP { - AP_GPS &gps(); -}; +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include +#include +#include +#include +#include +#include "GPS_detect_state.h" +#include + +/** + maximum number of GPS instances available on this platform. If more + than 1 then redundant sensors may be available + */ +#ifndef GPS_MAX_RECEIVERS +#define GPS_MAX_RECEIVERS 2 // maximum number of physical GPS sensors allowed - does not include virtual GPS created by blending receiver data +#endif +#ifndef GPS_MAX_INSTANCES +#define GPS_MAX_INSTANCES (GPS_MAX_RECEIVERS + 1) // maximum number of GPS instances including the 'virtual' GPS created by blending receiver data +#endif +#if GPS_MAX_INSTANCES > GPS_MAX_RECEIVERS +#define GPS_BLENDED_INSTANCE GPS_MAX_RECEIVERS // the virtual blended GPS is always the highest instance (2) +#endif +#define GPS_UNKNOWN_DOP UINT16_MAX // set unknown DOP's to maximum value, which is also correct for MAVLink + +// the number of GPS leap seconds +#define GPS_LEAPSECONDS_MILLIS 18000ULL + +#define UNIX_OFFSET_MSEC (17000ULL * 86400ULL + 52ULL * 10ULL * AP_MSEC_PER_WEEK - GPS_LEAPSECONDS_MILLIS) + +class AP_GPS_Backend; + +/// @class AP_GPS +/// GPS driver main class +class AP_GPS +{ + friend class AP_GPS_ERB; + friend class AP_GPS_GSOF; + friend class AP_GPS_MAV; + friend class AP_GPS_MTK; + friend class AP_GPS_MTK19; + friend class AP_GPS_NMEA; + friend class AP_GPS_NOVA; + friend class AP_GPS_PX4; + friend class AP_GPS_SBF; + friend class AP_GPS_SBP; + friend class AP_GPS_SBP2; + friend class AP_GPS_SIRF; + friend class AP_GPS_UBLOX; + friend class AP_GPS_Backend; + +public: + AP_GPS(); + + /* Do not allow copies */ + AP_GPS(const AP_GPS &other) = delete; + AP_GPS &operator=(const AP_GPS&) = delete; + + static AP_GPS *get_singleton() { + return _singleton; + } + + // GPS driver types + enum GPS_Type { + GPS_TYPE_NONE = 0, + GPS_TYPE_AUTO = 1, + GPS_TYPE_UBLOX = 2, + GPS_TYPE_MTK = 3, + GPS_TYPE_MTK19 = 4, + GPS_TYPE_NMEA = 5, + GPS_TYPE_SIRF = 6, + GPS_TYPE_HIL = 7, + GPS_TYPE_SBP = 8, + GPS_TYPE_UAVCAN = 9, + GPS_TYPE_SBF = 10, + GPS_TYPE_GSOF = 11, + GPS_TYPE_ERB = 13, + GPS_TYPE_MAV = 14, + GPS_TYPE_NOVA = 15, + GPS_TYPE_HEMI = 16, // hemisphere NMEA + }; + + /// GPS status codes + enum GPS_Status { + NO_GPS = GPS_FIX_TYPE_NO_GPS, ///< No GPS connected/detected + NO_FIX = GPS_FIX_TYPE_NO_FIX, ///< Receiving valid GPS messages but no lock + GPS_OK_FIX_2D = GPS_FIX_TYPE_2D_FIX, ///< Receiving valid messages and 2D lock + GPS_OK_FIX_3D = GPS_FIX_TYPE_3D_FIX, ///< Receiving valid messages and 3D lock + GPS_OK_FIX_3D_DGPS = GPS_FIX_TYPE_DGPS, ///< Receiving valid messages and 3D lock with differential improvements + GPS_OK_FIX_3D_RTK_FLOAT = GPS_FIX_TYPE_RTK_FLOAT, ///< Receiving valid messages and 3D RTK Float + GPS_OK_FIX_3D_RTK_FIXED = GPS_FIX_TYPE_RTK_FIXED, ///< Receiving valid messages and 3D RTK Fixed + }; + + // GPS navigation engine settings. Not all GPS receivers support + // this + enum GPS_Engine_Setting { + GPS_ENGINE_NONE = -1, + GPS_ENGINE_PORTABLE = 0, + GPS_ENGINE_STATIONARY = 2, + GPS_ENGINE_PEDESTRIAN = 3, + GPS_ENGINE_AUTOMOTIVE = 4, + GPS_ENGINE_SEA = 5, + GPS_ENGINE_AIRBORNE_1G = 6, + GPS_ENGINE_AIRBORNE_2G = 7, + GPS_ENGINE_AIRBORNE_4G = 8 + }; + + enum GPS_Config { + GPS_ALL_CONFIGURED = 255 + }; + + /* + The GPS_State structure is filled in by the backend driver as it + parses each message from the GPS. + */ + struct GPS_State { + uint8_t instance; // the instance number of this GPS + + // all the following fields must all be filled by the backend driver + GPS_Status status; ///< driver fix status + uint32_t time_week_ms; ///< GPS time (milliseconds from start of GPS week) + uint16_t time_week; ///< GPS week number + Location location; ///< last fix location + float ground_speed; ///< ground speed in m/sec + float ground_course; ///< ground course in degrees + float gps_yaw; ///< GPS derived yaw information, if available (degrees) + uint16_t hdop; ///< horizontal dilution of precision in cm + uint16_t vdop; ///< vertical dilution of precision in cm + uint8_t num_sats; ///< Number of visible satellites + Vector3f velocity; ///< 3D velocity in m/s, in NED format + float speed_accuracy; ///< 3D velocity RMS accuracy estimate in m/s + float horizontal_accuracy; ///< horizontal RMS accuracy estimate in m + float vertical_accuracy; ///< vertical RMS accuracy estimate in m + float gps_yaw_accuracy; ///< heading accuracy of the GPS in degrees + bool have_vertical_velocity; ///< does GPS give vertical velocity? Set to true only once available. + bool have_speed_accuracy; ///< does GPS give speed accuracy? Set to true only once available. + bool have_horizontal_accuracy; ///< does GPS give horizontal position accuracy? Set to true only once available. + bool have_vertical_accuracy; ///< does GPS give vertical position accuracy? Set to true only once available. + bool have_gps_yaw; ///< does GPS give yaw? Set to true only once available. + bool have_gps_yaw_accuracy; ///< does the GPS give a heading accuracy estimate? Set to true only once available + uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds + uint32_t uart_timestamp_ms; ///< optional timestamp from set_uart_timestamp() + uint32_t zr_date_timestamp; //2020-03-26 = 20200326 + // all the following fields must only all be filled by RTK capable backend drivers + uint32_t rtk_time_week_ms; ///< GPS Time of Week of last baseline in milliseconds + uint16_t rtk_week_number; ///< GPS Week Number of last baseline + uint32_t rtk_age_ms; ///< GPS age of last baseline correction in milliseconds (0 when no corrections, 0xFFFFFFFF indicates overflow) + uint8_t rtk_num_sats; ///< Current number of satellites used for RTK calculation + uint8_t rtk_baseline_coords_type; ///< Coordinate system of baseline. 0 == ECEF, 1 == NED + int32_t rtk_baseline_x_mm; ///< Current baseline in ECEF x or NED north component in mm + int32_t rtk_baseline_y_mm; ///< Current baseline in ECEF y or NED east component in mm + int32_t rtk_baseline_z_mm; ///< Current baseline in ECEF z or NED down component in mm + uint32_t rtk_accuracy; ///< Current estimate of 3D baseline accuracy (receiver dependent, typical 0 to 9999) + int32_t rtk_iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses + }; + + /// Startup initialisation. + void init(const AP_SerialManager& serial_manager); + + /// Update GPS state based on possible bytes received from the module. + /// This routine must be called periodically (typically at 10Hz or + /// more) to process incoming data. + void update(void); + + // Pass mavlink data to message handlers (for MAV type) + void handle_msg(const mavlink_message_t &msg); + + // Accessor functions + + // return number of active GPS sensors. Note that if the first GPS + // is not present but the 2nd is then we return 2. Note that a blended + // GPS solution is treated as an additional sensor. + uint8_t num_sensors(void) const; + + // Return the index of the primary sensor which is the index of the sensor contributing to + // the output. A blended solution is available as an additional instance + uint8_t primary_sensor(void) const { + return primary_instance; + } + + /// Query GPS status + GPS_Status status(uint8_t instance) const { + if (_force_disable_gps && state[instance].status > NO_FIX) { + return NO_FIX; + } + return state[instance].status; + } + GPS_Status status(void) const { + return status(primary_instance); + } + + // Query the highest status this GPS supports (always reports GPS_OK_FIX_3D for the blended GPS) + GPS_Status highest_supported_status(uint8_t instance) const; + + // location of last fix + const Location &location(uint8_t instance) const { + return state[instance].location; + } + const Location &location() const { + return location(primary_instance); + } + + // report speed accuracy + bool speed_accuracy(uint8_t instance, float &sacc) const; + bool speed_accuracy(float &sacc) const { + return speed_accuracy(primary_instance, sacc); + } + + bool horizontal_accuracy(uint8_t instance, float &hacc) const; + bool horizontal_accuracy(float &hacc) const { + return horizontal_accuracy(primary_instance, hacc); + } + + bool vertical_accuracy(uint8_t instance, float &vacc) const; + bool vertical_accuracy(float &vacc) const { + return vertical_accuracy(primary_instance, vacc); + } + + // 3D velocity in NED format + const Vector3f &velocity(uint8_t instance) const { + return state[instance].velocity; + } + const Vector3f &velocity() const { + return velocity(primary_instance); + } + + // ground speed in m/s + float ground_speed(uint8_t instance) const { + return state[instance].ground_speed; + } + float ground_speed() const { + return ground_speed(primary_instance); + } + + // ground speed in cm/s + uint32_t ground_speed_cm(void) { + return ground_speed() * 100; + } + + // ground course in degrees + float ground_course(uint8_t instance) const { + return state[instance].ground_course; + } + float ground_course() const { + return ground_course(primary_instance); + } + // ground course in centi-degrees + int32_t ground_course_cd(uint8_t instance) const { + return ground_course(instance) * 100; + } + int32_t ground_course_cd() const { + return ground_course_cd(primary_instance); + } + + // yaw in degrees if available + bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg) const { + if (!have_gps_yaw(instance)) { + return false; + } + yaw_deg = state[instance].gps_yaw; + if (state[instance].have_gps_yaw_accuracy) { + accuracy_deg = state[instance].gps_yaw_accuracy; + } else { + // fall back to 10 degrees as a generic default + accuracy_deg = 10; + } + return true; + } + bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg) const { + return gps_yaw_deg(primary_instance, yaw_deg, accuracy_deg); + } + + // number of locked satellites + uint8_t num_sats(uint8_t instance) const { + return state[instance].num_sats; + } + uint8_t num_sats() const { + return num_sats(primary_instance); + } + + // GPS time of week in milliseconds + uint32_t time_week_ms(uint8_t instance) const { + return state[instance].time_week_ms; + } + uint32_t time_week_ms() const { + return time_week_ms(primary_instance); + } + + // GPS week + uint16_t time_week(uint8_t instance) const { + return state[instance].time_week; + } + uint16_t time_week() const { + return time_week(primary_instance); + } + + // horizontal dilution of precision + uint16_t get_hdop(uint8_t instance) const { + return state[instance].hdop; + } + uint16_t get_hdop() const { + return get_hdop(primary_instance); + } + + // vertical dilution of precision + uint16_t get_vdop(uint8_t instance) const { + return state[instance].vdop; + } + uint16_t get_vdop() const { + return get_vdop(primary_instance); + } + + // the time we got our last fix in system milliseconds. This is + // used when calculating how far we might have moved since that fix + uint32_t last_fix_time_ms(uint8_t instance) const { + return timing[instance].last_fix_time_ms; + } + uint32_t last_fix_time_ms(void) const { + return last_fix_time_ms(primary_instance); + } + + // the time we last processed a message in milliseconds. This is + // used to indicate that we have new GPS data to process + uint32_t last_message_time_ms(uint8_t instance) const { + return timing[instance].last_message_time_ms; + } + uint32_t last_message_time_ms(void) const { + return last_message_time_ms(primary_instance); + } + + // system time delta between the last two reported positions + uint16_t last_message_delta_time_ms(uint8_t instance) const { + return timing[instance].delta_time_ms; + } + uint16_t last_message_delta_time_ms(void) const { + return last_message_delta_time_ms(primary_instance); + } + + // return true if the GPS supports vertical velocity values + bool have_vertical_velocity(uint8_t instance) const { + return state[instance].have_vertical_velocity; + } + bool have_vertical_velocity(void) const { + return have_vertical_velocity(primary_instance); + } + + // return true if the GPS supports yaw + bool have_gps_yaw(uint8_t instance) const { + return state[instance].have_gps_yaw; + } + bool have_gps_yaw(void) const { + return have_gps_yaw(primary_instance); + } + + // the expected lag (in seconds) in the position and velocity readings from the gps + // return true if the GPS hardware configuration is known or the lag parameter has been set manually + bool get_lag(uint8_t instance, float &lag_sec) const; + bool get_lag(float &lag_sec) const { + return get_lag(primary_instance, lag_sec); + } + + // return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin + const Vector3f &get_antenna_offset(uint8_t instance) const; + + // set position for HIL + void setHIL(uint8_t instance, GPS_Status status, uint64_t time_epoch_ms, + const Location &location, const Vector3f &velocity, uint8_t num_sats, + uint16_t hdop); + + // set accuracy for HIL + void setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc, bool _have_vertical_velocity, uint32_t sample_ms); + + // lock out a GPS port, allowing another application to use the port + void lock_port(uint8_t instance, bool locked); + + //MAVLink Status Sending + void send_mavlink_gps_raw(mavlink_channel_t chan); + void send_mavlink_gps2_raw(mavlink_channel_t chan); + + void send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst); + + // Returns true if there is an unconfigured GPS, and provides the instance number of the first non configured GPS + bool first_unconfigured_gps(uint8_t &instance) const WARN_IF_UNUSED; + void broadcast_first_configuration_failure_reason(void) const; + + // pre-arm check that all GPSs are close to each other. farthest distance between GPSs (in meters) is returned + bool all_consistent(float &distance) const; + + // pre-arm check of GPS blending. False if blending is unhealthy, True if healthy or blending is not being used + bool blend_health_check() const; + + // handle sending of initialisation strings to the GPS - only used by backends + void send_blob_start(uint8_t instance, const char *_blob, uint16_t size); + void send_blob_update(uint8_t instance); + + // return last fix time since the 1/1/1970 in microseconds + uint64_t time_epoch_usec(uint8_t instance) const; + uint64_t time_epoch_usec(void) const { + return time_epoch_usec(primary_instance); + } + + // convert GPS week and millis to unix epoch in ms + static uint64_t time_epoch_convert(uint16_t gps_week, uint32_t gps_ms); + + static const struct AP_Param::GroupInfo var_info[]; + + void Write_AP_Logger_Log_Startup_messages(); + + // indicate which bit in LOG_BITMASK indicates gps logging enabled + void set_log_gps_bit(uint32_t bit) { _log_gps_bit = bit; } + + // report if the gps is healthy (this is defined as existing, an update at a rate greater than 4Hz, + // as well as any driver specific behaviour) + bool is_healthy(uint8_t instance) const; + bool is_healthy(void) const { return is_healthy(primary_instance); } + + // returns true if all GPS instances have passed all final arming checks/state changes + bool prepare_for_arming(void); + + // used to disable GPS for GPS failure testing in flight + void force_disable(bool disable) { + _force_disable_gps = disable; + } + + // handle possibly fragmented RTCM injection data + void handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len); + void get_gps_headline_value(int32_t & gps_date_time); +protected: + + // configuration parameters + AP_Int8 _type[GPS_MAX_RECEIVERS]; + AP_Int8 _navfilter; + AP_Int8 _auto_switch; + AP_Int8 _min_dgps; + AP_Int16 _sbp_logmask; + AP_Int8 _inject_to; + uint32_t _last_instance_swap_ms; + AP_Int8 _sbas_mode; + AP_Int8 _min_elevation; + AP_Int8 _raw_data; + AP_Int8 _gnss_mode[GPS_MAX_RECEIVERS]; + AP_Int16 _rate_ms[GPS_MAX_RECEIVERS]; // this parameter should always be accessed using get_rate_ms() + AP_Int8 _save_config; + AP_Int8 _auto_config; + AP_Vector3f _antenna_offset[GPS_MAX_RECEIVERS]; + AP_Int16 _delay_ms[GPS_MAX_RECEIVERS]; + AP_Int8 _blend_mask; + AP_Float _blend_tc; + + uint32_t _log_gps_bit = -1; + +private: + static AP_GPS *_singleton; + + // returns the desired gps update rate in milliseconds + // this does not provide any guarantee that the GPS is updating at the requested + // rate it is simply a helper for use in the backends for determining what rate + // they should be configuring the GPS to run at + uint16_t get_rate_ms(uint8_t instance) const; + + struct GPS_timing { + // the time we got our last fix in system milliseconds + uint32_t last_fix_time_ms; + + // the time we got our last message in system milliseconds + uint32_t last_message_time_ms; + + // delta time between the last pair of GPS updates in system milliseconds + uint16_t delta_time_ms; + }; + // Note allowance for an additional instance to contain blended data + GPS_timing timing[GPS_MAX_INSTANCES]; + GPS_State state[GPS_MAX_INSTANCES]; + AP_GPS_Backend *drivers[GPS_MAX_RECEIVERS]; + AP_HAL::UARTDriver *_port[GPS_MAX_RECEIVERS]; + + /// primary GPS instance + uint8_t primary_instance; + + /// number of GPS instances present + uint8_t num_instances; + + // which ports are locked + uint8_t locked_ports; + + // state of auto-detection process, per instance + struct detect_state { + uint32_t last_baud_change_ms; + uint8_t current_baud; + bool auto_detected_baud; + struct UBLOX_detect_state ublox_detect_state; + struct MTK_detect_state mtk_detect_state; + struct MTK19_detect_state mtk19_detect_state; + struct SIRF_detect_state sirf_detect_state; + struct NMEA_detect_state nmea_detect_state; + struct SBP_detect_state sbp_detect_state; + struct SBP2_detect_state sbp2_detect_state; + struct ERB_detect_state erb_detect_state; + } detect_state[GPS_MAX_RECEIVERS]; + + struct { + const char *blob; + uint16_t remaining; + } initblob_state[GPS_MAX_RECEIVERS]; + + static const uint32_t _baudrates[]; + static const char _initialisation_blob[]; + static const char _initialisation_raw_blob[]; + + void detect_instance(uint8_t instance); + void update_instance(uint8_t instance); + + /* + buffer for re-assembling RTCM data for GPS injection. + The 8 bit flags field in GPS_RTCM_DATA is interpreted as: + 1 bit for "is fragmented" + 2 bits for fragment number + 5 bits for sequence number + + The rtcm_buffer is allocated on first use. Once a block of data + is successfully reassembled it is injected into all active GPS + backends. This assumes we don't want more than 4*180=720 bytes + in a RTCM data block + */ + struct rtcm_buffer { + uint8_t fragments_received; + uint8_t sequence; + uint8_t fragment_count; + uint16_t total_length; + uint8_t buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*4]; + } *rtcm_buffer; + + // re-assemble GPS_RTCM_DATA message + void handle_gps_rtcm_data(const mavlink_message_t &msg); + void handle_gps_inject(const mavlink_message_t &msg); + + //Inject a packet of raw binary to a GPS + void inject_data(const uint8_t *data, uint16_t len); + void inject_data(uint8_t instance, const uint8_t *data, uint16_t len); + + // GPS blending and switching + Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS]; // Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m) + float _hgt_offset_cm[GPS_MAX_RECEIVERS]; // Filtered height offset from GPS instance relative to blended solution in _output_state.location (cm) + Vector3f _blended_antenna_offset; // blended antenna offset + float _blended_lag_sec; // blended receiver lag in seconds + float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances. + uint32_t _last_time_updated[GPS_MAX_RECEIVERS]; // the last value of state.last_gps_time_ms read for that GPS instance - used to detect new data. + float _omega_lpf; // cutoff frequency in rad/sec of LPF applied to position offsets + bool _output_is_blended; // true when a blended GPS solution being output + uint8_t _blend_health_counter; // 0 = perfectly health, 100 = very unhealthy + + // calculate the blend weight. Returns true if blend could be calculated, false if not + bool calc_blend_weights(void); + + // calculate the blended state + void calc_blended_state(void); + + bool should_log() const; + + bool needs_uart(GPS_Type type) const; + + // Auto configure types + enum GPS_AUTO_CONFIG { + GPS_AUTO_CONFIG_DISABLE = 0, + GPS_AUTO_CONFIG_ENABLE = 1 + }; + + // used for flight testing with GPS loss + bool _force_disable_gps; +}; + +namespace AP { + AP_GPS &gps(); +}; diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 1a64e3730f..43bed3e985 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -1,291 +1,292 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -#include "AP_GPS.h" -#include "GPS_Backend.h" -#include - -#define GPS_BACKEND_DEBUGGING 0 - -#if GPS_BACKEND_DEBUGGING - # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) -#else - # define Debug(fmt, args ...) -#endif - -#include - -extern const AP_HAL::HAL& hal; - -AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : - port(_port), - gps(_gps), - state(_state) -{ - state.have_speed_accuracy = false; - state.have_horizontal_accuracy = false; - state.have_vertical_accuracy = false; -} - -int32_t AP_GPS_Backend::swap_int32(int32_t v) const -{ - const uint8_t *b = (const uint8_t *)&v; - union { - int32_t v; - uint8_t b[4]; - } u; - - u.b[0] = b[3]; - u.b[1] = b[2]; - u.b[2] = b[1]; - u.b[3] = b[0]; - - return u.v; -} - -int16_t AP_GPS_Backend::swap_int16(int16_t v) const -{ - const uint8_t *b = (const uint8_t *)&v; - union { - int16_t v; - uint8_t b[2]; - } u; - - u.b[0] = b[1]; - u.b[1] = b[0]; - - return u.v; -} - - -/** - fill in time_week_ms and time_week from BCD date and time components - assumes MTK19 millisecond form of bcd_time - */ -void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) -{ - uint8_t year, mon, day, hour, min, sec; - uint16_t msec; - - year = bcd_date % 100; - mon = (bcd_date / 100) % 100; - day = bcd_date / 10000; - - uint32_t v = bcd_milliseconds; - msec = v % 1000; v /= 1000; - sec = v % 100; v /= 100; - min = v % 100; v /= 100; - hour = v % 100; - - int8_t rmon = mon - 2; - if (0 >= rmon) { - rmon += 12; - year -= 1; - } - - // get time in seconds since unix epoch - uint32_t ret = (year/4) - (GPS_LEAPSECONDS_MILLIS / 1000UL) + 367*rmon/12 + day; - ret += year*365 + 10501; - ret = ret*24 + hour; - ret = ret*60 + min; - ret = ret*60 + sec; - - // convert to time since GPS epoch - ret -= 272764785UL; - - // get GPS week and time - state.time_week = ret / AP_SEC_PER_WEEK; - state.time_week_ms = (ret % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC; - state.time_week_ms += msec; -} - -/* - fill in 3D velocity for a GPS that doesn't give vertical velocity numbers - */ -void AP_GPS_Backend::fill_3d_velocity(void) -{ - float gps_heading = radians(state.ground_course); - - state.velocity.x = state.ground_speed * cosf(gps_heading); - state.velocity.y = state.ground_speed * sinf(gps_heading); - state.velocity.z = 0; - state.have_vertical_velocity = false; -} - -void -AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len) -{ - // not all backends have valid ports - if (port != nullptr) { - if (port->txspace() > len) { - port->write(data, len); - } else { - Debug("GPS %d: Not enough TXSPACE", state.instance + 1); - } - } -} - -void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) const -{ - const uint8_t instance = state.instance; - const struct AP_GPS::detect_state dstate = gps.detect_state[instance]; - - if (dstate.auto_detected_baud) { - hal.util->snprintf(buffer, buflen, - "GPS %d: detected as %s at %d baud", - instance + 1, - name(), - gps._baudrates[dstate.current_baud]); - } else { - hal.util->snprintf(buffer, buflen, - "GPS %d: specified as %s", - instance + 1, - name()); - } -} - - -void AP_GPS_Backend::broadcast_gps_type() const -{ -#ifndef HAL_NO_GCS - char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; - _detection_message(buffer, sizeof(buffer)); - gcs().send_text(MAV_SEVERITY_INFO, "%s", buffer); -#endif -} - -void AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages() const -{ -#ifndef HAL_NO_LOGGING - char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; - _detection_message(buffer, sizeof(buffer)); - AP::logger().Write_Message(buffer); -#endif -} - -bool AP_GPS_Backend::should_log() const -{ - return gps.should_log(); -} - - -void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan) -{ -#ifndef HAL_NO_GCS - const uint8_t instance = state.instance; - // send status - switch (instance) { - case 0: - mavlink_msg_gps_rtk_send(chan, - 0, // Not implemented yet - 0, // Not implemented yet - state.rtk_week_number, - state.rtk_time_week_ms, - 0, // Not implemented yet - 0, // Not implemented yet - state.rtk_num_sats, - state.rtk_baseline_coords_type, - state.rtk_baseline_x_mm, - state.rtk_baseline_y_mm, - state.rtk_baseline_z_mm, - state.rtk_accuracy, - state.rtk_iar_num_hypotheses); - break; - case 1: - mavlink_msg_gps2_rtk_send(chan, - 0, // Not implemented yet - 0, // Not implemented yet - state.rtk_week_number, - state.rtk_time_week_ms, - 0, // Not implemented yet - 0, // Not implemented yet - state.rtk_num_sats, - state.rtk_baseline_coords_type, - state.rtk_baseline_x_mm, - state.rtk_baseline_y_mm, - state.rtk_baseline_z_mm, - state.rtk_accuracy, - state.rtk_iar_num_hypotheses); - break; - } -#endif -} - - -/* - set a timestamp based on arrival time on uart at current byte, - assuming the message started nbytes ago -*/ -void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes) -{ - if (port) { - state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U; - } -} - - -void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length) -{ - if (itow != _last_itow) { - _last_itow = itow; - - /* - we need to calculate a pseudo-itow, which copes with the - iTow from the GPS changing in unexpected ways. We assume - that timestamps from the GPS are always in multiples of - 50ms. That means we can't handle a GPS with an update rate - of more than 20Hz. We could do more, but we'd need the GPS - poll time to be higher - */ - const uint32_t gps_min_period_ms = 50; - - // get the time the packet arrived on the UART - uint64_t uart_us = port->receive_time_constraint_us(msg_length); - - uint32_t now = AP_HAL::millis(); - uint32_t dt_ms = now - _last_ms; - _last_ms = now; - - // round to nearest 50ms period - dt_ms = ((dt_ms + (gps_min_period_ms/2)) / gps_min_period_ms) * gps_min_period_ms; - - // work out an actual message rate. If we get 5 messages in a - // row with a new rate we switch rate - if (_last_rate_ms == dt_ms) { - if (_rate_counter < 5) { - _rate_counter++; - } else if (_rate_ms != dt_ms) { - _rate_ms = dt_ms; - } - } else { - _rate_counter = 0; - _last_rate_ms = dt_ms; - } - if (_rate_ms == 0) { - // only allow 5Hz to 20Hz in user config - _rate_ms = constrain_int16(gps.get_rate_ms(state.instance), 50, 200); - } - - // round to calculated message rate - dt_ms = ((dt_ms + (_rate_ms/2)) / _rate_ms) * _rate_ms; - - // calculate pseudo-itow - _pseudo_itow += dt_ms * 1000U; - - // use msg arrival time, and correct for jitter - uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us); - state.uart_timestamp_ms = local_us / 1000U; - } -} +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_GPS.h" +#include "GPS_Backend.h" +#include + +#define GPS_BACKEND_DEBUGGING 0 + +#if GPS_BACKEND_DEBUGGING + # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) +#else + # define Debug(fmt, args ...) +#endif + +#include + +extern const AP_HAL::HAL& hal; + +AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : + port(_port), + gps(_gps), + state(_state) +{ + state.have_speed_accuracy = false; + state.have_horizontal_accuracy = false; + state.have_vertical_accuracy = false; +} + +int32_t AP_GPS_Backend::swap_int32(int32_t v) const +{ + const uint8_t *b = (const uint8_t *)&v; + union { + int32_t v; + uint8_t b[4]; + } u; + + u.b[0] = b[3]; + u.b[1] = b[2]; + u.b[2] = b[1]; + u.b[3] = b[0]; + + return u.v; +} + +int16_t AP_GPS_Backend::swap_int16(int16_t v) const +{ + const uint8_t *b = (const uint8_t *)&v; + union { + int16_t v; + uint8_t b[2]; + } u; + + u.b[0] = b[1]; + u.b[1] = b[0]; + + return u.v; +} + + +/** + fill in time_week_ms and time_week from BCD date and time components + assumes MTK19 millisecond form of bcd_time + */ +void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) +{ + uint8_t year, mon, day, hour, min, sec; + uint16_t msec; + + year = bcd_date % 100; + mon = (bcd_date / 100) % 100; + day = bcd_date / 10000; + + uint32_t v = bcd_milliseconds; + msec = v % 1000; v /= 1000; + sec = v % 100; v /= 100; + min = v % 100; v /= 100; + hour = v % 100; + + int8_t rmon = mon - 2; + if (0 >= rmon) { + rmon += 12; + year -= 1; + } + state.zr_date_timestamp =(year+2000)*10000+mon*100+day; //FIXME only calcute onecs + + // get time in seconds since unix epoch + uint32_t ret = (year/4) - (GPS_LEAPSECONDS_MILLIS / 1000UL) + 367*rmon/12 + day; + ret += year*365 + 10501; + ret = ret*24 + hour; + ret = ret*60 + min; + ret = ret*60 + sec; + + // convert to time since GPS epoch + ret -= 272764785UL; + + // get GPS week and time + state.time_week = ret / AP_SEC_PER_WEEK; + state.time_week_ms = (ret % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC; + state.time_week_ms += msec; +} + +/* + fill in 3D velocity for a GPS that doesn't give vertical velocity numbers + */ +void AP_GPS_Backend::fill_3d_velocity(void) +{ + float gps_heading = radians(state.ground_course); + + state.velocity.x = state.ground_speed * cosf(gps_heading); + state.velocity.y = state.ground_speed * sinf(gps_heading); + state.velocity.z = 0; + state.have_vertical_velocity = false; +} + +void +AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len) +{ + // not all backends have valid ports + if (port != nullptr) { + if (port->txspace() > len) { + port->write(data, len); + } else { + Debug("GPS %d: Not enough TXSPACE", state.instance + 1); + } + } +} + +void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) const +{ + const uint8_t instance = state.instance; + const struct AP_GPS::detect_state dstate = gps.detect_state[instance]; + + if (dstate.auto_detected_baud) { + hal.util->snprintf(buffer, buflen, + "GPS %d: detected as %s at %d baud", + instance + 1, + name(), + gps._baudrates[dstate.current_baud]); + } else { + hal.util->snprintf(buffer, buflen, + "GPS %d: specified as %s", + instance + 1, + name()); + } +} + + +void AP_GPS_Backend::broadcast_gps_type() const +{ +#ifndef HAL_NO_GCS + char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + _detection_message(buffer, sizeof(buffer)); + gcs().send_text(MAV_SEVERITY_INFO, "%s", buffer); +#endif +} + +void AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages() const +{ +#ifndef HAL_NO_LOGGING + char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + _detection_message(buffer, sizeof(buffer)); + AP::logger().Write_Message(buffer); +#endif +} + +bool AP_GPS_Backend::should_log() const +{ + return gps.should_log(); +} + + +void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan) +{ +#ifndef HAL_NO_GCS + const uint8_t instance = state.instance; + // send status + switch (instance) { + case 0: + mavlink_msg_gps_rtk_send(chan, + 0, // Not implemented yet + 0, // Not implemented yet + state.rtk_week_number, + state.rtk_time_week_ms, + 0, // Not implemented yet + 0, // Not implemented yet + state.rtk_num_sats, + state.rtk_baseline_coords_type, + state.rtk_baseline_x_mm, + state.rtk_baseline_y_mm, + state.rtk_baseline_z_mm, + state.rtk_accuracy, + state.rtk_iar_num_hypotheses); + break; + case 1: + mavlink_msg_gps2_rtk_send(chan, + 0, // Not implemented yet + 0, // Not implemented yet + state.rtk_week_number, + state.rtk_time_week_ms, + 0, // Not implemented yet + 0, // Not implemented yet + state.rtk_num_sats, + state.rtk_baseline_coords_type, + state.rtk_baseline_x_mm, + state.rtk_baseline_y_mm, + state.rtk_baseline_z_mm, + state.rtk_accuracy, + state.rtk_iar_num_hypotheses); + break; + } +#endif +} + + +/* + set a timestamp based on arrival time on uart at current byte, + assuming the message started nbytes ago +*/ +void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes) +{ + if (port) { + state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U; + } +} + + +void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length) +{ + if (itow != _last_itow) { + _last_itow = itow; + + /* + we need to calculate a pseudo-itow, which copes with the + iTow from the GPS changing in unexpected ways. We assume + that timestamps from the GPS are always in multiples of + 50ms. That means we can't handle a GPS with an update rate + of more than 20Hz. We could do more, but we'd need the GPS + poll time to be higher + */ + const uint32_t gps_min_period_ms = 50; + + // get the time the packet arrived on the UART + uint64_t uart_us = port->receive_time_constraint_us(msg_length); + + uint32_t now = AP_HAL::millis(); + uint32_t dt_ms = now - _last_ms; + _last_ms = now; + + // round to nearest 50ms period + dt_ms = ((dt_ms + (gps_min_period_ms/2)) / gps_min_period_ms) * gps_min_period_ms; + + // work out an actual message rate. If we get 5 messages in a + // row with a new rate we switch rate + if (_last_rate_ms == dt_ms) { + if (_rate_counter < 5) { + _rate_counter++; + } else if (_rate_ms != dt_ms) { + _rate_ms = dt_ms; + } + } else { + _rate_counter = 0; + _last_rate_ms = dt_ms; + } + if (_rate_ms == 0) { + // only allow 5Hz to 20Hz in user config + _rate_ms = constrain_int16(gps.get_rate_ms(state.instance), 50, 200); + } + + // round to calculated message rate + dt_ms = ((dt_ms + (_rate_ms/2)) / _rate_ms) * _rate_ms; + + // calculate pseudo-itow + _pseudo_itow += dt_ms * 1000U; + + // use msg arrival time, and correct for jitter + uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us); + state.uart_timestamp_ms = local_us / 1000U; + } +} diff --git a/modules/mavlink b/modules/mavlink index d2cc7dbff6..ff8ada062c 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit d2cc7dbff67f8c318a40e1ef57a99488b4737fab +Subproject commit ff8ada062c96f04dc830e54b657aca4430408433