From b121cfeb859c24cbdd405e7dfbb9923f4fe5cd3d Mon Sep 17 00:00:00 2001 From: Roman Dvorak Date: Thu, 27 Jul 2023 15:16:22 +0200 Subject: [PATCH] try to sync with upstream --- Tools/simulation/flightgear/flightgear_bridge | 2 +- .../FixedwingPositionControl.cpp | 98 ++++++ .../FixedwingPositionControl.hpp | 2 + .../autogyro_takeoff/AutogyroTakeoff.cpp | 303 ++++++++++-------- .../autogyro_takeoff/AutogyroTakeoff.h | 284 ++++++++++++---- 5 files changed, 488 insertions(+), 201 deletions(-) diff --git a/Tools/simulation/flightgear/flightgear_bridge b/Tools/simulation/flightgear/flightgear_bridge index f47ce7b5fbbb..2c6b979d8cbf 160000 --- a/Tools/simulation/flightgear/flightgear_bridge +++ b/Tools/simulation/flightgear/flightgear_bridge @@ -1 +1 @@ -Subproject commit f47ce7b5fbbb3aa43d33d2be1f6cd3746b13d5bf +Subproject commit 2c6b979d8cbf72ff3d65eee1118483d0865ff184 diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index c6cd4e663af2..0d083fb7320b 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -1339,6 +1339,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { + mavlink_log_info(&_mavlink_log_pub, "RUNWAY TAKEOFF init\t"); + _runway_takeoff.init(now, _yaw, global_position); _takeoff_ground_alt = _current_altitude; @@ -1431,6 +1433,101 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _new_landing_gear_position = landing_gear_s::GEAR_UP; } + } else if (_autogyro_takeoff.TakeoffEnabled()) { + if (!_autogyro_takeoff.isInitialized()) { + mavlink_log_info(&_mavlink_log_pub, "AUTOGYRO TAKEOFF init\t"); + _autogyro_takeoff.init(now, _yaw, global_position); + + _takeoff_ground_alt = _current_altitude; + + _launch_current_yaw = _yaw; + + events::send(events::ID("fixedwing_position_control_autogyro_takeoff"), events::Log::Info, "Takeoff on runway with autogyro"); + } + +// if (_skipping_takeoff_detection) { +// _autogyro_takeoff.forceSetFlyState(); +// } + + _autogyro_takeoff.update(now, takeoff_airspeed, _airspeed, _current_altitude - _takeoff_ground_alt, + clearance_altitude_amsl - _takeoff_ground_alt, _rpm_frequency, &_mavlink_log_pub); + + // yaw control is disabled once in "taking off" state + _att_sp.fw_control_yaw_wheel = _autogyro_takeoff.controlYaw(); + + // XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface. + if (_param_rwto_nudge.get()) { + _att_sp.yaw_sp_move_rate = _manual_control_setpoint.yaw; + } + + // tune up the lateral position control guidance when on the ground + if (_autogyro_takeoff.controlYaw()) { + _npfg.setPeriod(_param_rwto_npfg_period.get()); + + } + + const Vector2f start_pos_local = _global_local_proj_ref.project(_autogyro_takeoff.getStartPosition()(0), + _autogyro_takeoff.getStartPosition()(1)); + const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); + + // by default set the takeoff bearing to the takeoff yaw, but override in a mission takeoff with bearing to takeoff WP + Vector2f takeoff_bearing_vector = {cosf(_launch_current_yaw), sinf(_launch_current_yaw)}; + + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { + // the bearing from runway start to the takeoff waypoint is followed until the clearance altitude is exceeded + takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local); + } + + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); + navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed, + _wind_vel, 0.0f); + + _att_sp.roll_body = _autogyro_takeoff.getRoll(_npfg.getRollSetpoint()); + + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + + // use npfg's bearing to commanded course, controlled via yaw angle while on runway + const float bearing = _npfg.getBearing(); + + // heading hold mode will override this bearing setpoint + _att_sp.yaw_body = _autogyro_takeoff.getYaw(bearing); + + // update tecs + const float pitch_max = _autogyro_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get())); + const float pitch_min = _autogyro_takeoff.getMinPitch(math::radians(_takeoff_pitch_min.get()), + math::radians(_param_fw_p_lim_min.get())); + + if (_autogyro_takeoff.resetIntegrators()) { + // reset integrals except yaw (which also counts for the wheel controller) + _att_sp.reset_integral = true; + + // throttle is open loop anyway during ground roll, no need to wind up the integrator + _tecs.resetIntegrals(); + } + + tecs_update_pitch_throttle(control_interval, + altitude_setpoint_amsl, + target_airspeed, + pitch_min, + pitch_max, + _param_fw_thr_min.get(), + _param_fw_thr_max.get(), + _param_sinkrate_target.get(), + _param_fw_t_clmb_max.get()); + + _tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation + + _att_sp.pitch_body = _autogyro_takeoff.getPitch(get_tecs_pitch()); + _att_sp.thrust_body[0] = _autogyro_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust()); + + _flaps_setpoint = _param_fw_flaps_to_scl.get(); + + // retract ladning gear once passed the climbout state + if (_autogyro_takeoff.getState() > AutogyroTakeoffState::TAKEOFF_CLIMBOUT) { + _new_landing_gear_position = landing_gear_s::GEAR_UP; + } + } else { /* Perform launch detection */ if (!_skipping_takeoff_detection && _param_fw_laun_detcn_on.get() && @@ -2282,6 +2379,7 @@ FixedwingPositionControl::Run() vehicle_command_poll(); vehicle_control_mode_poll(); wind_poll(); + rpm_poll(); vehicle_air_data_s air_data; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index fc28346ff3e6..a8a99e2d36f5 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -191,6 +191,8 @@ class FixedwingPositionControl final : public ModuleBase */ @@ -55,39 +55,45 @@ namespace autogyrotakeoff { -void AutogyroTakeoff::init(const hrt_abstime &now, float yaw, const matrix::Vector2d &start_pos_global) + +//void AutogyroTakeoff::init(const hrt_abstime &now, float yaw, const matrix::Vector2d &start_pos_global) +void AutogyroTakeoff::init(const hrt_abstime &time_now, const float initial_yaw, const matrix::Vector2d &start_pos_global) { - _init_yaw = yaw; - _initialized = true; - _state = AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE_START; - _initialized_time = now; - _time_in_state = now; - _last_sent_release_status = now; - _climbout = true; // this is true until climbout is finished - // _takeoff_wp(0) = current_lat; - // _takeoff_wp(1) = current_lon; - // _initial_wp(0) = current_lat; - // _initial_wp(1) = current_lon; - - - _takeoff_wp = start_pos_global; - _initial_wp = start_pos_global; + initial_yaw_ = initial_yaw; + initialized_= true; + state_= AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE_START; + state_last_ = AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE_START; + time_initialized_ = time_now; + //_time_in_state_= time_now; + last_sent_release_status_ = time_now; + climbout_ = true; // this is true until climbout is finished + // takeoff_wp_(0) = current_lat; + // takeoff_wp_(1) = current_lon; + // initial_wp_(0) = current_lat; + // initial_wp_(1) = current_lon; + + + takeoff_wp_ = start_pos_global; + initial_wp_ = start_pos_global; } -void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor_rpm, float alt_agl, - double current_lat, double current_lon, orb_advert_t *mavlink_log_pub) +void AutogyroTakeoff::update(const hrt_abstime &time_now, const float takeoff_airspeed, const float calibrated_airspeed, + const float vehicle_altitude, const float clearance_altitude, float rotor_rpm, orb_advert_t *mavlink_log_pub) { - _climbout = true; + climbout_ = true; takeoff_status_s takeoff_status = {}; actuator_armed_s actuator_armed; _actuator_armed_sub.update(&actuator_armed); - if (actuator_armed.manual_lockdown && _state <= AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP) { - _state = AutogyroTakeoffState::TAKEOFF_ERROR; - } + PX4_INFO("Takeoff: state %i, altitude: %f to %f", state_, (double)vehicle_altitude, (double)clearance_altitude); - switch (_state) { +// TODO Zachrana pri uzemeni po startu. +// if (actuator_armed.manual_lockdown && state_<= AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP) { +// state_= AutogyroTakeoffState::TAKEOFF_ERROR; +// } + + switch (state_) { /* Hangling error states of takeoff mode. Should lead in allerting operator and/or abrod takeoff process @@ -95,7 +101,7 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor IN: error state */ case AutogyroTakeoffState::TAKEOFF_ERROR: { - if (_state != _state_last) { + if (state_!= state_last_) { PX4_INFO("ERR STATE"); mavlink_log_info(mavlink_log_pub, "#Takeoff: Error state"); } @@ -110,25 +116,25 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor */ case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE_START: - if (rotor_rpm > _param_ag_prerotator_minimal_rpm.get()) { + if (rotor_rpm > param_ag_prerotator_minimal_rpm_.get()) { // Eletrical prerotator, controlled from autopilot - if (_param_ag_prerotator_type.get() == AutogyroTakeoffType::ELPREROT_PLATFORM - || _param_ag_prerotator_type.get() == AutogyroTakeoffType::ELPREROT_RUNWAY) { + if (param_ag_prerotator_type_.get() == AutogyroTakeoffType::ELPREROT_PLATFORM + || param_ag_prerotator_type_.get() == AutogyroTakeoffType::ELPREROT_RUNWAY) { if (doPrerotate()) { play_next_tone(); - _state = AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE; - _time_in_state = now; + state_= AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE; + time_in_state_ = time_now; } } else { // manually controlled prerotator or prerotation by forward movement play_next_tone(); - _state = AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE; - _time_in_state = now; + state_= AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE; + time_in_state_ = time_now; } mavlink_log_info(mavlink_log_pub, "Takeoff: minimal RPM for prerotator reached"); - //PX4_INFO("Takeoff: minimal RPM for prerotator reached"); + PX4_INFO("Takeoff: minimal RPM for prerotator reached"); } break; @@ -145,12 +151,12 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor */ case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE: // 1 - if (rotor_rpm > _param_ag_prerotator_target_rpm.get()) { - _state = AutogyroTakeoffState::PRE_TAKEOFF_DONE; - _time_in_state = now; + if (rotor_rpm > param_ag_prerotator_target_rpm_.get()) { + state_= AutogyroTakeoffState::PRE_TAKEOFF_DONE; + time_in_state_ = time_now; play_next_tone(); mavlink_log_info(mavlink_log_pub, "Takeoff, prerotator RPM reached"); - //PX4_INFO("Takeoff: prerotator RPM reached"); + PX4_INFO("Takeoff: prerotator RPM reached"); } break; @@ -165,26 +171,27 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor case AutogyroTakeoffState::PRE_TAKEOFF_DONE: { // 2 bool ready_for_release = true; - if (rotor_rpm < _param_ag_rotor_flight_rpm.get()) { + if (rotor_rpm < param_ag_rotor_flight_rpm_.get()) { ready_for_release = false; //PX4_INFO("Takeofff, waiting for flight rpm."); // Some histesis needs to be applied for the start interrupt procedure. // Currently, this does not allow the start to be interrupted. //_state = AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE; - //_time_in_state = now; + //time_in_state_ = time_now; } // check minimal airspeed - if (airspeed < (_param_fw_airspd_min.get() * _param_rwto_airspd_scl.get())) { + if (takeoff_airspeed < (param_fw_airspd_min_.get() * param_rwto_airspd_scl_.get())) { ready_for_release = false; //PX4_INFO("Takeofff, waiting for min airspeed."); } if (ready_for_release) { - _init_yaw = get_bearing_to_next_waypoint(_initial_wp(0), _initial_wp(1), current_lat, current_lon); + //initial_yaw_ = get_bearing_to_next_waypoint(initial_wp_(0), initial_wp_(1), current, current_lon); + //initial_yaw_ - _state = AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP; - _time_in_state = now; + state_= AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP; + time_in_state_ = time_now; mavlink_log_info(mavlink_log_pub, "Ready to start motor"); //PX4_INFO("Takeoff, Please release."); play_next_tone(); @@ -200,23 +207,23 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor case AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP: { bool ready_for_release = true; - if (rotor_rpm < _param_ag_rotor_flight_rpm.get()) { + if (rotor_rpm < param_ag_rotor_flight_rpm_.get()) { ready_for_release = false; //PX4_INFO("Takeofff, waiting for flight rpm."); // Some histesis needs to be applied for the start interrupt procedure. // Currently, this does not allow the start to be interrupted. //_state = AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE; - //_time_in_state = now; + //time_in_state_ = time_now; } // check minimal airspeed - if (airspeed < (_param_fw_airspd_min.get() * _param_rwto_airspd_scl.get())) { + if (takeoff_airspeed < (param_fw_airspd_min_.get() * param_rwto_airspd_scl_.get())) { ready_for_release = false; //PX4_INFO("Takeofff, waiting for min airspeed."); } // ramup time elapsed - if (hrt_elapsed_time(&_time_in_state) < (_param_rwto_ramp_time.get() * 1_s * 1.5f)) { + if (hrt_elapsed_time(&time_in_state_) < (param_rwto_ramp_time_.get() * 1_s * 1.5f)) { ready_for_release = false; } @@ -228,8 +235,8 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor if (ready_for_release) { - _state = AutogyroTakeoffState::TAKEOFF_RELEASE; - _time_in_state = now; + state_= AutogyroTakeoffState::TAKEOFF_RELEASE; + time_in_state_ = time_now; PX4_INFO("Takeoff, Please release."); play_next_tone(); } @@ -248,23 +255,24 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor */ case AutogyroTakeoffState::TAKEOFF_RELEASE: { // Waiting to get full throttle - if (hrt_elapsed_time(&_time_in_state) < (_param_rwto_ramp_time.get() * 1_s)) { + if (hrt_elapsed_time(&time_in_state_) < (param_rwto_ramp_time_.get() * 1_s)) { // Send release CMD play_release_tone(); } - if (alt_agl > _param_ag_nav_alt.get()) { + if (vehicle_altitude > param_ag_nav_alt_.get()) { mavlink_log_info(mavlink_log_pub, "Climbout"); PX4_INFO("Takeoff: Climbout."); - _state = AutogyroTakeoffState::TAKEOFF_CLIMBOUT; + state_= AutogyroTakeoffState::TAKEOFF_CLIMBOUT; play_next_tone(); - _time_in_state = now; + time_in_state_ = time_now; // set current position as center of loiter - if (_param_rwto_hdg.get() == 0) { - _takeoff_wp(0) = current_lat; - _takeoff_wp(1) = current_lon; - } + // TODO + // if (param_rwto_hdg_.get() == 0) { + // takeoff_wp_(0) = current_lat; + // takeoff_wp_(1) = current_lon; + // } } } break; @@ -276,18 +284,18 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor OUT: Mission continue */ case AutogyroTakeoffState::TAKEOFF_CLIMBOUT: - if (alt_agl > _param_fw_clmbout_diff.get()) { - _climbout = false; - _state = AutogyroTakeoffState::FLY; - _time_in_state = now; + if (vehicle_altitude > param_fw_clmbout_diff_.get()) { + climbout_ = false; + state_= AutogyroTakeoffState::FLY; + time_in_state_ = time_now; PX4_INFO("Takeoff:FLY."); } - //_climbout = false; + //climbout_ = false; break; case AutogyroTakeoffState::FLY: - _climbout = false; + climbout_ = false; break; default: @@ -296,20 +304,20 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor - takeoff_status.time_in_state = hrt_elapsed_time(&_time_in_state); - takeoff_status.takeoff_state = (int) _state; + takeoff_status.time_in_state = hrt_elapsed_time(&time_in_state_); + takeoff_status.takeoff_state = (int) state_; _takeoff_status_pub.publish(takeoff_status); - if (hrt_elapsed_time(&_last_sent_release_status) > 1_s / 4 || _state != _state_last) { - _last_sent_release_status = now; + if (hrt_elapsed_time(&last_sent_release_status_) > 1_s / 4 || state_!= state_last_) { + last_sent_release_status_ = time_now; debug_value_s takeoff_information{}; - takeoff_information.timestamp = now; + takeoff_information.timestamp = time_now; // templorary sollution, should be changed by custom mavlink message - takeoff_information.value = _state; + takeoff_information.value = state_; _takeoff_informations_pub.publish(takeoff_information); } - _state_last = _state; + state_last_ = state_; } /* @@ -329,19 +337,19 @@ bool AutogyroTakeoff::doPrerotate() return true; } -float AutogyroTakeoff::getRequestedAirspeed() -{ - switch (_state) { - case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE_START: - case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE: - case AutogyroTakeoffState::PRE_TAKEOFF_DONE: - case AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP: - return _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get(); +// float AutogyroTakeoff::getRequestedAirspeed() +// { +// switch (_state) { +// case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE_START: +// case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE: +// case AutogyroTakeoffState::PRE_TAKEOFF_DONE: +// case AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP: +// return param_fw_airspd_min_.get() * param_rwto_airspd_scl_.get(); - default: - return _param_fw_airspd_trim.get(); - } -} +// default: +// return param_fw_airspd_trim_.get(); +// } +// } /* * Returns true as long as we're below navigation altitude @@ -349,7 +357,7 @@ float AutogyroTakeoff::getRequestedAirspeed() bool AutogyroTakeoff::controlYaw() { // keep controlling yaw directly until we start navigation - return _state < AutogyroTakeoffState::TAKEOFF_CLIMBOUT; + return state_< AutogyroTakeoffState::TAKEOFF_CLIMBOUT; } /* @@ -358,9 +366,9 @@ bool AutogyroTakeoff::controlYaw() * Limited (parameter) as long as the plane is on runway. Otherwise * use the one from TECS */ -float AutogyroTakeoff::getPitch(float tecsPitch) +float AutogyroTakeoff::getPitch(float external_pitch_setpoint) { - switch (_state) { + switch (state_) { case AutogyroTakeoffState::TAKEOFF_ERROR: return 0; @@ -369,33 +377,33 @@ float AutogyroTakeoff::getPitch(float tecsPitch) case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE: // 1 maximal pitch case AutogyroTakeoffState::PRE_TAKEOFF_DONE: // 2 case AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP: - //return math::radians(_param_rwto_max_pitch.get()); - return math::radians(_param_rwto_psp.get()); + //return math::radians(param_rwto_max_pitch_.get()); + return math::radians(param_rwto_psp_.get()); // FLy default: - return tecsPitch; + return external_pitch_setpoint; } } /* * Returns the roll setpoint to use. */ -float AutogyroTakeoff::getRoll(float navigatorRoll) +float AutogyroTakeoff::getRoll(float external_roll_setpoint) { // until we have enough ground clearance, set roll to 0 - if (_state < AutogyroTakeoffState::TAKEOFF_RELEASE) { + if (state_< AutogyroTakeoffState::TAKEOFF_RELEASE) { return 0.0f; } // allow some limited roll during RELEASE and CLIMBOUT - else if (_state < AutogyroTakeoffState::FLY) { - return math::constrain(navigatorRoll, - math::radians(-_param_ag_tko_max_roll.get()), - math::radians(_param_ag_tko_max_roll.get())); + else if (state_< AutogyroTakeoffState::FLY) { + return math::constrain(external_roll_setpoint, + math::radians(-param_ag_tko_max_roll_.get()), + math::radians(param_ag_tko_max_roll_.get())); } - return navigatorRoll; + return external_roll_setpoint; } /* @@ -404,15 +412,15 @@ float AutogyroTakeoff::getRoll(float navigatorRoll) * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the * runway. When it has enough ground clearance we start navigation towards WP. */ -float AutogyroTakeoff::getYaw(float navigatorYaw) +float AutogyroTakeoff::getYaw(float external_yaw_setpoint) { - return navigatorYaw; + return external_yaw_setpoint; - if (_param_rwto_hdg.get() == 0 && _state < AutogyroTakeoffState::TAKEOFF_CLIMBOUT) { - return _init_yaw; + if (param_rwto_hdg_.get() == 0 && state_< AutogyroTakeoffState::TAKEOFF_CLIMBOUT) { + return initial_yaw_; } else { - return navigatorYaw; + return external_yaw_setpoint; } } @@ -422,14 +430,16 @@ float AutogyroTakeoff::getYaw(float navigatorYaw) * Ramps up in the beginning, until it lifts off the runway it is set to * parameter value, then it returns the TECS throttle. */ -float AutogyroTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle) +float AutogyroTakeoff::getThrottle(const float idle_throttle, const float external_throttle_setpoint) const +//float AutogyroTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle) { - float idle = (double)_param_fw_thr_idle.get(); + //float idle = (double)param_fw_thr_idle_.get(); + float idle = (double)idle_throttle; - //PX4_INFO("GET THROTTLE %f, state: %f, time: %f", (double)idle, (double)_state, (double)(now - _time_in_state)); + //PX4_INFO("GET THROTTLE %f, state: %f, time: %f", (double)idle, (double)state_, (double)(now - time_in_state_)); - switch (_state) { + switch (state_) { case AutogyroTakeoffState::TAKEOFF_ERROR: case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE_START: @@ -437,10 +447,11 @@ float AutogyroTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle) case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE: case AutogyroTakeoffState::PRE_TAKEOFF_DONE: { - if (_param_ag_prerotator_type.get() == AutogyroTakeoffType::WOPREROT_RUNWAY) { - float throttle = ((now - _time_in_state) / (_param_rwto_ramp_time.get() * 1_s)) * _param_rwto_max_thr.get(); + if (param_ag_prerotator_type_.get() == AutogyroTakeoffType::WOPREROT_RUNWAY) { + // TODO float throttle = ((time_now - time_in_state_) / (param_rwto_ramp_time_.get() * 1_s)) * param_rwto_max_thr_.get(); + float throttle = param_rwto_max_thr_.get(); //PX4_INFO("Thortle: %f",(double)throttle); - return math::min(throttle, _param_rwto_max_thr.get()); + return math::min(throttle, param_rwto_max_thr_.get()); } else { return idle; @@ -450,12 +461,13 @@ float AutogyroTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle) case AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP: { float throttle = idle; - if (_param_ag_prerotator_type.get() == AutogyroTakeoffType::WOPREROT_RUNWAY) { - throttle = _param_rwto_max_thr.get(); + if (param_ag_prerotator_type_.get() == AutogyroTakeoffType::WOPREROT_RUNWAY) { + throttle = param_rwto_max_thr_.get(); - } else if (_param_ag_prerotator_type.get() == AutogyroTakeoffType::WOPREROT_PLATFORM) { - throttle = ((now - _time_in_state) / (_param_rwto_ramp_time.get() * 1_s)) * _param_rwto_max_thr.get(); - throttle = math::min(throttle, _param_rwto_max_thr.get()); + } else if (param_ag_prerotator_type_.get() == AutogyroTakeoffType::WOPREROT_PLATFORM) { + //throttle = ((time_now - time_in_state_) / (param_rwto_ramp_time_.get() * 1_s)) * param_rwto_max_thr_.get(); + throttle = param_rwto_max_thr_.get(); + throttle = math::min(throttle, param_rwto_max_thr_.get()); } return throttle; @@ -465,33 +477,22 @@ float AutogyroTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle) case AutogyroTakeoffState::TAKEOFF_RELEASE: { float throttle = idle; - if (_param_ag_prerotator_type.get() == AutogyroTakeoffType::WOPREROT_RUNWAY) { - throttle = _param_rwto_max_thr.get(); + if (param_ag_prerotator_type_.get() == AutogyroTakeoffType::WOPREROT_RUNWAY) { + throttle = param_rwto_max_thr_.get(); - } else if (_param_ag_prerotator_type.get() == AutogyroTakeoffType::WOPREROT_PLATFORM) { - throttle = _param_rwto_max_thr.get(); + } else if (param_ag_prerotator_type_.get() == AutogyroTakeoffType::WOPREROT_PLATFORM) { + throttle = param_rwto_max_thr_.get(); } - return math::min(throttle, _param_rwto_max_thr.get()); + return math::min(throttle, param_rwto_max_thr_.get()); } // TAKEOFF_CLIMBOUT a FLY default: - return tecsThrottle; + return external_throttle_setpoint; } } -bool AutogyroTakeoff::resetIntegrators() -{ - // reset integrators if we're still on runway - return _state < AutogyroTakeoffState::TAKEOFF_RELEASE; -} - - -bool AutogyroTakeoff::resetAltTakeoff() -{ - return _state < AutogyroTakeoffState::TAKEOFF_RELEASE; -} /* * Returns the minimum pitch for TECS to use. @@ -500,14 +501,14 @@ bool AutogyroTakeoff::resetAltTakeoff() * the climbtout minimum pitch (parameter). * Otherwise use the minimum that is enforced generally (parameter). */ -float AutogyroTakeoff::getMinPitch(float climbout_min, float min) +float AutogyroTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch) const { - if (_state < AutogyroTakeoffState::FLY) { - return climbout_min; + if (state_< AutogyroTakeoffState::FLY) { + return min_pitch_in_climbout; } else { - return min; + return min_pitch; } } @@ -516,22 +517,46 @@ float AutogyroTakeoff::getMinPitch(float climbout_min, float min) * * Limited by parameter (if set) until climbout is done. */ -float AutogyroTakeoff::getMaxPitch(float max) +float AutogyroTakeoff::getMaxPitch(float max_pitch) const { // use max pitch from parameter if set (> 0.1) - if (_state < AutogyroTakeoffState::FLY && _param_rwto_max_pitch.get() > 0.1f) { - return _param_rwto_max_pitch.get(); + if (state_< AutogyroTakeoffState::FLY && param_rwto_max_pitch_.get() > 0.1f) { + return param_rwto_max_pitch_.get(); } else { - return max; + return max_pitch; } } + +bool AutogyroTakeoff::resetIntegrators() +{ + // reset integrators if we're still on runway + return state_< AutogyroTakeoffState::TAKEOFF_RELEASE; +} + + +// bool AutogyroTakeoff::resetAltTakeoff() +// { +// return state_< AutogyroTakeoffState::TAKEOFF_RELEASE; +// } + void AutogyroTakeoff::reset() { - _initialized = false; - _state = AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE_START; + initialized_= false; + state_= AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE_START; + takeoff_time_ = 0; +} + + +float AutogyroTakeoff::interpolateValuesOverAbsoluteTime(const float start_value, const float end_value, + const hrt_abstime &start_time, const float interpolation_time) const +{ + const float seconds_since_start = hrt_elapsed_time(&start_time) * 1.e-6f; + const float interpolator = math::constrain(seconds_since_start / interpolation_time, 0.0f, 1.0f); + + return interpolator * end_value + (1.0f - interpolator) * start_value; } void AutogyroTakeoff::play_next_tone() diff --git a/src/modules/fw_pos_control/autogyro_takeoff/AutogyroTakeoff.h b/src/modules/fw_pos_control/autogyro_takeoff/AutogyroTakeoff.h index c7c3babf3045..f36c41e32a6c 100644 --- a/src/modules/fw_pos_control/autogyro_takeoff/AutogyroTakeoff.h +++ b/src/modules/fw_pos_control/autogyro_takeoff/AutogyroTakeoff.h @@ -57,6 +57,8 @@ #include #include +#include "../runway_takeoff/RunwayTakeoff.h" + namespace autogyrotakeoff { @@ -72,6 +74,7 @@ enum AutogyroTakeoffState { FLY /**< fly to next waypoint */ }; + enum AutogyroTakeoffType { WOPREROT_PLATFORM = 0, // platform without prerotator WOPREROT_RUNWAY = 1, // Without prerotator on runway @@ -86,39 +89,164 @@ class __EXPORT AutogyroTakeoff : public ModuleParams AutogyroTakeoff(ModuleParams *parent) : ModuleParams(parent) {} ~AutogyroTakeoff() = default; - void init(const hrt_abstime &now, float yaw, const matrix::Vector2d &start_pos_global); - void update(const hrt_abstime &now, float airspeed, float rotor_rpm, float alt_agl, double current_lat, - double current_lon, - orb_advert_t *mavlink_log_pub); + + /** + * @brief Initializes the state machine. + * + * @param time_now Absolute time since system boot [us] + * @param initial_yaw Vehicle yaw angle at time of initialization [us] + * @param start_pos_global Vehicle global (lat, lon) position at time of initialization [deg] + */ + void init(const hrt_abstime &time_now, const float initial_yaw, const matrix::Vector2d &start_pos_global); + +// void init(const hrt_abstime &now, float yaw, const matrix::Vector2d &start_pos_global); + + /** + * @brief Updates the state machine based on the current vehicle condition. + * + * @param time_now Absolute time since system boot [us] + * @param takeoff_airspeed Calibrated airspeed setpoint for the takeoff climbout [m/s] + * @param calibrated_airspeed Vehicle calibrated airspeed [m/s] + * @param vehicle_altitude Vehicle altitude (AGL) [m] + * @param clearance_altitude Altitude (AGL) above which we have cleared all occlusions in the runway path [m] + * @param rotor_rpm Current rotor RPM [rpm] + * @param mavlink_log_pub Mavlink log uORB handle + */ + void update(const hrt_abstime &time_now, const float takeoff_airspeed, const float calibrated_airspeed, + const float vehicle_altitude, const float clearance_altitude, const float rotor_rpm, orb_advert_t *mavlink_log_pub); bool doRelease(); bool doPrerotate(); - AutogyroTakeoffState getState() { return _state; } - float getMinAirspeedScaling() { return _param_rwto_airspd_scl.get(); } - bool isInitialized() { return _initialized; } +// AutogyroTakeoffState getState() { return state_; } +// float getMinAirspeedScaling() { return _param_rwto_airspd_scl.get(); } +// bool isInitialized() { return _initialized; } + + /** + * @return Current takeoff state + */ + AutogyroTakeoffState getState() { return takeoff_state_; } + + /** + * @return The state machine is initialized + */ + bool isInitialized() { return initialized_; } + + /** + * @return Runway takeoff is enabled + */ + bool TakeoffEnabled() { return param_ag_tkoff_.get();} + - // bool autogyroTakeoffEnabled() { return _param_ag_tkoff.get(); } - bool autogyroTakeoffEnabled() { return _param_ag_tkoff.get(); } - float getRequestedAirspeed(); - float getInitYaw() { return _init_yaw; } + /** + * @return Initial vehicle yaw angle [rad] + */ + float getInitYaw() { return initial_yaw_; } + /** + * @return The vehicle should control yaw via rudder or nose gear + */ bool controlYaw(); - bool climbout() { return _climbout; } - float getPitch(float tecsPitch); - float getRoll(float navigatorRoll); - float getYaw(float navigatorYaw); - float getThrottle(const hrt_abstime &now, float tecsThrottle); + + /** + * @param external_pitch_setpoint Externally commanded pitch angle setpoint (usually from TECS) [rad] + * @return Pitch angle setpoint (limited while plane is on runway) [rad] + */ + float getPitch(float external_pitch_setpoint); + + /** + * @param external_roll_setpoint Externally commanded roll angle setpoint (usually from path navigation) [rad] + * @return Roll angle setpoint [rad] + */ + float getRoll(float external_roll_setpoint); + + /** + * @brief Returns the appropriate yaw angle setpoint. + * + * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the runway. + * When it has enough ground clearance we start navigation towards WP. + * + * @param external_yaw_setpoint Externally commanded yaw angle setpoint [rad] + * @return Yaw angle setpoint [rad] + */ + float getYaw(float external_yaw_setpoint); + + /** + * @brief Returns the throttle setpoint. + * + * Ramps up over RWTO_RAMP_TIME to RWTO_MAX_THR until the aircraft lifts off the runway and then + * ramps from RWTO_MAX_THR to the externally defined throttle setting over the takeoff rotation time + * + * @param idle_throttle normalized [0,1] + * @param external_throttle_setpoint Externally commanded throttle setpoint (usually from TECS), normalized [0,1] + * @return Throttle setpoint, normalized [0,1] + */ + float getThrottle(const float idle_throttle, const float external_throttle_setpoint) const; + + /** + * @param min_pitch_in_climbout Minimum pitch angle during climbout [rad] + * @param min_pitch Externally commanded minimum pitch angle [rad] + * @return Minimum pitch angle [rad] + */ + float getMinPitch(const float min_pitch_in_climbout, const float min_pitch) const; + + /** + * @param max_pitch Externally commanded maximum pitch angle [rad] + * @return Maximum pitch angle [rad] + */ + float getMaxPitch(const float max_pitch) const; + + /** + * @return Runway takeoff starting position in global frame (lat, lon) [deg] + */ + const matrix::Vector2d &getStartPosition() const { return start_pos_global_; }; + + // NOTE: this is only to be used for mistaken mode transitions to takeoff while already in air + void forceSetFlyState() { takeoff_state_ = AutogyroTakeoffState::FLY; } + + /** + * @return If the attitude / rate control integrators should be continually reset. + * This is the case during ground roll. + */ bool resetIntegrators(); - bool resetAltTakeoff(); - float getMinPitch(float climbout_min, float min); - float getMaxPitch(float max); - // bool setState(int new_state); - const matrix::Vector2d &getStartWP() const { return _takeoff_wp; }; + /** + * @brief Reset the state machine. + */ void reset(); - uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + /** + * @brief Linearly interpolates between a start and end value over an absolute time span. + * + * @param start_value + * @param end_value + * @param start_time Absolute start time [us] + * @param interpolation_time The time span to interpolate over [s] + * @return interpolated value + */ + float interpolateValuesOverAbsoluteTime(const float start_value, const float end_value, const hrt_abstime &start_time, + const float interpolation_time) const; + + +// float getRequestedAirspeed(); +// float getInitYaw() { return _init_yaw; } + +// bool controlYaw(); + bool climbout() { return climbout_; } +// float getPitch(float tecsPitch); +// float getRoll(float navigatorRoll); +// float getYaw(float navigatorYaw); +// float getThrottle(const hrt_abstime &now, float tecsThrottle); +// bool resetIntegrators(); +// bool resetAltTakeoff(); +// float getMinPitch(float climbout_min, float min); +// float getMaxPitch(float max); +// // bool setState(int new_state); +// const matrix::Vector2d &getStartWP() const { return _takeoff_wp; }; + +// void reset(); + + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; void play_next_tone(); void play_release_tone(); @@ -126,54 +254,88 @@ class __EXPORT AutogyroTakeoff : public ModuleParams private: /** state variables **/ - AutogyroTakeoffState _state{PRE_TAKEOFF_PREROTATE_START}; - AutogyroTakeoffState _state_last{PRE_TAKEOFF_PREROTATE_START}; - bool _initialized{false}; - hrt_abstime _initialized_time{0}; - hrt_abstime _time_in_state{0}; - hrt_abstime _last_sent_release_status{0}; - float _init_yaw{0.f}; - bool _climbout{false}; - - matrix::Vector2d _initial_wp; - matrix::Vector2d _takeoff_wp; - - uORB::Publication _tune_control{ORB_ID(tune_control)}; - uORB::Publication _takeoff_status_pub{ORB_ID(takeoff_status)}; - - // TODO: templorary sollution. Should be replaced with autogyro takeoff status with - // translation into custom mavlink message. Used to inform launch platform to - // release drone from lock - uORB::Publication _takeoff_informations_pub{ORB_ID(debug_value)}; + AutogyroTakeoffState state_{PRE_TAKEOFF_PREROTATE_START}; + AutogyroTakeoffState state_last_{PRE_TAKEOFF_PREROTATE_START}; + + /** + * Current state of runway takeoff procedure + */ + AutogyroTakeoffState takeoff_state_{PRE_TAKEOFF_PREROTATE_START}; + + /** + * True if the runway state machine is initialized + */ + bool initialized_{false}; + + /** + * The absolute time since system boot at which the state machine was intialized [us] + */ + hrt_abstime time_initialized_{0}; + + /** + * The absolute time the takeoff state transitions from CLAMPED_TO_RUNWAY -> CLIMBOUT [us] + */ + hrt_abstime takeoff_time_{0}; + + /** + * Initial yaw of the vehicle on first pass through the runway takeoff state machine. + * used for heading hold mode. [rad] + */ + float initial_yaw_{0.f}; + + /** + * The global (lat, lon) position of the vehicle on first pass through the runway takeoff state machine. The + * takeoff path emanates from this point to correct for any GNSS uncertainty from the planned takeoff point. The + * vehicle should accordingly be set on the center of the runway before engaging the mission. [deg] + */ + matrix::Vector2d start_pos_global_{}; + hrt_abstime time_in_state_{0}; + hrt_abstime last_sent_release_status_{0}; + float init_yaw_{0.f}; + bool climbout_{false}; + + matrix::Vector2d initial_wp_; + matrix::Vector2d takeoff_wp_; + + uORB::Publication _tune_control{ORB_ID(tune_control)}; + uORB::Publication _takeoff_status_pub{ORB_ID(takeoff_status)}; + +// // TODO: templorary sollution. Should be replaced with autogyro takeoff status with +// // translation into custom mavlink message. Used to inform launch platform to +// // release drone from lock + uORB::Publication _takeoff_informations_pub{ORB_ID(debug_value)}; DEFINE_PARAMETERS( - (ParamBool) _param_ag_tkoff, - (ParamBool) _param_rwto_tkoff, - (ParamInt) _param_rwto_hdg, - (ParamFloat) _param_rwto_max_thr, - (ParamFloat) _param_rwto_psp, - (ParamFloat) _param_rwto_max_pitch, - (ParamFloat) _param_rwto_airspd_scl, - (ParamFloat) _param_rwto_ramp_time, + (ParamBool) param_ag_tkoff_, + (ParamBool) param_rwto_tkoff_, + (ParamInt) param_rwto_hdg_, + (ParamFloat) param_rwto_max_thr_, + (ParamFloat) param_rwto_psp_, + (ParamFloat) param_rwto_max_pitch_, + (ParamFloat) param_rwto_airspd_scl_, + (ParamFloat) param_rwto_ramp_time_, - (ParamFloat) _param_fw_clmbout_diff, - (ParamFloat) _param_fw_airspd_max, - (ParamFloat) _param_fw_airspd_min, - (ParamFloat) _param_fw_airspd_trim, - (ParamFloat) _param_fw_thr_idle, + (ParamFloat) param_fw_clmbout_diff_, + (ParamFloat) param_fw_airspd_max_, + (ParamFloat) param_fw_airspd_min_, + (ParamFloat) param_fw_airspd_trim_, + (ParamFloat) param_fw_thr_idle_, - (ParamFloat) _param_ag_prerotator_minimal_rpm, - (ParamFloat) _param_ag_prerotator_target_rpm, - (ParamFloat) _param_ag_rotor_flight_rpm, - (ParamInt) _param_ag_prerotator_type, + (ParamFloat) param_ag_prerotator_minimal_rpm_, + (ParamFloat) param_ag_prerotator_target_rpm_, + (ParamFloat) param_ag_rotor_flight_rpm_, + (ParamInt) param_ag_prerotator_type_, - (ParamFloat) _param_ag_nav_alt, - (ParamFloat) _param_ag_tko_max_roll + (ParamFloat) param_ag_nav_alt_, + (ParamFloat) param_ag_tko_max_roll_ ) -}; + + + + }; }