diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index c9912cb0f7..34d6403150 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -90,9 +90,6 @@ void Blimp::fast_loop() // update INS immediately to get current gyro data populated ins.update(); - // run low level rate controllers that only require IMU data - // attitude_control->rate_controller_run(); - // send outputs to the motors library immediately motors_output(); @@ -113,10 +110,6 @@ void Blimp::fast_loop() // update home from EKF if necessary update_home_from_EKF(); - // check if we've landed or crashed - // Skip for now since Blimp won't land - // update_land_and_crash_detectors(); - // log sensor health if (should_log(MASK_LOG_ANY)) { Log_Sensor_Health(); @@ -129,64 +122,9 @@ void Blimp::fast_loop() //copied in from Copter's Attitude.cpp float Blimp::get_non_takeoff_throttle() { - // return MAX(0,motors->get_throttle_hover()/2.0f); return 0.0f; //MIR no idle throttle. } -// start takeoff to given altitude (for use by scripting) -// bool Blimp::start_takeoff(float alt) -// { -// // exit if vehicle is not in Guided mode or Auto-Guided mode -// if (!flightmode->in_guided_mode()) { -// return false; -// } - -// if (mode_guided.do_user_takeoff_start(alt * 100.0f)) { -// blimp.set_auto_armed(true); -// return true; -// } -// return false; -// } - -// set target location (for use by scripting) -// bool Blimp::set_target_location(const Location& target_loc) -// { -// // exit if vehicle is not in Guided mode or Auto-Guided mode -// if (!flightmode->in_guided_mode()) { -// return false; -// } - -// return mode_guided.set_destination(target_loc); -// } - -// bool Blimp::set_target_velocity_NED(const Vector3f& vel_ned) -// { -// // exit if vehicle is not in Guided mode or Auto-Guided mode -// if (!flightmode->in_guided_mode()) { -// return false; -// } - -// // convert vector to neu in cm -// const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f); -// mode_guided.set_velocity(vel_neu_cms); -// return true; -// } - -// bool Blimp::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) -// { -// // exit if vehicle is not in Guided mode or Auto-Guided mode -// if (!flightmode->in_guided_mode()) { -// return false; -// } - -// Quaternion q; -// q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg)); - -// mode_guided.set_angle(q, climb_rate_ms*100, use_yaw_rate, radians(yaw_rate_degs), false); -// return true; -// } - - // rc_loops - reads user input from transmitter/receiver // called at 100hz void Blimp::rc_loop() diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index f608777a37..9788980f41 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -629,7 +629,6 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { constructor for g2 object */ ParametersG2::ParametersG2(void) -// : temp_calibration() // this doesn't actually need constructing, but removing it here is problematic syntax-wise { AP_Param::setup_object_defaults(this, var_info); } diff --git a/Blimp/RC_Channel.cpp b/Blimp/RC_Channel.cpp index f375ae8625..49102c85e0 100644 --- a/Blimp/RC_Channel.cpp +++ b/Blimp/RC_Channel.cpp @@ -106,68 +106,6 @@ bool RC_Channel_Blimp::do_aux_function(const aux_func_t ch_option, const AuxSwit } break; - // case AUX_FUNC::SAVE_WP: - // #if MODE_AUTO_ENABLED == ENABLED - // // save waypoint when switch is brought high - // if (ch_flag == RC_Channel::AuxSwitchPos::HIGH) { - - // // do not allow saving new waypoints while we're in auto or disarmed - // if (blimp.control_mode == Mode::Number::AUTO || !blimp.motors->armed()) { - // return; - // } - - // // do not allow saving the first waypoint with zero throttle - // if ((blimp.mode_auto.mission.num_commands() == 0) && (blimp.channel_down->get_control_in() == 0)) { - // return; - // } - - // // create new mission command - // AP_Mission::Mission_Command cmd = {}; - - // // if the mission is empty save a takeoff command - // if (blimp.mode_auto.mission.num_commands() == 0) { - // // set our location ID to 16, MAV_CMD_NAV_WAYPOINT - // cmd.id = MAV_CMD_NAV_TAKEOFF; - // cmd.content.location.alt = MAX(blimp.current_loc.alt,100); - - // // use the current altitude for the target alt for takeoff. - // // only altitude will matter to the AP mission script for takeoff. - // if (blimp.mode_auto.mission.add_cmd(cmd)) { - // // log event - // AP::logger().Write_Event(LogEvent::SAVEWP_ADD_WP); - // } - // } - - // // set new waypoint to current location - // cmd.content.location = blimp.current_loc; - - // // if throttle is above zero, create waypoint command - // if (blimp.channel_down->get_control_in() > 0) { - // cmd.id = MAV_CMD_NAV_WAYPOINT; - // } else { - // // with zero throttle, create LAND command - // cmd.id = MAV_CMD_NAV_LAND; - // } - - // // save command - // if (blimp.mode_auto.mission.add_cmd(cmd)) { - // // log event - // AP::logger().Write_Event(LogEvent::SAVEWP_ADD_WP); - // } - // } - // #endif - // break; - - // case AUX_FUNC::AUTO: - // #if MODE_AUTO_ENABLED == ENABLED - // do_aux_function_change_mode(Mode::Number::AUTO, ch_flag); - // #endif - // break; - - // case AUX_FUNC::LOITER: - // do_aux_function_change_mode(Mode::Number::LOITER, ch_flag); - // break; - case AUX_FUNC::MANUAL: do_aux_function_change_mode(Mode::Number::MANUAL, ch_flag); break; diff --git a/Blimp/ekf_check.cpp b/Blimp/ekf_check.cpp index 7811aa79bb..7dfb7c411a 100644 --- a/Blimp/ekf_check.cpp +++ b/Blimp/ekf_check.cpp @@ -181,7 +181,6 @@ void Blimp::check_ekf_reset() float yaw_angle_change_rad; uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad); if (new_ekfYawReset_ms != ekfYawReset_ms) { - // attitude_control->inertial_frame_reset(); ekfYawReset_ms = new_ekfYawReset_ms; AP::logger().Write_Event(LogEvent::EKF_YAW_RESET); } @@ -189,7 +188,6 @@ void Blimp::check_ekf_reset() #if AP_AHRS_NAVEKF_AVAILABLE && (HAL_NAVEKF2_AVAILABLE || HAL_NAVEKF3_AVAILABLE) // check for change in primary EKF, reset attitude target and log. AC_PosControl handles position target adjustment if ((ahrs.get_primary_core_index() != ekf_primary_core) && (ahrs.get_primary_core_index() != -1)) { - // attitude_control->inertial_frame_reset(); ekf_primary_core = ahrs.get_primary_core_index(); AP::logger().Write_Error(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core)); gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core); @@ -235,7 +233,6 @@ void Blimp::check_vibration() if (now - vibration_check.clear_ms > 15000) { // restore ekf gains, reset timers and update user vibration_check.high_vibes = false; - // pos_control->set_vibe_comp(false); vibration_check.clear_ms = 0; AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED); gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF"); @@ -257,7 +254,6 @@ void Blimp::check_vibration() if (!vibration_check.high_vibes) { // switch ekf to use resistant gains vibration_check.high_vibes = true; - // pos_control->set_vibe_comp(true); AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED); gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON"); } diff --git a/Blimp/mode.cpp b/Blimp/mode.cpp index c48f7d4cec..6fd04f6400 100644 --- a/Blimp/mode.cpp +++ b/Blimp/mode.cpp @@ -11,12 +11,8 @@ Mode::Mode(void) : g(blimp.g), g2(blimp.g2), - // wp_nav(blimp.wp_nav), - // loiter_nav(blimp.loiter_nav), - // pos_control(blimp.pos_control), inertial_nav(blimp.inertial_nav), ahrs(blimp.ahrs), - // attitude_control(blimp.attitude_control), motors(blimp.motors), channel_right(blimp.channel_right), channel_front(blimp.channel_front), @@ -152,15 +148,6 @@ void Blimp::update_flight_mode() void Blimp::exit_mode(Mode *&old_flightmode, Mode *&new_flightmode) { - - // smooth throttle transition when switching from manual to automatic flight modes - if (old_flightmode->has_manual_throttle() && !new_flightmode->has_manual_throttle() && motors->armed() && !ap.land_complete) { - // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle - // set_accel_throttle_I_from_pilot_throttle(); - } - - // cancel any takeoffs in progress - // old_flightmode->takeoff_stop(); } // notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device @@ -189,41 +176,8 @@ void Mode::get_pilot_desired_accelerations(float &right_out, float &front_out) c // fetch roll and pitch inputs right_out = channel_right->get_control_in(); front_out = channel_front->get_control_in(); - - - // // do circular limit - // float total_in = norm(pitch_out, roll_out); - // if (total_in > angle_limit) { - // float ratio = angle_limit / total_in; - // roll_out *= ratio; - // pitch_out *= ratio; - // } - - // do lateral tilt to euler roll conversion - // roll_out = (18000/M_PI) * atanf(cosf(pitch_out*(M_PI/18000))*tanf(roll_out*(M_PI/18000))); - - // roll_out and pitch_out are returned } -// bool Mode::_TakeOff::triggered(const float target_climb_rate) const -// { -// if (!blimp.ap.land_complete) { -// // can't take off if we're already flying -// return false; -// } -// if (target_climb_rate <= 0.0f) { -// // can't takeoff unless we want to go up... -// return false; -// } - -// if (blimp.motors->get_spool_state() != Fins::SpoolState::THROTTLE_UNLIMITED) { -// // hold aircraft on the ground until rotor speed runup has finished -// return false; -// } - -// return true; -// } - bool Mode::is_disarmed_or_landed() const { if (!motors->armed() || !blimp.ap.auto_armed || blimp.ap.land_complete) { @@ -241,39 +195,6 @@ void Mode::zero_throttle_and_relax_ac(bool spool_up) } } -// void Mode::zero_throttle_and_hold_attitude() -// { -// // run attitude controller -// attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); -// attitude_control->set_throttle_out(0.0f, false, blimp.g.throttle_filt); -// } - -// void Mode::make_safe_spool_down() -// { -// // command aircraft to initiate the shutdown process -// motors->set_desired_spool_state(Fins::DesiredSpoolState::GROUND_IDLE); -// switch (motors->get_spool_state()) { - -// case Fins::SpoolState::SHUT_DOWN: -// case Fins::SpoolState::GROUND_IDLE: -// // relax controllers during idle states -// // attitude_control->reset_rate_controller_I_terms_smoothly(); -// // attitude_control->set_yaw_target_to_current_heading(); -// break; - -// case Fins::SpoolState::SPOOLING_UP: -// case Fins::SpoolState::THROTTLE_UNLIMITED: -// case Fins::SpoolState::SPOOLING_DOWN: -// // while transitioning though active states continue to operate normally -// break; -// } - -// // pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero -// // pos_control->update_z_controller(); -// // we may need to move this out -// // attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); -// } - /* get a height above ground estimate for landing */ diff --git a/Blimp/system.cpp b/Blimp/system.cpp index 79ecb3a1b6..d2fd7e2892 100644 --- a/Blimp/system.cpp +++ b/Blimp/system.cpp @@ -72,9 +72,6 @@ void Blimp::init_ardupilot() AP::compass().set_log_bit(MASK_LOG_COMPASS); AP::compass().init(); - // attitude_control->parameter_sanity_check(); - // pos_control->set_dt(scheduler.get_loop_period_s()); - #if HIL_MODE != HIL_MODE_DISABLED while (barometer.get_last_update() == 0) { // the barometer begins updating when we get the first @@ -92,11 +89,6 @@ void Blimp::init_ardupilot() barometer.set_log_baro_bit(MASK_LOG_IMU); barometer.calibrate(); - // #if MODE_AUTO_ENABLED == ENABLED - // // initialise mission library - // mode_auto.mission.init(); - // #endif - // initialise AP_Logger library logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&blimp, &Blimp::Log_Write_Vehicle_Startup_Messages, void)); @@ -106,18 +98,11 @@ void Blimp::init_ardupilot() 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); // setup fin output @@ -200,16 +185,6 @@ bool Blimp::ekf_has_relative_position() const // return immediately if neither optflow nor visual odometry is enabled bool enabled = false; - // #if OPTFLOW == ENABLED - // if (optflow.enabled()) { - // enabled = true; - // } - // #endif - // #if HAL_VISUALODOM_ENABLED - // if (visual_odom.enabled()) { - // enabled = true; - // } - // #endif if (!enabled) { return false; } @@ -303,34 +278,6 @@ void Blimp::allocate_motors(void) } AP_Param::load_object_from_eeprom(motors, Fins::var_info); - // const struct AP_Param::GroupInfo *ac_var_info; - - // attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s()); - // ac_var_info = AC_AttitudeControl_Multi::var_info; - // 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); - - // wp_nav = new AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control); - - // 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); - // reload lines from the defaults file that may now be accessible AP_Param::reload_defaults_file(true);