// crash_check - disarms motors if a crash or block has been detected // crashes are detected by the vehicle being static (no speed) for more than CRASH_CHECK_TRIGGER_SEC and motor are running // called at 10Hz void Rover::crash_check() { static uint16_t crash_counter; // number of iterations vehicle may have been crashed bool crashed = false; //stores crash state // return immediately if disarmed, crash checking is disabled or vehicle is Hold, Manual or Acro mode(if it is not a balance bot) if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || ((!control_mode->is_autopilot_mode()) && (!is_balancebot()))) { crash_counter = 0; return; } // Crashed if pitch/roll > crash_angle if ((g2.crash_angle != 0) && ((fabsf(ahrs.pitch) > radians(g2.crash_angle)) || (fabsf(ahrs.roll) > radians(g2.crash_angle)))) { crashed = true; } // TODO : Check if min vel can be calculated // min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise; if (!is_balancebot()) { if (!crashed && ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN) || // Check velocity (fabsf(ahrs.get_gyro().z) >= CRASH_CHECK_VEL_MIN) || // Check turn speed (fabsf(g2.motors.get_throttle()) < CRASH_CHECK_THROTTLE_MIN))) { crash_counter = 0; return; } // we may be crashing crash_counter++; // check if crashing for 2 seconds if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) { crashed = true; } } if (crashed) { AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH); if (is_balancebot()) { // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Disarming"); disarm_motors(); } else { // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD"); // change mode to hold and disarm set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE); if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) { disarm_motors(); } } } }
void Rover::load_parameters(void) { if (!AP_Param::check_var_info()) { hal.console->printf("Bad var table\n"); AP_HAL::panic("Bad var table"); } if (!g.format_version.load() || g.format_version != Parameters::k_format_version) { // erase all parameters hal.console->printf("Firmware change: erasing EEPROM...\n"); AP_Param::erase_all(); // save the current format version g.format_version.set_and_save(Parameters::k_format_version); hal.console->printf("done.\n"); } const 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)); AP_Param::set_frame_type_flags(AP_PARAM_FRAME_ROVER); SRV_Channels::set_default_function(CH_1, SRV_Channel::k_steering); SRV_Channels::set_default_function(CH_3, SRV_Channel::k_throttle); if (is_balancebot()) { g2.crash_angle.set_default(30); } const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old, Parameters::k_param_rc_2_old, Parameters::k_param_rc_3_old, Parameters::k_param_rc_4_old, Parameters::k_param_rc_5_old, Parameters::k_param_rc_6_old, Parameters::k_param_rc_7_old, Parameters::k_param_rc_8_old, Parameters::k_param_rc_9_old, Parameters::k_param_rc_10_old, Parameters::k_param_rc_11_old, Parameters::k_param_rc_12_old, Parameters::k_param_rc_13_old, Parameters::k_param_rc_14_old }; const uint16_t old_aux_chan_mask = 0x3FFA; SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, &rcmap); hal.console->printf("load_all took %uus\n", unsigned(micros() - before)); // set a more reasonable default NAVL1_PERIOD for rovers L1_controller.set_default_period(NAVL1_PERIOD); // configure safety switch to allow stopping the motors while armed #if AP_FEATURE_SAFETY_BUTTON AP_Param::set_default_by_name("BRD_SAFETYOPTION", AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF| AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON| AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED); #endif }
// Write an attitude packet void Rover::Log_Write_Attitude() { float desired_pitch_cd = degrees(g2.attitude_control.get_desired_pitch()) * 100.0f; const Vector3f targets(0.0f, desired_pitch_cd, 0.0f); DataFlash.Log_Write_Attitude(ahrs, targets); #if AP_AHRS_NAVEKF_AVAILABLE DataFlash.Log_Write_EKF(ahrs); DataFlash.Log_Write_AHRS2(ahrs); #endif DataFlash.Log_Write_POS(ahrs); // log steering rate controller DataFlash.Log_Write_PID(LOG_PIDS_MSG, g2.attitude_control.get_steering_rate_pid().get_pid_info()); DataFlash.Log_Write_PID(LOG_PIDA_MSG, g2.attitude_control.get_throttle_speed_pid().get_pid_info()); // log pitch control for balance bots if (is_balancebot()) { DataFlash.Log_Write_PID(LOG_PIDP_MSG, g2.attitude_control.get_pitch_to_throttle_pid().get_pid_info()); } }
void Rover::load_parameters(void) { if (!AP_Param::check_var_info()) { hal.console->printf("Bad var table\n"); AP_HAL::panic("Bad var table"); } 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"); } const 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)); AP_Param::set_frame_type_flags(AP_PARAM_FRAME_ROVER); SRV_Channels::set_default_function(CH_1, SRV_Channel::k_steering); SRV_Channels::set_default_function(CH_3, SRV_Channel::k_throttle); if (is_balancebot()) { g2.crash_angle.set_default(30); } // sailboat defaults if (g2.motors.has_sail()) { g2.crash_angle.set_default(0); g2.loit_type.set_default(1); g2.loit_radius.set_default(5); g.waypoint_overshoot.set_default(10); } const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old, Parameters::k_param_rc_2_old, Parameters::k_param_rc_3_old, Parameters::k_param_rc_4_old, Parameters::k_param_rc_5_old, Parameters::k_param_rc_6_old, Parameters::k_param_rc_7_old, Parameters::k_param_rc_8_old, Parameters::k_param_rc_9_old, Parameters::k_param_rc_10_old, Parameters::k_param_rc_11_old, Parameters::k_param_rc_12_old, Parameters::k_param_rc_13_old, Parameters::k_param_rc_14_old }; const uint16_t old_aux_chan_mask = 0x3FFA; SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, &rcmap); hal.console->printf("load_all took %uus\n", unsigned(micros() - before)); // set a more reasonable default NAVL1_PERIOD for rovers L1_controller.set_default_period(NAVL1_PERIOD); // convert CH7_OPTION to RC7_OPTION for Rover-3.4 to 3.5 upgrade const AP_Param::ConversionInfo ch7_option_info = { Parameters::k_param_ch7_option, 0, AP_PARAM_INT8, "RC7_OPTION" }; AP_Int8 ch7_opt_old; if (AP_Param::find_old_parameter(&ch7_option_info, &ch7_opt_old)) { const uint8_t ch7_opt_map[] = {0,7,50,41,51,52,53,54,16,4,42,55,56}; const uint8_t ch7_opt_old_val = (uint8_t)ch7_opt_old.get(); if (ch7_opt_old_val < ARRAY_SIZE(ch7_opt_map)) { AP_Param::set_default_by_name(ch7_option_info.new_name, ch7_opt_map[ch7_opt_old_val]); } } // configure safety switch to allow stopping the motors while armed #if HAL_HAVE_SAFETY_SWITCH AP_Param::set_default_by_name("BRD_SAFETYOPTION", AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF| AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON| AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED); #endif }