Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Blimp: Remove the comment out of the process

This commit is contained in:
murata 2021-05-15 08:04:25 +09:00 committed by Randy Mackay
parent 74045ba50a
commit 06ef0a0993
6 changed files with 0 additions and 261 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
Blimp/Blimp.cpp
View File

@ -90,9 +90,6 @@ void Blimp::fast_loop()
// update INS immediately to get current gyro data populated // update INS immediately to get current gyro data populated
ins.update(); ins.update();
// run low level rate controllers that only require IMU data
// attitude_control->rate_controller_run();
// send outputs to the motors library immediately // send outputs to the motors library immediately
motors_output(); motors_output();
@ -113,10 +110,6 @@ void Blimp::fast_loop()
// update home from EKF if necessary // update home from EKF if necessary
update_home_from_EKF(); 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 // log sensor health
if (should_log(MASK_LOG_ANY)) { if (should_log(MASK_LOG_ANY)) {
Log_Sensor_Health(); Log_Sensor_Health();
@ -129,64 +122,9 @@ void Blimp::fast_loop()
//copied in from Copter's Attitude.cpp //copied in from Copter's Attitude.cpp
float Blimp::get_non_takeoff_throttle() float Blimp::get_non_takeoff_throttle()
{ {
// return MAX(0,motors->get_throttle_hover()/2.0f);
return 0.0f; //MIR no idle throttle. 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 // rc_loops - reads user input from transmitter/receiver
// called at 100hz // called at 100hz
void Blimp::rc_loop() void Blimp::rc_loop()

1
Blimp/Parameters.cpp
View File

@ -629,7 +629,6 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
constructor for g2 object constructor for g2 object
*/ */
ParametersG2::ParametersG2(void) 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); AP_Param::setup_object_defaults(this, var_info);
} }

62
Blimp/RC_Channel.cpp
View File

@ -106,68 +106,6 @@ bool RC_Channel_Blimp::do_aux_function(const aux_func_t ch_option, const AuxSwit
} }
break; 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: case AUX_FUNC::MANUAL:
do_aux_function_change_mode(Mode::Number::MANUAL, ch_flag); do_aux_function_change_mode(Mode::Number::MANUAL, ch_flag);
break; break;

4
Blimp/ekf_check.cpp
View File

@ -181,7 +181,6 @@ void Blimp::check_ekf_reset()
float yaw_angle_change_rad; float yaw_angle_change_rad;
uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad); uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
if (new_ekfYawReset_ms != ekfYawReset_ms) { if (new_ekfYawReset_ms != ekfYawReset_ms) {
// attitude_control->inertial_frame_reset();
ekfYawReset_ms = new_ekfYawReset_ms; ekfYawReset_ms = new_ekfYawReset_ms;
AP::logger().Write_Event(LogEvent::EKF_YAW_RESET); 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) #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 // 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)) { 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(); ekf_primary_core = ahrs.get_primary_core_index();
AP::logger().Write_Error(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core)); 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); 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) { if (now - vibration_check.clear_ms > 15000) {
// restore ekf gains, reset timers and update user // restore ekf gains, reset timers and update user
vibration_check.high_vibes = false; vibration_check.high_vibes = false;
// pos_control->set_vibe_comp(false);
vibration_check.clear_ms = 0; vibration_check.clear_ms = 0;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED); AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF");
@ -257,7 +254,6 @@ void Blimp::check_vibration()
if (!vibration_check.high_vibes) { if (!vibration_check.high_vibes) {
// switch ekf to use resistant gains // switch ekf to use resistant gains
vibration_check.high_vibes = true; vibration_check.high_vibes = true;
// pos_control->set_vibe_comp(true);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED); AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON");
} }

79
Blimp/mode.cpp
View File

@ -11,12 +11,8 @@
Mode::Mode(void) : Mode::Mode(void) :
g(blimp.g), g(blimp.g),
g2(blimp.g2), g2(blimp.g2),
// wp_nav(blimp.wp_nav),
// loiter_nav(blimp.loiter_nav),
// pos_control(blimp.pos_control),
inertial_nav(blimp.inertial_nav), inertial_nav(blimp.inertial_nav),
ahrs(blimp.ahrs), ahrs(blimp.ahrs),
// attitude_control(blimp.attitude_control),
motors(blimp.motors), motors(blimp.motors),
channel_right(blimp.channel_right), channel_right(blimp.channel_right),
channel_front(blimp.channel_front), channel_front(blimp.channel_front),
@ -152,15 +148,6 @@ void Blimp::update_flight_mode()
void Blimp::exit_mode(Mode *&old_flightmode, void Blimp::exit_mode(Mode *&old_flightmode,
Mode *&new_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 // 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 // fetch roll and pitch inputs
right_out = channel_right->get_control_in(); right_out = channel_right->get_control_in();
front_out = channel_front->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 bool Mode::is_disarmed_or_landed() const
{ {
if (!motors->armed() || !blimp.ap.auto_armed || blimp.ap.land_complete) { 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 get a height above ground estimate for landing
*/ */

53
Blimp/system.cpp
View File

@ -72,9 +72,6 @@ void Blimp::init_ardupilot()
AP::compass().set_log_bit(MASK_LOG_COMPASS); AP::compass().set_log_bit(MASK_LOG_COMPASS);
AP::compass().init(); AP::compass().init();
// attitude_control->parameter_sanity_check();
// pos_control->set_dt(scheduler.get_loop_period_s());
#if HIL_MODE != HIL_MODE_DISABLED #if HIL_MODE != HIL_MODE_DISABLED
while (barometer.get_last_update() == 0) { while (barometer.get_last_update() == 0) {
// the barometer begins updating when we get the first // 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.set_log_baro_bit(MASK_LOG_IMU);
barometer.calibrate(); barometer.calibrate();
// #if MODE_AUTO_ENABLED == ENABLED
// // initialise mission library
// mode_auto.mission.init();
// #endif
// initialise AP_Logger library // initialise AP_Logger library
logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&blimp, &Blimp::Log_Write_Vehicle_Startup_Messages, void)); 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(); g2.scripting.init();
#endif // ENABLE_SCRIPTING #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 // 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 // mid-flight, so set the serial ports non-blocking once we are
// ready to fly // ready to fly
serial_manager.set_blocking_writes_all(false); serial_manager.set_blocking_writes_all(false);
// enable CPU failsafe
// failsafe_enable();
ins.set_log_raw_bit(MASK_LOG_IMU_RAW); ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
// setup fin output // setup fin output
@ -200,16 +185,6 @@ bool Blimp::ekf_has_relative_position() const
// return immediately if neither optflow nor visual odometry is enabled // return immediately if neither optflow nor visual odometry is enabled
bool enabled = false; 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) { if (!enabled) {
return false; return false;
} }
@ -303,34 +278,6 @@ void Blimp::allocate_motors(void)
} }
AP_Param::load_object_from_eeprom(motors, Fins::var_info); 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 // reload lines from the defaults file that may now be accessible
AP_Param::reload_defaults_file(true); AP_Param::reload_defaults_file(true);