From 20ec727d9fdbec46be75d21a838fef9e1c20cb88 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 10 Sep 2015 14:22:00 +0200 Subject: [PATCH 01/58] added option for direct yaw control with rudder for fixed wing --- msg/vehicle_attitude_setpoint.msg | 2 ++ 1 file changed, 2 insertions(+) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 7bbb670b31..3bbb5efa7b 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -21,3 +21,5 @@ float32 thrust # Thrust in Newton the power system should generate bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool yaw_reset_integral # Reset yaw integral part (navigation logic change) + +bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) From ee4249f30f72b11552d84fe98fe5c1d46a3624c3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 10 Sep 2015 14:22:39 +0200 Subject: [PATCH 02/58] extended ecl yaw controller for yaw tracking with rudder --- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 34 +++++++++++++++++++ src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 10 ++++-- 2 files changed, 41 insertions(+), 3 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 4849bdad79..240a426bd0 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -67,6 +67,9 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data case COORD_METHOD_CLOSEACC: return control_attitude_impl_accclosedloop(ctl_data); + case COORD_METHOD_HEADING: + return control_heading(ctl_data); + default: static hrt_abstime last_print = 0; @@ -238,3 +241,34 @@ float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_Co /* dont set a rate setpoint */ return 0.0f; } + +float ECL_YawController::control_heading(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && + PX4_ISFINITE(ctl_data.yaw) && + PX4_ISFINITE(ctl_data.airspeed))) { + perf_count(_nonfinite_input_perf); + warnx("not controlling yaw"); + return _rate_setpoint; + } + + /* Calculate the error */ + float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = yaw_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + } + + return _rate_setpoint; +} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 81d8210549..e3e529b30f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -76,14 +76,16 @@ public: _coordinated_method = coordinated_method; } -protected: - float _coordinated_min_speed; - enum { COORD_METHOD_OPEN = 0, COORD_METHOD_CLOSEACC = 1, + COORD_METHOD_HEADING = 2 }; +protected: + float _coordinated_min_speed; + float _max_rate; + int32_t _coordinated_method; float control_bodyrate_impl(const struct ECL_ControlData &ctl_data); @@ -92,6 +94,8 @@ protected: float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data); + float control_heading(const struct ECL_ControlData &ctl_data); + }; #endif // ECL_YAW_CONTROLLER_H From f43d50fbc97a852dff084be29cf8580085f51cad Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 10 Sep 2015 14:35:40 +0200 Subject: [PATCH 03/58] implemented runway takeoff for fw --- .../fw_att_control/fw_att_control_main.cpp | 8 + .../fw_pos_control_l1_main.cpp | 202 ++++++++++++------ .../fw_pos_control_l1_params.c | 9 + 3 files changed, 150 insertions(+), 69 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 3ed5b3a5a3..081645c3ae 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -811,6 +811,7 @@ FixedwingAttitudeControl::task_main() float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; + float yaw_sp = 0.0f; float yaw_manual = 0.0f; float throttle_sp = 0.0f; @@ -824,6 +825,7 @@ FixedwingAttitudeControl::task_main() /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + yaw_sp = _att_sp.yaw_body; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -952,12 +954,18 @@ FixedwingAttitudeControl::task_main() control_input.acc_body_z = _accel.z; control_input.roll_setpoint = roll_sp; control_input.pitch_setpoint = pitch_sp; + control_input.yaw_setpoint = yaw_sp; control_input.airspeed_min = _parameters.airspeed_min; control_input.airspeed_max = _parameters.airspeed_max; control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; + if (_att_sp.fw_control_yaw == true) { + // this method controls heading directly with rudder. Used for auto takeoff on runway + _yaw_ctrl.set_coordinated_method(ECL_YawController::COORD_METHOD_HEADING); + } + /* Run attitude controllers */ if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f21bfc5f2f..dbf26912ca 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -195,6 +195,8 @@ private: bool land_useterrain; bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ + bool _takeoff_initialized; + hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ /* takeoff/launch states */ @@ -278,6 +280,8 @@ private: float land_flare_pitch_max_deg; int land_use_terrain_estimate; + int do_runway_takeoff; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -326,6 +330,8 @@ private: param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; + param_t do_runway_takeoff; + } _parameter_handles; /**< handles for interesting parameters */ @@ -520,6 +526,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_onslope(false), land_useterrain(false), _was_in_air(false), + _takeoff_initialized(false), + _takeoff_runway_start(0), _time_went_in_air(0), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), @@ -563,6 +571,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); + _parameter_handles.do_runway_takeoff = param_find("FW_RUNWAY_TKOFF"); + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -666,6 +676,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); + param_get(_parameter_handles.do_runway_takeoff, &(_parameters.do_runway_takeoff)); + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -1054,6 +1066,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; + _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder + float eas2tas = 1.0f; // XXX calculate actual number based on current measurements /* filter speed and altitude for controller */ @@ -1338,94 +1352,144 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - /* Perform launch detection */ - if (launchDetector.launchDetectionEnabled() && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { - /* Inform user that launchdetection is running */ - static hrt_abstime last_sent = 0; - if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_critical(_mavlink_fd, "Launchdetection running"); - last_sent = hrt_absolute_time(); + if (_parameters.do_runway_takeoff == true) { + if (!_takeoff_initialized) { + _hdg_hold_yaw = _att.yaw; + _takeoff_runway_start = hrt_absolute_time(); + _takeoff_initialized = true; } - /* Detect launch */ - launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); - - /* update our copy of the launch detection state */ - launch_detection_state = launchDetector.getLaunchDetected(); - } else { - /* no takeoff detection --> fly */ - launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; - } - - /* Set control values depending on the detection state */ - if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { - /* Launch has been detected, hence we have to control the plane. */ - - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - - /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use - * full throttle, otherwise we use the preTakeOff Throttle */ - float takeoff_throttle = launch_detection_state != - LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? - launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; - - /* select maximum pitch: the launchdetector may impose another limit for the pitch - * depending on the state of the launch */ - float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); - float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); - - /* apply minimum pitch and limit roll if target altitude is not within climbout_diff - * meters */ - if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { + if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { + _att_sp.pitch_body = math::radians(10.0f); // XXX consider this magic value + // ramp up throttle to max throttle + _att_sp.thrust = _att_sp.thrust < _parameters.throttle_max ? hrt_elapsed_time(&_takeoff_runway_start) / (2000000) : _parameters.throttle_max; + } else { + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, math::radians(_parameters.pitch_limit_min), takeoff_pitch_max_rad, - _parameters.throttle_min, takeoff_throttle, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, true, - math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::max(math::radians(_pos_sp_triplet.current.pitch_min), math::radians(10.0f)), _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_TAKEOFF, takeoff_pitch_max_deg != _parameters.pitch_limit_max); - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); - - } else { - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(_parameters.airspeed_trim), - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - takeoff_throttle, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); + _att_sp.pitch_body = _tecs.get_pitch_demand(); + _att_sp.thrust = math::min(_tecs.get_throttle_demand(), _parameters.throttle_max); } - } else { - /* Tell the attitude controller to stop integrating while we are waiting - * for the launch */ - _att_sp.roll_reset_integral = true; - _att_sp.pitch_reset_integral = true; - _att_sp.yaw_reset_integral = true; - /* Set default roll and pitch setpoints during detection phase */ - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), - math::radians(10.0f)); + if (_global_pos.alt - _global_pos.terrain_alt > 4.0f) { // XXX investigate on this + // start to navigate to takeoff waypoint as soon as we are high enough in the air + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + } else { + // for takeoff we want heading control with rudder, roll and pitch stabilization to zero + // throttle should be ramped up to max + _att_sp.roll_body = 0.0f; + _att_sp.yaw_body = _hdg_hold_yaw; + _att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + } + + + } else { + /* Perform launch detection */ + if (launchDetector.launchDetectionEnabled() && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + /* Inform user that launchdetection is running */ + static hrt_abstime last_sent = 0; + if(hrt_absolute_time() - last_sent > 4e6) { + mavlink_log_critical(_mavlink_fd, "Launchdetection running"); + last_sent = hrt_absolute_time(); + } + + /* Detect launch */ + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + + /* update our copy of the launch detection state */ + launch_detection_state = launchDetector.getLaunchDetected(); + } else { + /* no takeoff detection --> fly */ + launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + } + + /* Set control values depending on the detection state */ + if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { + /* Launch has been detected, hence we have to control the plane. */ + + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use + * full throttle, otherwise we use the preTakeOff Throttle */ + float takeoff_throttle = launch_detection_state != + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? + launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; + + /* select maximum pitch: the launchdetector may impose another limit for the pitch + * depending on the state of the launch */ + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); + + /* apply minimum pitch and limit roll if target altitude is not within climbout_diff + * meters */ + if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(1.3f * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, takeoff_throttle, + _parameters.throttle_cruise, + true, + math::max(math::radians(_pos_sp_triplet.current.pitch_min), + math::radians(10.0f)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); + + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + + } else { + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(_parameters.airspeed_trim), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + takeoff_throttle, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); + } + } else { + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + + /* Set default roll and pitch setpoints during detection phase */ + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)); + } } } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 5870039c24..2865ec7ce6 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -426,6 +426,15 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); */ PARAM_DEFINE_INT32(FW_LND_USETER, 0); +/** + * Enable or disable runway takeoff with landing gear + * + * 0: disabled, 1: enabled + * + * @group L1 Control + */ +PARAM_DEFINE_INT32(FW_RUNWAY_TKOFF, 0); + /** * Flare, minimum pitch * From 059e40f78028cdf210f3dff2a2e0f7fcc86b2e8a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 12 Sep 2015 01:57:09 +0200 Subject: [PATCH 04/58] - fixed throttle ramp-up - added parameter to specify which heading to keep on runway - validate terrain alt before using it --- .../fw_pos_control_l1_main.cpp | 76 +++++++++++++++---- .../fw_pos_control_l1_params.c | 9 +++ 2 files changed, 70 insertions(+), 15 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index dbf26912ca..56da2f7c6e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -196,6 +196,7 @@ private: bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ bool _takeoff_initialized; + bool _takeoff_on_runway; /**< true as long as runway takeoff is in progress */ hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ @@ -281,6 +282,7 @@ private: int land_use_terrain_estimate; int do_runway_takeoff; + int runway_takeoff_heading; } _parameters; /**< local copies of interesting parameters */ @@ -331,6 +333,7 @@ private: param_t land_use_terrain_estimate; param_t do_runway_takeoff; + param_t runway_takeoff_heading; } _parameter_handles; /**< handles for interesting parameters */ @@ -397,6 +400,11 @@ private: */ float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); + /** + * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available + */ + float get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos); + /** * Check if we are in a takeoff situation */ @@ -527,7 +535,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_useterrain(false), _was_in_air(false), _takeoff_initialized(false), - _takeoff_runway_start(0), + _takeoff_on_runway(false), + _takeoff_runway_start(0), _time_went_in_air(0), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), @@ -572,6 +581,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); _parameter_handles.do_runway_takeoff = param_find("FW_RUNWAY_TKOFF"); + _parameter_handles.runway_takeoff_heading = param_find("FW_TKOFF_HDG"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -677,6 +687,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); param_get(_parameter_handles.do_runway_takeoff, &(_parameters.do_runway_takeoff)); + param_get(_parameter_handles.runway_takeoff_heading, &(_parameters.runway_takeoff_heading)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -970,6 +981,15 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint } } +float FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos) +{ + if (PX4_ISFINITE(global_pos.terrain_alt) && global_pos.terrain_alt_valid) { + return global_pos.terrain_alt; + } + + return takeoff_alt; +} + bool FixedwingPositionControl::update_desired_altitude(float dt) { /* @@ -1357,19 +1377,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _hdg_hold_yaw = _att.yaw; _takeoff_runway_start = hrt_absolute_time(); _takeoff_initialized = true; + _takeoff_on_runway = true; + /* need this already before takeoff is detected + * doesn't matter if it gets reset when takeoff is detected eventually */ + _takeoff_ground_alt = _global_pos.alt; } - - if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { - _att_sp.pitch_body = math::radians(10.0f); // XXX consider this magic value - // ramp up throttle to max throttle - _att_sp.thrust = _att_sp.thrust < _parameters.throttle_max ? hrt_elapsed_time(&_takeoff_runway_start) / (2000000) : _parameters.throttle_max; + if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { // XXX takeoff: magic value + _takeoff_on_runway = true; + _att_sp.pitch_body = math::radians(10.0f); // XXX takeoff: magic value } else { - float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + // min airspeed reached, let tecs control it + _takeoff_on_runway = false; + float takeoff_pitch_max_deg = _parameters.pitch_limit_max; float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); - /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(1.3f * _parameters.airspeed_min), + calculate_target_airspeed(1.3f * _parameters.airspeed_min), // XXX takeoff: magic value eas2tas, math::radians(_parameters.pitch_limit_min), takeoff_pitch_max_rad, @@ -1384,20 +1408,34 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi takeoff_pitch_max_deg != _parameters.pitch_limit_max); _att_sp.pitch_body = _tecs.get_pitch_demand(); - _att_sp.thrust = math::min(_tecs.get_throttle_demand(), _parameters.throttle_max); } - if (_global_pos.alt - _global_pos.terrain_alt > 4.0f) { // XXX investigate on this + // update navigation + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); + //PX4_WARN("talt: %f", (double)terrain_alt); + + if (_global_pos.alt - terrain_alt > 4.0f) { // XXX takeoff: magic value + // XXX once in here we should maybe not fall out again (although it shouldn't matter when we're climbing) // start to navigate to takeoff waypoint as soon as we are high enough in the air - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); } else { // for takeoff we want heading control with rudder, roll and pitch stabilization to zero // throttle should be ramped up to max _att_sp.roll_body = 0.0f; - _att_sp.yaw_body = _hdg_hold_yaw; - _att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + //_att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + + if (_parameters.runway_takeoff_heading == 0) { + // fix heading in the direction the airframe points + _att_sp.yaw_body = _hdg_hold_yaw; + //PX4_WARN("using initial heading: %.4f", (double)_att_sp.yaw_body); + } else if (_parameters.runway_takeoff_heading == 1) { + // or head into the direction of the takeoff waypoint + // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) + _att_sp.yaw_body = _l1_control.nav_bearing(); // XXX check if this is really the actual bearing + //PX4_WARN("using nav heading: %.4f", (double)_att_sp.yaw_body); + } } @@ -1703,10 +1741,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = 0.0f; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && + !_parameters.do_runway_takeoff) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + _parameters.do_runway_takeoff && _takeoff_on_runway == true) { + /* Ramp-up thrust and stay at max */ + _att_sp.thrust = _att_sp.thrust < _parameters.throttle_max ? hrt_elapsed_time(&_takeoff_runway_start) / (float)2000000 : _parameters.throttle_max; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; @@ -1889,6 +1933,8 @@ void FixedwingPositionControl::reset_takeoff_state() { launch_detection_state = LAUNCHDETECTION_RES_NONE; launchDetector.reset(); + _takeoff_initialized = false; + _takeoff_on_runway = false; } void FixedwingPositionControl::reset_landing_state() diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 2865ec7ce6..29fbeed386 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -435,6 +435,15 @@ PARAM_DEFINE_INT32(FW_LND_USETER, 0); */ PARAM_DEFINE_INT32(FW_RUNWAY_TKOFF, 0); +/** + * Specifies which heading should be held during runnway takeoff. + * + * 0: airframe heading, 1: heading towards takeoff waypoint + * + * @group L1 Control + */ +PARAM_DEFINE_INT32(FW_TKOFF_HDG, 0); + /** * Flare, minimum pitch * From ea884b34f0f13168fee6d36d0b61b0a027f6f8ef Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 12 Sep 2015 02:06:37 +0200 Subject: [PATCH 05/58] use and reset the new yaw control method --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 ++ src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 081645c3ae..de8ee9173e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -964,6 +964,8 @@ FixedwingAttitudeControl::task_main() if (_att_sp.fw_control_yaw == true) { // this method controls heading directly with rudder. Used for auto takeoff on runway _yaw_ctrl.set_coordinated_method(ECL_YawController::COORD_METHOD_HEADING); + } else { + _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); } /* Run attitude controllers */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 56da2f7c6e..c8832d830a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1424,7 +1424,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // for takeoff we want heading control with rudder, roll and pitch stabilization to zero // throttle should be ramped up to max _att_sp.roll_body = 0.0f; - //_att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + _att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly if (_parameters.runway_takeoff_heading == 0) { // fix heading in the direction the airframe points From 24179a0d936c0ba3b93f4fc438f4a9546c42a096 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 12 Sep 2015 10:29:14 +0200 Subject: [PATCH 06/58] consitent parameter naming for runway takeoff, added parameters for important values --- .../fw_pos_control_l1_main.cpp | 29 +++++++++---- .../fw_pos_control_l1_params.c | 41 ++++++++++++++++++- 2 files changed, 61 insertions(+), 9 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index c8832d830a..6771921b2f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -283,6 +283,9 @@ private: int do_runway_takeoff; int runway_takeoff_heading; + float runway_takeoff_nav_alt; + float runway_takeoff_throttle; + float runway_takeoff_pitch_sp; } _parameters; /**< local copies of interesting parameters */ @@ -334,6 +337,9 @@ private: param_t do_runway_takeoff; param_t runway_takeoff_heading; + param_t runway_takeoff_nav_alt; + param_t runway_takeoff_throttle; + param_t runway_takeoff_pitch_sp; } _parameter_handles; /**< handles for interesting parameters */ @@ -580,8 +586,11 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); - _parameter_handles.do_runway_takeoff = param_find("FW_RUNWAY_TKOFF"); - _parameter_handles.runway_takeoff_heading = param_find("FW_TKOFF_HDG"); + _parameter_handles.do_runway_takeoff = param_find("FW_RNW_TKOFF"); + _parameter_handles.runway_takeoff_heading = param_find("FW_RNW_HDG"); + _parameter_handles.runway_takeoff_nav_alt = param_find("FW_RNW_NAV_ALT"); + _parameter_handles.runway_takeoff_throttle = param_find("FW_RNW_MAX_THR"); + _parameter_handles.runway_takeoff_pitch_sp = param_find("FW_RNW_PSP"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -688,6 +697,9 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.do_runway_takeoff, &(_parameters.do_runway_takeoff)); param_get(_parameter_handles.runway_takeoff_heading, &(_parameters.runway_takeoff_heading)); + param_get(_parameter_handles.runway_takeoff_nav_alt, &(_parameters.runway_takeoff_nav_alt)); + param_get(_parameter_handles.runway_takeoff_throttle, &(_parameters.runway_takeoff_throttle)); + param_get(_parameter_handles.runway_takeoff_pitch_sp, &(_parameters.runway_takeoff_pitch_sp)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -1385,7 +1397,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { // XXX takeoff: magic value _takeoff_on_runway = true; - _att_sp.pitch_body = math::radians(10.0f); // XXX takeoff: magic value + _att_sp.pitch_body = math::radians(_parameters.runway_takeoff_pitch_sp); } else { // min airspeed reached, let tecs control it _takeoff_on_runway = false; @@ -1397,11 +1409,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi eas2tas, math::radians(_parameters.pitch_limit_min), takeoff_pitch_max_rad, - _parameters.throttle_min, _parameters.throttle_max, + _parameters.throttle_min, + _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? _parameters.throttle_cruise, true, math::max(math::radians(_pos_sp_triplet.current.pitch_min), - math::radians(10.0f)), + math::radians(10.0f)), _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_TAKEOFF, @@ -1415,7 +1428,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); //PX4_WARN("talt: %f", (double)terrain_alt); - if (_global_pos.alt - terrain_alt > 4.0f) { // XXX takeoff: magic value + if (_global_pos.alt - terrain_alt > _parameters.runway_takeoff_nav_alt) { // XXX once in here we should maybe not fall out again (although it shouldn't matter when we're climbing) // start to navigate to takeoff waypoint as soon as we are high enough in the air _att_sp.roll_body = _l1_control.nav_roll(); @@ -1750,7 +1763,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _parameters.do_runway_takeoff && _takeoff_on_runway == true) { /* Ramp-up thrust and stay at max */ - _att_sp.thrust = _att_sp.thrust < _parameters.throttle_max ? hrt_elapsed_time(&_takeoff_runway_start) / (float)2000000 : _parameters.throttle_max; + _att_sp.thrust = _att_sp.thrust < _parameters.runway_takeoff_throttle ? + hrt_elapsed_time(&_takeoff_runway_start) / (float)2000000 : + _parameters.runway_takeoff_throttle; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 29fbeed386..5e05c6b9ef 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -431,18 +431,55 @@ PARAM_DEFINE_INT32(FW_LND_USETER, 0); * * 0: disabled, 1: enabled * + * @min 0 + * @max 1 * @group L1 Control */ -PARAM_DEFINE_INT32(FW_RUNWAY_TKOFF, 0); +PARAM_DEFINE_INT32(FW_RNW_TKOFF, 0); /** * Specifies which heading should be held during runnway takeoff. * * 0: airframe heading, 1: heading towards takeoff waypoint * + * @min 0 + * @max 1 * @group L1 Control */ -PARAM_DEFINE_INT32(FW_TKOFF_HDG, 0); +PARAM_DEFINE_INT32(FW_RNW_HDG, 0); + +/** + * Altitude AGL at which navigation towards takeoff waypoint starts. + * Until FW_RNW_NAV_ALT is reached the plane is held level and only + * rudder is used to keep the heading (see FW_RNW_HDG). + * + * @min 0.0 + * @max 100.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_RNW_NAV_ALT, 5.0); + +/** + * Max throttle during runway takeoff. + * (Can be limited to test taxi on runway) + * + * @min 0.0 + * @max 1.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_RNW_MAX_THR, 1.0); + +/** + * Pitch setpoint during runway takeoff. + * A taildragger with stearable wheel might need to pitch up + * a little to keep it's wheel on the ground before airspeed + * to takeoff is reached. + * + * @min 0.0 + * @max 20.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_RNW_PSP, 0.0); /** * Flare, minimum pitch From b93f34c99bb5d71bc4260541235b9e906d9b2796 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 12 Sep 2015 10:42:35 +0200 Subject: [PATCH 07/58] use separate heading parameter beacause the other one gets reset in auto --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 6771921b2f..16ed16ad10 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -197,6 +197,7 @@ private: bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ bool _takeoff_initialized; bool _takeoff_on_runway; /**< true as long as runway takeoff is in progress */ + float _runway_takeoff_hdg_hold; hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ @@ -542,7 +543,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _was_in_air(false), _takeoff_initialized(false), _takeoff_on_runway(false), - _takeoff_runway_start(0), + _runway_takeoff_hdg_hold(0), + _takeoff_runway_start(0), _time_went_in_air(0), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), @@ -1386,7 +1388,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_parameters.do_runway_takeoff == true) { if (!_takeoff_initialized) { - _hdg_hold_yaw = _att.yaw; + _runway_takeoff_hdg_hold = _att.yaw; _takeoff_runway_start = hrt_absolute_time(); _takeoff_initialized = true; _takeoff_on_runway = true; @@ -1441,7 +1443,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_parameters.runway_takeoff_heading == 0) { // fix heading in the direction the airframe points - _att_sp.yaw_body = _hdg_hold_yaw; + _att_sp.yaw_body = _runway_takeoff_hdg_hold; //PX4_WARN("using initial heading: %.4f", (double)_att_sp.yaw_body); } else if (_parameters.runway_takeoff_heading == 1) { // or head into the direction of the takeoff waypoint From 1f8ebb71f51ed67aef6a14cc300d771fca789bb9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 14 Sep 2015 10:08:33 +0200 Subject: [PATCH 08/58] fix comment --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 5e05c6b9ef..76b22f8350 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -461,7 +461,7 @@ PARAM_DEFINE_FLOAT(FW_RNW_NAV_ALT, 5.0); /** * Max throttle during runway takeoff. - * (Can be limited to test taxi on runway) + * (Can be used to test taxi on runway) * * @min 0.0 * @max 1.0 From 51ef8541739ffb864fd4797874df15fb5314fac0 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 16 Sep 2015 01:00:44 +0200 Subject: [PATCH 09/58] extracted runway takeoff logic into external class --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 208 ++++++++++++++++++ src/lib/runway_takeoff/RunwayTakeoff.h | 115 ++++++++++ src/lib/runway_takeoff/module.mk | 41 ++++ .../runway_takeoff/runway_takeoff_params.c | 100 +++++++++ .../fw_pos_control_l1_main.cpp | 153 +++++-------- .../fw_pos_control_l1_params.c | 55 ----- 6 files changed, 520 insertions(+), 152 deletions(-) create mode 100644 src/lib/runway_takeoff/RunwayTakeoff.cpp create mode 100644 src/lib/runway_takeoff/RunwayTakeoff.h create mode 100644 src/lib/runway_takeoff/module.mk create mode 100644 src/lib/runway_takeoff/runway_takeoff_params.c diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp new file mode 100644 index 0000000000..e1814cecc1 --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file RunwayTakeoff.cpp + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#include +#include +#include + +#include "RunwayTakeoff.h" +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +RunwayTakeoff::RunwayTakeoff() : + SuperBlock(NULL, "RWTO"), + _state(), + _initialized(false), + _initialized_time(0), + _init_yaw(0), + _climbout(false), + _min_airspeed_scaling(1.3f), + _runway_takeoff_enabled(this, "TKOFF"), + _runway_takeoff_heading(this, "HDG"), + _runway_takeoff_nav_alt(this, "NAV_ALT"), + _runway_takeoff_throttle(this, "MAX_THR"), + _runway_takeoff_pitch_sp(this, "PSP"), + _airspeed_min(this, "FW_AIRSPD_MIN", false), + _climbout_diff(this, "FW_CLMBOUT_DIFF", false) +{ + + updateParams(); +} + +RunwayTakeoff::~RunwayTakeoff() +{ + +} + +void RunwayTakeoff::init(float yaw) +{ + _init_yaw = yaw; + _initialized = true; + _state = RunwayTakeoffState::THROTTLE_RAMP; + _initialized_time = hrt_absolute_time(); +} + +void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) +{ + if (_climbout_diff.get() > 0.0001f && alt_agl < _climbout_diff.get()) { + _climbout = true; + } + + else { + _climbout = false; + } + + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: + if (hrt_elapsed_time(&_initialized_time) > 1e6) { + _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; + } + + break; + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { + _state = RunwayTakeoffState::TAKEOFF; + mavlink_log_info(mavlink_fd, "#audio: Takeoff airspeed reached"); + } + + break; + + case RunwayTakeoffState::TAKEOFF: + if (alt_agl > math::max(_runway_takeoff_nav_alt.get(), _climbout_diff.get())) { + _state = RunwayTakeoffState::FLY; + mavlink_log_info(mavlink_fd, "#audio: Navigating to waypoint"); + } + + break; + + default: + return; + } +} + +bool RunwayTakeoff::controlYaw() +{ + // keep controlling yaw with rudder until we have enough ground clearance + return _state < RunwayTakeoffState::FLY; +} + +float RunwayTakeoff::getPitch(float tecsPitch) +{ + if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { + return math::radians(_runway_takeoff_pitch_sp.get()); + } + + return tecsPitch; +} + +float RunwayTakeoff::getRoll(float navigatorRoll) +{ + if (_state < RunwayTakeoffState::FLY) { + return 0.0f; + } + + return navigatorRoll; +} + +float RunwayTakeoff::getYaw(float navigatorYaw) +{ + if (_state < RunwayTakeoffState::FLY) { + if (_runway_takeoff_heading.get() == 0) { + // fix heading in the direction the airframe points + return _init_yaw; + + } else if (_runway_takeoff_heading.get() == 1) { + // or head into the direction of the takeoff waypoint + // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) + return navigatorYaw; + } + } + + return navigatorYaw; +} + +float RunwayTakeoff::getThrottle(float tecsThrottle) +{ + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: { + float throttle = hrt_elapsed_time(&_initialized_time) / (float)2000000 * + _runway_takeoff_throttle.get(); + return throttle < _runway_takeoff_throttle.get() ? + throttle : + _runway_takeoff_throttle.get(); + } + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + return _runway_takeoff_throttle.get(); + + default: + return tecsThrottle; + } +} + +bool RunwayTakeoff::resetIntegrators() +{ + return _state <= RunwayTakeoffState::TAKEOFF; +} + +float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) +{ + if (_climbout) { + return math::max(sp_min, climbout_min); + } + + else { + return min; + } +} + +void RunwayTakeoff::reset() +{ + _initialized = false; + _state = RunwayTakeoffState::THROTTLE_RAMP; +} + +} diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h new file mode 100644 index 0000000000..7a4dd17f50 --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file RunwayTakeoff.h + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#ifndef RUNWAYTAKEOFF_H +#define RUNWAYTAKEOFF_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +enum RunwayTakeoffState { + THROTTLE_RAMP = 0, /**< */ + CLAMPED_TO_RUNWAY = 1, /**< */ + TAKEOFF = 2, /**< */ + FLY = 3 /**< */ +}; + +class __EXPORT RunwayTakeoff : public control::SuperBlock +{ +public: + RunwayTakeoff(); + ~RunwayTakeoff(); + + void init(float yaw); + void update(float airspeed, float alt_agl, int mavlink_fd); + + RunwayTakeoffState getState() { return _state; }; + bool isInitialized() { return _initialized; }; + + bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; + + bool controlYaw(); + bool climbout() { return _climbout; }; + float getPitch(float tecsPitch); + float getRoll(float navigatorRoll); + float getYaw(float navigatorYaw); + float getThrottle(float tecsThrottle); + bool resetIntegrators(); + float getMinAirspeedScaling() { return _min_airspeed_scaling; }; + float getMinPitch(float sp_min, float climbout_min, float min); + + void reset(); + +protected: +private: + /** state variables **/ + RunwayTakeoffState _state; + bool _initialized; + hrt_abstime _initialized_time; + float _init_yaw; + bool _climbout; + + /** magic values **/ + float _min_airspeed_scaling; + + /** parameters **/ + control::BlockParamInt _runway_takeoff_enabled; + control::BlockParamInt _runway_takeoff_heading; + control::BlockParamFloat _runway_takeoff_nav_alt; + control::BlockParamFloat _runway_takeoff_throttle; + control::BlockParamFloat _runway_takeoff_pitch_sp; + control::BlockParamFloat _airspeed_min; + control::BlockParamFloat _climbout_diff; + +}; + +} + +#endif // RUNWAYTAKEOFF_H diff --git a/src/lib/runway_takeoff/module.mk b/src/lib/runway_takeoff/module.mk new file mode 100644 index 0000000000..95b9aea53e --- /dev/null +++ b/src/lib/runway_takeoff/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# RunwayTakeoff Library +# + +SRCS = RunwayTakeoff.cpp \ + runway_takeoff_params.c + +MAXOPTIMIZATION = -Os diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c new file mode 100644 index 0000000000..6ccc21e76a --- /dev/null +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file runway_takeoff_params.c + * + * Parameters for runway takeoff + * + * @author Andreas Antener + */ + +#include + +#include + +/** + * Enable or disable runway takeoff with landing gear + * + * 0: disabled, 1: enabled + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_TKOFF, 0); + +/** + * Specifies which heading should be held during runnway takeoff. + * + * 0: airframe heading, 1: heading towards takeoff waypoint + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_HDG, 0); + +/** + * Altitude AGL at which navigation towards takeoff waypoint starts. + * Until RWTO_NAV_ALT is reached the plane is held level and only + * rudder is used to keep the heading (see RWTO_HDG). If this is lower + * than FW_CLMBOUT_DIFF, FW_CLMBOUT_DIFF is used instead. + * + * @min 0.0 + * @max 100.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); + +/** + * Max throttle during runway takeoff. + * (Can be used to test taxi on runway) + * + * @min 0.0 + * @max 1.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); + +/** + * Pitch setpoint during runway takeoff. + * A taildragger with stearable wheel might need to pitch up + * a little to keep it's wheel on the ground before airspeed + * to takeoff is reached. + * + * @min 0.0 + * @max 20.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 16ed16ad10..34f414f0d6 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -95,6 +95,7 @@ #include "landingslope.h" #include "mtecs/mTecs.h" #include +#include static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode @@ -195,12 +196,10 @@ private: bool land_useterrain; bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ - bool _takeoff_initialized; - bool _takeoff_on_runway; /**< true as long as runway takeoff is in progress */ - float _runway_takeoff_hdg_hold; - hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ + runwaytakeoff::RunwayTakeoff _runway_takeoff; + /* takeoff/launch states */ LaunchDetectionResult launch_detection_state; @@ -282,12 +281,6 @@ private: float land_flare_pitch_max_deg; int land_use_terrain_estimate; - int do_runway_takeoff; - int runway_takeoff_heading; - float runway_takeoff_nav_alt; - float runway_takeoff_throttle; - float runway_takeoff_pitch_sp; - } _parameters; /**< local copies of interesting parameters */ struct { @@ -336,12 +329,6 @@ private: param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; - param_t do_runway_takeoff; - param_t runway_takeoff_heading; - param_t runway_takeoff_nav_alt; - param_t runway_takeoff_throttle; - param_t runway_takeoff_pitch_sp; - } _parameter_handles; /**< handles for interesting parameters */ @@ -541,11 +528,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_onslope(false), land_useterrain(false), _was_in_air(false), - _takeoff_initialized(false), - _takeoff_on_runway(false), - _runway_takeoff_hdg_hold(0), - _takeoff_runway_start(0), _time_went_in_air(0), + _runway_takeoff(), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), landingslope(), @@ -588,12 +572,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); - _parameter_handles.do_runway_takeoff = param_find("FW_RNW_TKOFF"); - _parameter_handles.runway_takeoff_heading = param_find("FW_RNW_HDG"); - _parameter_handles.runway_takeoff_nav_alt = param_find("FW_RNW_NAV_ALT"); - _parameter_handles.runway_takeoff_throttle = param_find("FW_RNW_MAX_THR"); - _parameter_handles.runway_takeoff_pitch_sp = param_find("FW_RNW_PSP"); - _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -697,12 +675,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); - param_get(_parameter_handles.do_runway_takeoff, &(_parameters.do_runway_takeoff)); - param_get(_parameter_handles.runway_takeoff_heading, &(_parameters.runway_takeoff_heading)); - param_get(_parameter_handles.runway_takeoff_nav_alt, &(_parameters.runway_takeoff_nav_alt)); - param_get(_parameter_handles.runway_takeoff_throttle, &(_parameters.runway_takeoff_throttle)); - param_get(_parameter_handles.runway_takeoff_pitch_sp, &(_parameters.runway_takeoff_pitch_sp)); - _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -752,6 +724,8 @@ FixedwingPositionControl::parameters_update() /* Update the mTecs */ _mTecs.updateParams(); + _runway_takeoff.updateParams(); + return OK; } @@ -1386,72 +1360,54 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - if (_parameters.do_runway_takeoff == true) { - if (!_takeoff_initialized) { - _runway_takeoff_hdg_hold = _att.yaw; - _takeoff_runway_start = hrt_absolute_time(); - _takeoff_initialized = true; - _takeoff_on_runway = true; + if (_runway_takeoff.runwayTakeoffEnabled()) { + if (!_runway_takeoff.isInitialized()) { + _runway_takeoff.init(_att.yaw); + /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; - } - - if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { // XXX takeoff: magic value - _takeoff_on_runway = true; - _att_sp.pitch_body = math::radians(_parameters.runway_takeoff_pitch_sp); - } else { - // min airspeed reached, let tecs control it - _takeoff_on_runway = false; - float takeoff_pitch_max_deg = _parameters.pitch_limit_max; - float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); - - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(1.3f * _parameters.airspeed_min), // XXX takeoff: magic value - eas2tas, - math::radians(_parameters.pitch_limit_min), - takeoff_pitch_max_rad, - _parameters.throttle_min, - _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? - _parameters.throttle_cruise, - true, - math::max(math::radians(_pos_sp_triplet.current.pitch_min), - math::radians(10.0f)), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_TAKEOFF, - takeoff_pitch_max_deg != _parameters.pitch_limit_max); - - _att_sp.pitch_body = _tecs.get_pitch_demand(); + mavlink_log_info(_mavlink_fd, "#audio: Takeoff on runway"); } // update navigation _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); - //PX4_WARN("talt: %f", (double)terrain_alt); - if (_global_pos.alt - terrain_alt > _parameters.runway_takeoff_nav_alt) { - // XXX once in here we should maybe not fall out again (although it shouldn't matter when we're climbing) - // start to navigate to takeoff waypoint as soon as we are high enough in the air - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - } else { - // for takeoff we want heading control with rudder, roll and pitch stabilization to zero - // throttle should be ramped up to max - _att_sp.roll_body = 0.0f; - _att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + // update runway takeoff helper + _runway_takeoff.update( + _airspeed.true_airspeed_m_s, + _global_pos.alt - terrain_alt, + _mavlink_fd); - if (_parameters.runway_takeoff_heading == 0) { - // fix heading in the direction the airframe points - _att_sp.yaw_body = _runway_takeoff_hdg_hold; - //PX4_WARN("using initial heading: %.4f", (double)_att_sp.yaw_body); - } else if (_parameters.runway_takeoff_heading == 1) { - // or head into the direction of the takeoff waypoint - // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) - _att_sp.yaw_body = _l1_control.nav_bearing(); // XXX check if this is really the actual bearing - //PX4_WARN("using nav heading: %.4f", (double)_att_sp.yaw_body); - } - } + // update tecs + float takeoff_pitch_max_deg = _parameters.pitch_limit_max; + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); + + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed( + _runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, + _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? + _parameters.throttle_cruise, + _runway_takeoff.climbout(), + math::radians(_runway_takeoff.getMinPitch( + _pos_sp_triplet.current.pitch_min, + 10.0f, + _parameters.pitch_limit_min)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); + + // assign values + _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); + _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); + _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); // tell attitude controller if he should control yaw directly + _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); } else { @@ -1553,6 +1509,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset takeoff/launch state */ + // FIXME: reset on arm/disarm cycle and mode switch if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -1754,23 +1711,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && - !_parameters.do_runway_takeoff) { + !_runway_takeoff.runwayTakeoffEnabled()) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); - } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto + + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - _parameters.do_runway_takeoff && _takeoff_on_runway == true) { - /* Ramp-up thrust and stay at max */ - _att_sp.thrust = _att_sp.thrust < _parameters.runway_takeoff_throttle ? - hrt_elapsed_time(&_takeoff_runway_start) / (float)2000000 : - _parameters.runway_takeoff_throttle; + _runway_takeoff.runwayTakeoffEnabled()) { + _att_sp.thrust = _runway_takeoff.getThrottle( + math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max)); + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; + } else { /* Copy thrust and pitch values from tecs */ if (_vehicle_status.condition_landed && @@ -1950,8 +1910,7 @@ void FixedwingPositionControl::reset_takeoff_state() { launch_detection_state = LAUNCHDETECTION_RES_NONE; launchDetector.reset(); - _takeoff_initialized = false; - _takeoff_on_runway = false; + _runway_takeoff.reset(); } void FixedwingPositionControl::reset_landing_state() diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 76b22f8350..5870039c24 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -426,61 +426,6 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); */ PARAM_DEFINE_INT32(FW_LND_USETER, 0); -/** - * Enable or disable runway takeoff with landing gear - * - * 0: disabled, 1: enabled - * - * @min 0 - * @max 1 - * @group L1 Control - */ -PARAM_DEFINE_INT32(FW_RNW_TKOFF, 0); - -/** - * Specifies which heading should be held during runnway takeoff. - * - * 0: airframe heading, 1: heading towards takeoff waypoint - * - * @min 0 - * @max 1 - * @group L1 Control - */ -PARAM_DEFINE_INT32(FW_RNW_HDG, 0); - -/** - * Altitude AGL at which navigation towards takeoff waypoint starts. - * Until FW_RNW_NAV_ALT is reached the plane is held level and only - * rudder is used to keep the heading (see FW_RNW_HDG). - * - * @min 0.0 - * @max 100.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_RNW_NAV_ALT, 5.0); - -/** - * Max throttle during runway takeoff. - * (Can be used to test taxi on runway) - * - * @min 0.0 - * @max 1.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_RNW_MAX_THR, 1.0); - -/** - * Pitch setpoint during runway takeoff. - * A taildragger with stearable wheel might need to pitch up - * a little to keep it's wheel on the ground before airspeed - * to takeoff is reached. - * - * @min 0.0 - * @max 20.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_RNW_PSP, 0.0); - /** * Flare, minimum pitch * From 6c31421889f5bc4fe1b389a72713cd12274774fe Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 23 Sep 2015 12:30:38 +0200 Subject: [PATCH 10/58] extracted heading controller --- .../attitude_fw/ecl_heading_controller.cpp | 163 ++++++++++++++++++ .../ecl/attitude_fw/ecl_heading_controller.h | 70 ++++++++ .../ecl/attitude_fw/ecl_yaw_controller.cpp | 34 ---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 5 +- src/lib/runway_takeoff/RunwayTakeoff.cpp | 4 +- .../fw_att_control/fw_att_control_main.cpp | 26 ++- .../fw_pos_control_l1_main.cpp | 19 +- 7 files changed, 265 insertions(+), 56 deletions(-) create mode 100644 src/lib/ecl/attitude_fw/ecl_heading_controller.cpp create mode 100644 src/lib/ecl/attitude_fw/ecl_heading_controller.h diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp b/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp new file mode 100644 index 0000000000..97dfa2f617 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_yaw_controller.cpp + * Implementation of a simple orthogonal coordinated turn yaw PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_heading_controller.h" +#include +#include +#include +#include +#include +#include +#include + +ECL_HeadingController::ECL_HeadingController() : + ECL_Controller("heading") +{ +} + +ECL_HeadingController::~ECL_HeadingController() +{ +} + +float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { + perf_count(_nonfinite_input_perf); + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; + + if (dt_micros > 500000) { + lock_integrator = true; + } + + /* input conditioning */ + float airspeed = ctl_data.airspeed; + + if (!PX4_ISFINITE(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); + + } else if (airspeed < ctl_data.airspeed_min) { + airspeed = ctl_data.airspeed_min; + } + + + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + + cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint; + + /* Calculate body angular rate error */ + _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error + + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { + + float id = _rate_error * dt; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); + + /* Apply PI rate controller and store non-limited output */ + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * + ctl_data.scaler; //scaler is proportional to 1/airspeed + warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); + + + return math::constrain(_last_output, -1.0f, 1.0f); +} + + +float ECL_HeadingController::control_attitude(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && + PX4_ISFINITE(ctl_data.yaw) && + PX4_ISFINITE(ctl_data.airspeed))) { + perf_count(_nonfinite_input_perf); + warnx("not controlling yaw"); + return _rate_setpoint; + } + + /* Calculate the error */ + float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = yaw_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + } + + return _rate_setpoint; +} diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.h b/src/lib/ecl/attitude_fw/ecl_heading_controller.h new file mode 100644 index 0000000000..5dd79cd7d4 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_heading_controller.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_yaw_controller.h + * Definition of a simple orthogonal coordinated turn yaw PID controller. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Andreas Antener + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ +#ifndef ECL_HEADING_CONTROLLER_H +#define ECL_HEADING_CONTROLLER_H + +#include +#include + +#include "ecl_controller.h" + +class __EXPORT ECL_HeadingController : + public ECL_Controller +{ +public: + ECL_HeadingController(); + + ~ECL_HeadingController(); + + float control_attitude(const struct ECL_ControlData &ctl_data); + + float control_bodyrate(const struct ECL_ControlData &ctl_data); +}; + +#endif // ECL_HEADING_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 240a426bd0..4849bdad79 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -67,9 +67,6 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data case COORD_METHOD_CLOSEACC: return control_attitude_impl_accclosedloop(ctl_data); - case COORD_METHOD_HEADING: - return control_heading(ctl_data); - default: static hrt_abstime last_print = 0; @@ -241,34 +238,3 @@ float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_Co /* dont set a rate setpoint */ return 0.0f; } - -float ECL_YawController::control_heading(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && - PX4_ISFINITE(ctl_data.yaw) && - PX4_ISFINITE(ctl_data.airspeed))) { - perf_count(_nonfinite_input_perf); - warnx("not controlling yaw"); - return _rate_setpoint; - } - - /* Calculate the error */ - float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; - - /* Apply P controller: rate setpoint from current error and time constant */ - _rate_setpoint = yaw_error / _tc; - - /* limit the rate */ - if (_max_rate > 0.01f) { - if (_rate_setpoint > 0.0f) { - _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; - - } else { - _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; - } - - } - - return _rate_setpoint; -} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index e3e529b30f..511b5fd36f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -78,8 +78,7 @@ public: enum { COORD_METHOD_OPEN = 0, - COORD_METHOD_CLOSEACC = 1, - COORD_METHOD_HEADING = 2 + COORD_METHOD_CLOSEACC = 1 }; protected: @@ -94,8 +93,6 @@ protected: float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data); - float control_heading(const struct ECL_ControlData &ctl_data); - }; #endif // ECL_YAW_CONTROLLER_H diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index e1814cecc1..d7c33d7094 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -105,7 +105,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLAMPED_TO_RUNWAY: if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { _state = RunwayTakeoffState::TAKEOFF; - mavlink_log_info(mavlink_fd, "#audio: Takeoff airspeed reached"); + mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } break; @@ -113,7 +113,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::TAKEOFF: if (alt_agl > math::max(_runway_takeoff_nav_alt.get(), _climbout_diff.get())) { _state = RunwayTakeoffState::FLY; - mavlink_log_info(mavlink_fd, "#audio: Navigating to waypoint"); + mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); } break; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index de8ee9173e..938347ff95 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -82,6 +82,7 @@ #include #include #include +#include #include /** @@ -248,6 +249,7 @@ private: ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; + ECL_HeadingController _heading_ctrl; /** @@ -502,6 +504,13 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + /* heading control parameters */ + _heading_ctrl.set_k_p(_parameters.y_p); + _heading_ctrl.set_k_i(_parameters.y_i); + _heading_ctrl.set_k_ff(_parameters.y_ff); + _heading_ctrl.set_integrator_max(_parameters.y_integrator_max); + _heading_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + return OK; } @@ -961,18 +970,14 @@ FixedwingAttitudeControl::task_main() control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; - if (_att_sp.fw_control_yaw == true) { - // this method controls heading directly with rudder. Used for auto takeoff on runway - _yaw_ctrl.set_coordinated_method(ECL_YawController::COORD_METHOD_HEADING); - } else { - _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); - } + _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); /* Run attitude controllers */ if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude + _heading_ctrl.control_attitude(control_input); /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); @@ -1012,7 +1017,14 @@ FixedwingAttitudeControl::task_main() } } - float yaw_u = _yaw_ctrl.control_bodyrate(control_input); + float yaw_u = 0.0f; + if (_att_sp.fw_control_yaw == true) { + yaw_u = _heading_ctrl.control_bodyrate(control_input); + } + + else { + yaw_u = _yaw_ctrl.control_bodyrate(control_input); + } _actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 34f414f0d6..7f7d0b15b3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -960,7 +960,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint * for the whole landing */ if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { if(!land_useterrain) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); + mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate"); land_useterrain = true; } return global_pos.terrain_alt; @@ -1229,7 +1229,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { target_bearing = _yaw; } - mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); + mavlink_log_info(_mavlink_fd, "#Landing, heading hold"); } // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); @@ -1290,7 +1290,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; - mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle"); + mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle"); } } @@ -1314,7 +1314,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!land_noreturn_vertical) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); + mavlink_log_info(_mavlink_fd, "#Landing, flaring"); land_noreturn_vertical = true; } //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); @@ -1336,7 +1336,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); + mavlink_log_info(_mavlink_fd, "#Landing, on slope"); land_onslope = true; } } else { @@ -1367,7 +1367,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "#audio: Takeoff on runway"); + mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); } // update navigation @@ -1417,7 +1417,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Inform user that launchdetection is running */ static hrt_abstime last_sent = 0; if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_critical(_mavlink_fd, "Launchdetection running"); + mavlink_log_critical(_mavlink_fd, "#Launchdetection running"); last_sent = hrt_absolute_time(); } @@ -1509,7 +1509,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset takeoff/launch state */ - // FIXME: reset on arm/disarm cycle and mode switch if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -1750,7 +1749,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * already (not by tecs) */ if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + (launch_detection_state == LAUNCHDETECTION_RES_NONE || + _runway_takeoff.runwayTakeoffEnabled()) + )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } From 234a200e6081bc19c0489a5bd266ed4d00cee917 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 24 Sep 2015 15:57:51 +0200 Subject: [PATCH 11/58] renamed heading controller to wheel controller, added groundspeed dependency and separate parameters --- msg/vehicle_attitude_setpoint.msg | 2 +- src/lib/ecl/attitude_fw/ecl_controller.h | 1 + ...ontroller.cpp => ecl_wheel_controller.cpp} | 55 +++++++---------- ...ng_controller.h => ecl_wheel_controller.h} | 8 +-- src/lib/runway_takeoff/RunwayTakeoff.cpp | 7 ++- src/lib/runway_takeoff/RunwayTakeoff.h | 2 +- .../fw_att_control/fw_att_control_main.cpp | 51 ++++++++++++---- .../fw_att_control/fw_att_control_params.c | 60 +++++++++++++++++++ .../fw_pos_control_l1_main.cpp | 7 ++- 9 files changed, 138 insertions(+), 55 deletions(-) rename src/lib/ecl/attitude_fw/{ecl_heading_controller.cpp => ecl_wheel_controller.cpp} (67%) rename src/lib/ecl/attitude_fw/{ecl_heading_controller.h => ecl_wheel_controller.h} (95%) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 3bbb5efa7b..bcfe0f41da 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -22,4 +22,4 @@ bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool yaw_reset_integral # Reset yaw integral part (navigation logic change) -bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) +bool fw_control_wheel # control heading with rudder (used for auto takeoff on runway) diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h index ac1ac88d04..0817204ccc 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_controller.h @@ -75,6 +75,7 @@ struct ECL_ControlData { float airspeed_max; float airspeed; float scaler; + float ground_speed; bool lock_integrator; }; diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp similarity index 67% rename from src/lib/ecl/attitude_fw/ecl_heading_controller.cpp rename to src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index 97dfa2f617..b0df2a95d6 100644 --- a/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -32,13 +32,13 @@ ****************************************************************************/ /** - * @file ecl_yaw_controller.cpp + * @file ecl_wheel_controller.cpp * Implementation of a simple orthogonal coordinated turn yaw PID controller. * * Authors and acknowledgements in header. */ -#include "ecl_heading_controller.h" +#include "ecl_wheel_controller.h" #include #include #include @@ -47,22 +47,20 @@ #include #include -ECL_HeadingController::ECL_HeadingController() : - ECL_Controller("heading") +ECL_WheelController::ECL_WheelController() : + ECL_Controller("wheel") { } -ECL_HeadingController::~ECL_HeadingController() +ECL_WheelController::~ECL_WheelController() { } -float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_data) +float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) && - PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && - PX4_ISFINITE(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.ground_speed))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -80,27 +78,21 @@ float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_ } /* input conditioning */ - float airspeed = ctl_data.airspeed; - - if (!PX4_ISFINITE(airspeed)) { - /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); - - } else if (airspeed < ctl_data.airspeed_min) { - airspeed = ctl_data.airspeed_min; + float min_speed = 1.0f; + /* assume minimum speed to prevent oscillations */ + float speed = min_speed; + if (ctl_data.ground_speed > speed) { + speed = ctl_data.ground_speed; } - - /* Transform setpoint to body angular rates (jacobian) */ - _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + - cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint; + float scaler = 1.0f / speed; /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error + _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error - if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && speed > min_speed) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -122,23 +114,22 @@ float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_ float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * - ctl_data.scaler; //scaler is proportional to 1/airspeed - warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); + _last_output = (_rate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler; + /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f", + (double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/ return math::constrain(_last_output, -1.0f, 1.0f); } -float ECL_HeadingController::control_attitude(const struct ECL_ControlData &ctl_data) +float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && - PX4_ISFINITE(ctl_data.yaw) && - PX4_ISFINITE(ctl_data.airspeed))) { + PX4_ISFINITE(ctl_data.yaw))) { perf_count(_nonfinite_input_perf); - warnx("not controlling yaw"); + warnx("not controlling wheel"); return _rate_setpoint; } diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.h b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h similarity index 95% rename from src/lib/ecl/attitude_fw/ecl_heading_controller.h rename to src/lib/ecl/attitude_fw/ecl_wheel_controller.h index 5dd79cd7d4..f153a8f8f1 100644 --- a/src/lib/ecl/attitude_fw/ecl_heading_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file ecl_yaw_controller.h + * @file ecl_wheel_controller.h * Definition of a simple orthogonal coordinated turn yaw PID controller. * * @author Lorenz Meier @@ -54,13 +54,13 @@ #include "ecl_controller.h" -class __EXPORT ECL_HeadingController : +class __EXPORT ECL_WheelController : public ECL_Controller { public: - ECL_HeadingController(); + ECL_WheelController(); - ~ECL_HeadingController(); + ~ECL_WheelController(); float control_attitude(const struct ECL_ControlData &ctl_data); diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index d7c33d7094..5067260d32 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -123,10 +123,10 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) } } -bool RunwayTakeoff::controlYaw() +bool RunwayTakeoff::controlWheel() { - // keep controlling yaw with rudder until we have enough ground clearance - return _state < RunwayTakeoffState::FLY; + // keep controlling wheel until takeoff + return _state < RunwayTakeoffState::TAKEOFF; } float RunwayTakeoff::getPitch(float tecsPitch) @@ -140,6 +140,7 @@ float RunwayTakeoff::getPitch(float tecsPitch) float RunwayTakeoff::getRoll(float navigatorRoll) { + // until we have enough ground clearance, set roll to 0 if (_state < RunwayTakeoffState::FLY) { return 0.0f; } diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 7a4dd17f50..2f84afee2b 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -75,7 +75,7 @@ public: bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; - bool controlYaw(); + bool controlWheel(); bool climbout() { return _climbout; }; float getPitch(float tecsPitch); float getRoll(float navigatorRoll); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 938347ff95..144a8ed048 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -82,7 +82,7 @@ #include #include #include -#include +#include #include /** @@ -182,6 +182,11 @@ private: float y_coordinated_min_speed; int32_t y_coordinated_method; float y_rmax; + float w_p; + float w_i; + float w_ff; + float w_integrator_max; + float w_rmax; float airspeed_min; float airspeed_trim; @@ -223,6 +228,11 @@ private: param_t y_coordinated_min_speed; param_t y_coordinated_method; param_t y_rmax; + param_t w_p; + param_t w_i; + param_t w_ff; + param_t w_integrator_max; + param_t w_rmax; param_t airspeed_min; param_t airspeed_trim; @@ -249,7 +259,7 @@ private: ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; - ECL_HeadingController _heading_ctrl; + ECL_WheelController _wheel_ctrl; /** @@ -382,6 +392,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); + _parameter_handles.w_p = param_find("FW_WR_P"); + _parameter_handles.w_i = param_find("FW_WR_I"); + _parameter_handles.w_ff = param_find("FW_WR_FF"); + _parameter_handles.w_integrator_max = param_find("FW_WR_IMAX"); + _parameter_handles.w_rmax = param_find("FW_W_RMAX"); + _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); @@ -460,6 +476,12 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); + param_get(_parameter_handles.w_p, &(_parameters.w_p)); + param_get(_parameter_handles.w_i, &(_parameters.w_i)); + param_get(_parameter_handles.w_ff, &(_parameters.w_ff)); + param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max)); + param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax)); + param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); @@ -504,12 +526,12 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); - /* heading control parameters */ - _heading_ctrl.set_k_p(_parameters.y_p); - _heading_ctrl.set_k_i(_parameters.y_i); - _heading_ctrl.set_k_ff(_parameters.y_ff); - _heading_ctrl.set_integrator_max(_parameters.y_integrator_max); - _heading_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + /* wheel control parameters */ + _wheel_ctrl.set_k_p(_parameters.w_p); + _wheel_ctrl.set_k_i(_parameters.w_i); + _wheel_ctrl.set_k_ff(_parameters.w_ff); + _wheel_ctrl.set_integrator_max(_parameters.w_integrator_max); + _wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax)); return OK; } @@ -846,6 +868,7 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { @@ -871,6 +894,7 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_altitude_enabled) { @@ -890,6 +914,7 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else { /* @@ -940,6 +965,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } /* Prepare speed_body_u and speed_body_w */ @@ -969,6 +995,8 @@ FixedwingAttitudeControl::task_main() control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; + control_input.ground_speed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + + _global_pos.vel_e * _global_pos.vel_e); _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); @@ -977,7 +1005,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude - _heading_ctrl.control_attitude(control_input); + _wheel_ctrl.control_attitude(control_input); /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); @@ -1018,8 +1046,8 @@ FixedwingAttitudeControl::task_main() } float yaw_u = 0.0f; - if (_att_sp.fw_control_yaw == true) { - yaw_u = _heading_ctrl.control_bodyrate(control_input); + if (_att_sp.fw_control_wheel == true) { + yaw_u = _wheel_ctrl.control_bodyrate(control_input); } else { @@ -1031,6 +1059,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[2] += yaw_manual; if (!PX4_ISFINITE(yaw_u)) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 51c06fabb9..d5fdcd5e01 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -220,6 +220,55 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); */ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); +/** + * Wheel steering rate proportional gain + * + * This defines how much the wheel steering input will be commanded depending on the + * current body angular rate error. + * + * @min 0.005 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_P, 0.05f); + +/** + * Wheel steering rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @min 0.0 + * @max 50.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_I, 0.0f); + +/** + * Wheel steering rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_IMAX, 0.2f); + +/** + * Maximum wheel steering rate + * + * This limits the maximum wheel steering rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f); + /** * Roll rate feed forward * @@ -255,6 +304,17 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); */ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); +/** + * Wheel steering rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @min 0.0 + * @max 10.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f); + /** * Minimal speed for yaw coordination * diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7f7d0b15b3..08deeb60a9 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1074,7 +1074,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; - _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder + _att_sp.fw_control_wheel = false; // by default we don't want yaw to be contoller directly with rudder float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -1406,9 +1406,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // assign values _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); - _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); // tell attitude controller if he should control yaw directly + _att_sp.fw_control_wheel = _runway_takeoff.controlWheel(); _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); - + /*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body, + (double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/ } else { /* Perform launch detection */ From 0c875dd6d1091f6f49cea6dbc0a0b9513a378566 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 24 Sep 2015 20:56:42 +0200 Subject: [PATCH 12/58] calculate shortest yaw error --- src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index b0df2a95d6..12d2668f87 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -135,6 +135,9 @@ float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_da /* Calculate the error */ float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + /* shortest angle (wrap around) */ + yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F; + /*warnx("yaw_error: %.4f", (double)yaw_error);*/ /* Apply P controller: rate setpoint from current error and time constant */ _rate_setpoint = yaw_error / _tc; From e987082292b82e2dce5065dc343867521741f47c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 24 Sep 2015 21:53:10 +0200 Subject: [PATCH 13/58] split takeoff into 2 phases, reseting integrators when still on runway --- msg/vehicle_attitude_setpoint.msg | 2 +- src/lib/runway_takeoff/RunwayTakeoff.cpp | 29 +++++++++++++++---- src/lib/runway_takeoff/RunwayTakeoff.h | 12 ++++---- .../runway_takeoff/runway_takeoff_params.c | 6 ++-- .../fw_att_control/fw_att_control_main.cpp | 2 +- .../fw_pos_control_l1_main.cpp | 10 +++++-- 6 files changed, 43 insertions(+), 18 deletions(-) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index bcfe0f41da..3bbb5efa7b 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -22,4 +22,4 @@ bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool yaw_reset_integral # Reset yaw integral part (navigation logic change) -bool fw_control_wheel # control heading with rudder (used for auto takeoff on runway) +bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 5067260d32..77d2047882 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -59,6 +59,7 @@ RunwayTakeoff::RunwayTakeoff() : _init_yaw(0), _climbout(false), _min_airspeed_scaling(1.3f), + _max_takeoff_roll(15.0f), _runway_takeoff_enabled(this, "TKOFF"), _runway_takeoff_heading(this, "HDG"), _runway_takeoff_nav_alt(this, "NAV_ALT"), @@ -111,7 +112,15 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; case RunwayTakeoffState::TAKEOFF: - if (alt_agl > math::max(_runway_takeoff_nav_alt.get(), _climbout_diff.get())) { + if (alt_agl > _runway_takeoff_nav_alt.get()) { + _state = RunwayTakeoffState::CLIMBOUT; + mavlink_log_info(mavlink_fd, "#Climbout"); + } + + break; + + case RunwayTakeoffState::CLIMBOUT: + if (alt_agl > _climbout_diff.get()) { _state = RunwayTakeoffState::FLY; mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); } @@ -123,10 +132,10 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) } } -bool RunwayTakeoff::controlWheel() +bool RunwayTakeoff::controlYaw() { - // keep controlling wheel until takeoff - return _state < RunwayTakeoffState::TAKEOFF; + // keep controlling yaw directly until we start navigation + return _state < RunwayTakeoffState::CLIMBOUT; } float RunwayTakeoff::getPitch(float tecsPitch) @@ -141,10 +150,17 @@ float RunwayTakeoff::getPitch(float tecsPitch) float RunwayTakeoff::getRoll(float navigatorRoll) { // until we have enough ground clearance, set roll to 0 - if (_state < RunwayTakeoffState::FLY) { + if (_state < RunwayTakeoffState::CLIMBOUT) { return 0.0f; } + // allow some roll during climbout + else if (_state < RunwayTakeoffState::FLY) { + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll), + math::radians(_max_takeoff_roll)); + } + return navigatorRoll; } @@ -186,7 +202,8 @@ float RunwayTakeoff::getThrottle(float tecsThrottle) bool RunwayTakeoff::resetIntegrators() { - return _state <= RunwayTakeoffState::TAKEOFF; + // reset integrators if we're still on runway + return _state < RunwayTakeoffState::TAKEOFF; } float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 2f84afee2b..265ff24cb3 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -55,10 +55,11 @@ namespace runwaytakeoff { enum RunwayTakeoffState { - THROTTLE_RAMP = 0, /**< */ - CLAMPED_TO_RUNWAY = 1, /**< */ - TAKEOFF = 2, /**< */ - FLY = 3 /**< */ + THROTTLE_RAMP = 0, /**< ramping up throttle */ + CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */ + TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */ + CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */ + FLY = 4 /**< fly towards takeoff waypoint */ }; class __EXPORT RunwayTakeoff : public control::SuperBlock @@ -75,7 +76,7 @@ public: bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; - bool controlWheel(); + bool controlYaw(); bool climbout() { return _climbout; }; float getPitch(float tecsPitch); float getRoll(float navigatorRoll); @@ -98,6 +99,7 @@ private: /** magic values **/ float _min_airspeed_scaling; + float _max_takeoff_roll; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index 6ccc21e76a..d3ce5d238d 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -66,10 +66,10 @@ PARAM_DEFINE_INT32(RWTO_TKOFF, 0); PARAM_DEFINE_INT32(RWTO_HDG, 0); /** - * Altitude AGL at which navigation towards takeoff waypoint starts. + * Altitude AGL at which we have enough ground clearance to allow some roll. * Until RWTO_NAV_ALT is reached the plane is held level and only - * rudder is used to keep the heading (see RWTO_HDG). If this is lower - * than FW_CLMBOUT_DIFF, FW_CLMBOUT_DIFF is used instead. + * rudder is used to keep the heading (see RWTO_HDG). This should be below + * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0. * * @min 0.0 * @max 100.0 diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 144a8ed048..da91fda9f7 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1046,7 +1046,7 @@ FixedwingAttitudeControl::task_main() } float yaw_u = 0.0f; - if (_att_sp.fw_control_wheel == true) { + if (_att_sp.fw_control_yaw == true) { yaw_u = _wheel_ctrl.control_bodyrate(control_input); } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 08deeb60a9..ed71cf2955 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1074,7 +1074,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; - _att_sp.fw_control_wheel = false; // by default we don't want yaw to be contoller directly with rudder + _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -1371,6 +1371,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } // update navigation + // FIXME: after reselecting the takeoff wp, prev_wp is set to the next wp which causes wrong navigation behaviour _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); @@ -1406,8 +1407,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // assign values _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); - _att_sp.fw_control_wheel = _runway_takeoff.controlWheel(); + _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); + + _att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators(); + _att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators(); + _att_sp.yaw_reset_integral = _runway_takeoff.resetIntegrators(); + /*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body, (double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/ From 178ec7f4fc41fb136592212d1b645c822a0c1bad Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Sep 2015 11:05:56 +0200 Subject: [PATCH 14/58] stay out of climbout once height has been reached, don't mix navigator roll with fixed heading --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 77d2047882..599cb721d0 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -87,13 +87,6 @@ void RunwayTakeoff::init(float yaw) void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) { - if (_climbout_diff.get() > 0.0001f && alt_agl < _climbout_diff.get()) { - _climbout = true; - } - - else { - _climbout = false; - } switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: @@ -105,6 +98,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLAMPED_TO_RUNWAY: if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { + _climbout = true; _state = RunwayTakeoffState::TAKEOFF; mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } @@ -121,6 +115,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLIMBOUT: if (alt_agl > _climbout_diff.get()) { + _climbout = false; _state = RunwayTakeoffState::FLY; mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); } @@ -154,11 +149,17 @@ float RunwayTakeoff::getRoll(float navigatorRoll) return 0.0f; } - // allow some roll during climbout + // allow some roll during climbout if waypoint heading is targeted else if (_state < RunwayTakeoffState::FLY) { - return math::constrain(navigatorRoll, - math::radians(-_max_takeoff_roll), - math::radians(_max_takeoff_roll)); + if (_runway_takeoff_heading.get() == 0) { + // otherwise stay at 0 roll + return 0.0f; + + } else if (_runway_takeoff_heading.get() == 1) { + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll), + math::radians(_max_takeoff_roll)); + } } return navigatorRoll; From 0769ec53454de195ed804c8c3b01d4d210b1d4c5 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Sep 2015 14:30:01 +0200 Subject: [PATCH 15/58] added max pitch parameter for climbout phase --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 14 +++++++++++++- src/lib/runway_takeoff/RunwayTakeoff.h | 2 ++ src/lib/runway_takeoff/runway_takeoff_params.c | 13 ++++++++++++- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 4 files changed, 28 insertions(+), 3 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 599cb721d0..7b2ab97e8e 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -65,6 +65,7 @@ RunwayTakeoff::RunwayTakeoff() : _runway_takeoff_nav_alt(this, "NAV_ALT"), _runway_takeoff_throttle(this, "MAX_THR"), _runway_takeoff_pitch_sp(this, "PSP"), + _runway_takeoff_max_pitch(this, "MAX_PITCH"), _airspeed_min(this, "FW_AIRSPD_MIN", false), _climbout_diff(this, "FW_CLMBOUT_DIFF", false) { @@ -83,6 +84,7 @@ void RunwayTakeoff::init(float yaw) _initialized = true; _state = RunwayTakeoffState::THROTTLE_RAMP; _initialized_time = hrt_absolute_time(); + _climbout = true; } void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) @@ -98,7 +100,6 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLAMPED_TO_RUNWAY: if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { - _climbout = true; _state = RunwayTakeoffState::TAKEOFF; mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } @@ -218,6 +219,17 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) } } +float RunwayTakeoff::getMaxPitch(float max) +{ + if (_climbout && _runway_takeoff_max_pitch.get() > 0.1f) { + return _runway_takeoff_max_pitch.get(); + } + + else { + return max; + } +} + void RunwayTakeoff::reset() { _initialized = false; diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 265ff24cb3..86b8c01b6e 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -85,6 +85,7 @@ public: bool resetIntegrators(); float getMinAirspeedScaling() { return _min_airspeed_scaling; }; float getMinPitch(float sp_min, float climbout_min, float min); + float getMaxPitch(float max); void reset(); @@ -107,6 +108,7 @@ private: control::BlockParamFloat _runway_takeoff_nav_alt; control::BlockParamFloat _runway_takeoff_throttle; control::BlockParamFloat _runway_takeoff_pitch_sp; + control::BlockParamFloat _runway_takeoff_max_pitch; control::BlockParamFloat _airspeed_min; control::BlockParamFloat _climbout_diff; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index d3ce5d238d..137732c074 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); /** - * Pitch setpoint during runway takeoff. + * Pitch setpoint during taxi / before takeoff airspeed is reached. * A taildragger with stearable wheel might need to pitch up * a little to keep it's wheel on the ground before airspeed * to takeoff is reached. @@ -98,3 +98,14 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); + +/** + * Max pitch during takeoff. + * Fixed-wing settings are used if set to 0. Note that there is also a minimum + * pitch of 10 degrees during takeoff, so this must be larger if set. + * + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ed71cf2955..711e888416 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1382,7 +1382,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _mavlink_fd); // update tecs - float takeoff_pitch_max_deg = _parameters.pitch_limit_max; + float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, From 3eb0ce84df03c8abfbc1634a3183e4bc5eb6c5c3 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Sep 2015 15:42:43 +0200 Subject: [PATCH 16/58] set disarmed/min/max pwm for throttle channel in default fw configs --- ROMFS/px4fmu_common/init.d/2101_fw_AERT | 4 +++- ROMFS/px4fmu_common/init.d/2104_fw_AETR | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/2101_fw_AERT b/ROMFS/px4fmu_common/init.d/2101_fw_AERT index 3631a1e36c..c5bb1477c5 100644 --- a/ROMFS/px4fmu_common/init.d/2101_fw_AERT +++ b/ROMFS/px4fmu_common/init.d/2101_fw_AERT @@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults set MIXER AERT -# The ESC requires a specific pulse to arm. +# use PWM parameters for throttle channel set PWM_OUT 4 set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/init.d/2104_fw_AETR b/ROMFS/px4fmu_common/init.d/2104_fw_AETR index 3113ede87a..f533ce66fe 100644 --- a/ROMFS/px4fmu_common/init.d/2104_fw_AETR +++ b/ROMFS/px4fmu_common/init.d/2104_fw_AETR @@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults set MIXER AETR -# The ESC requires a specific pulse to arm. +# use PWM parameters for throttle channel set PWM_OUT 3 set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX From ff57c809b87c1f0e234071c442e00f2ebae6d13a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Sep 2015 17:45:09 +0200 Subject: [PATCH 17/58] updated default wheel params after first test --- src/modules/fw_att_control/fw_att_control_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index d5fdcd5e01..f4cfa7fd8d 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_P, 0.05f); +PARAM_DEFINE_FLOAT(FW_WR_P, 0.3f); /** * Wheel steering rate integrator gain @@ -242,7 +242,7 @@ PARAM_DEFINE_FLOAT(FW_WR_P, 0.05f); * @max 50.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); /** * Wheel steering rate integrator limit From d015fbd6782e942bb8f0a3afe0aa7772093ed021 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 2 Oct 2015 11:50:18 +0200 Subject: [PATCH 18/58] added startup config for Maja and new generic mixer that uses channel 5 for wheel steering --- ROMFS/px4fmu_common/init.d/2101_fw_AERT | 4 +- ROMFS/px4fmu_common/init.d/2105_maja | 48 ++++++++++ ROMFS/px4fmu_common/mixers/AERTW.main.mix | 95 +++++++++++++++++++ .../fw_pos_control_l1_main.cpp | 1 + 4 files changed, 146 insertions(+), 2 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/2105_maja create mode 100644 ROMFS/px4fmu_common/mixers/AERTW.main.mix diff --git a/ROMFS/px4fmu_common/init.d/2101_fw_AERT b/ROMFS/px4fmu_common/init.d/2101_fw_AERT index c5bb1477c5..d13514d03a 100644 --- a/ROMFS/px4fmu_common/init.d/2101_fw_AERT +++ b/ROMFS/px4fmu_common/init.d/2101_fw_AERT @@ -6,8 +6,8 @@ # # @output MAIN1 aileron # @output MAIN2 elevator -# @output MAIN4 rudder -# @output MAIN3 throttle +# @output MAIN3 rudder +# @output MAIN4 throttle # @output MAIN5 flaps # # @output AUX1 feed-through of RC AUX1 channel diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja new file mode 100644 index 0000000000..af27eca5d9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -0,0 +1,48 @@ +#!nsh +# +# @name Bormatec Maja +# +# @type Standard Plane +# +# @output MAIN1 aileron +# @output MAIN2 elevator +# @output MAIN3 rudder +# @output MAIN4 throttle +# @output MAIN5 wheel +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Andreas Antener +# + +sh /etc/init.d/rc.fw_defaults + +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 15 + param set FW_AIRSPD_MAX 20 + + param set FW_MAN_P_MAX 55 + param set FW_MAN_R_MAX 55 + param set FW_R_LIM 55 + + param set FW_WR_FF 0.2 + param set FW_WR_I 0.2 + param set FW_WR_IMAX 0.8 + param set FW_WR_P 1 + param set FW_W_RMAX 0 + + # set disarmed value for the ESC + param set PWM_DISARMED 1000 +fi + +set MIXER AERTW + +# use PWM parameters for throttle channel +set PWM_OUT 4 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTW.main.mix new file mode 100644 index 0000000000..af548826dc --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/AERTW.main.mix @@ -0,0 +1,95 @@ +Aileron/rudder/elevator/throttle/wheel mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, rudder, elevator, throttle and steerable wheel controls using PX4FMU. +The configuration assumes the aileron servo(s) are connected to PX4FMU servo +output 0, the elevator to output 1, the rudder to output 2, the throttle +to output 3 and the wheel to output 4. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust). + +Aileron mixer +------------- +Two scalers total (output, roll). + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +As there is only one output, if using two servos adjustments to compensate for +differences between the servos must be made mechanically. To obtain the correct +motion using a Y cable, the servos can be positioned reversed from one another. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Rudder mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Wheel mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the wheel servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + + +Gimbal / flaps / payload mixer for last three channels, +using the payload control group +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 711e888416..b9f24a6eb2 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -49,6 +49,7 @@ * * @author Lorenz Meier * @author Thomas Gubler + * @author Andreas Antener */ #include From 9c70eb0b63e1079f1fd0fcf8714cbd2fd138baed Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 2 Oct 2015 16:25:14 +0200 Subject: [PATCH 19/58] reduce wheel control speed scaling --- src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index 12d2668f87..d67a46bb1c 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -85,7 +85,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da speed = ctl_data.ground_speed; } - float scaler = 1.0f / speed; + /* only scale a certain amount with speed else the corrections get to small */ + float scaler = 0.7f + 0.3f / speed; /* Calculate body angular rate error */ _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error From f5f61e42af0530142d41a8186f9fa71e6b87be6a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 7 Oct 2015 10:12:35 +0200 Subject: [PATCH 20/58] replaced magic values with parameters, renamed internal representations --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 44 +++++++++---------- src/lib/runway_takeoff/RunwayTakeoff.h | 18 ++++---- .../runway_takeoff/runway_takeoff_params.c | 33 ++++++++++++++ 3 files changed, 63 insertions(+), 32 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 7b2ab97e8e..c04ad780f2 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -58,14 +58,14 @@ RunwayTakeoff::RunwayTakeoff() : _initialized_time(0), _init_yaw(0), _climbout(false), - _min_airspeed_scaling(1.3f), - _max_takeoff_roll(15.0f), _runway_takeoff_enabled(this, "TKOFF"), - _runway_takeoff_heading(this, "HDG"), - _runway_takeoff_nav_alt(this, "NAV_ALT"), - _runway_takeoff_throttle(this, "MAX_THR"), - _runway_takeoff_pitch_sp(this, "PSP"), - _runway_takeoff_max_pitch(this, "MAX_PITCH"), + _heading_mode(this, "HDG"), + _nav_alt(this, "NAV_ALT"), + _takeoff_throttle(this, "MAX_THR"), + _runway_pitch_sp(this, "PSP"), + _max_takeoff_pitch(this, "MAX_PITCH"), + _max_takeoff_roll(this, "MAX_ROLL"), + _min_airspeed_scaling(this, "AIRSPD_SCL"), _airspeed_min(this, "FW_AIRSPD_MIN", false), _climbout_diff(this, "FW_CLMBOUT_DIFF", false) { @@ -99,7 +99,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { + if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) { _state = RunwayTakeoffState::TAKEOFF; mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } @@ -107,7 +107,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; case RunwayTakeoffState::TAKEOFF: - if (alt_agl > _runway_takeoff_nav_alt.get()) { + if (alt_agl > _nav_alt.get()) { _state = RunwayTakeoffState::CLIMBOUT; mavlink_log_info(mavlink_fd, "#Climbout"); } @@ -137,7 +137,7 @@ bool RunwayTakeoff::controlYaw() float RunwayTakeoff::getPitch(float tecsPitch) { if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { - return math::radians(_runway_takeoff_pitch_sp.get()); + return math::radians(_runway_pitch_sp.get()); } return tecsPitch; @@ -152,14 +152,14 @@ float RunwayTakeoff::getRoll(float navigatorRoll) // allow some roll during climbout if waypoint heading is targeted else if (_state < RunwayTakeoffState::FLY) { - if (_runway_takeoff_heading.get() == 0) { + if (_heading_mode.get() == 0) { // otherwise stay at 0 roll return 0.0f; - } else if (_runway_takeoff_heading.get() == 1) { + } else if (_heading_mode.get() == 1) { return math::constrain(navigatorRoll, - math::radians(-_max_takeoff_roll), - math::radians(_max_takeoff_roll)); + math::radians(-_max_takeoff_roll.get()), + math::radians(_max_takeoff_roll).get()); } } @@ -169,11 +169,11 @@ float RunwayTakeoff::getRoll(float navigatorRoll) float RunwayTakeoff::getYaw(float navigatorYaw) { if (_state < RunwayTakeoffState::FLY) { - if (_runway_takeoff_heading.get() == 0) { + if (_heading_mode.get() == 0) { // fix heading in the direction the airframe points return _init_yaw; - } else if (_runway_takeoff_heading.get() == 1) { + } else if (_heading_mode.get() == 1) { // or head into the direction of the takeoff waypoint // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) return navigatorYaw; @@ -188,14 +188,14 @@ float RunwayTakeoff::getThrottle(float tecsThrottle) switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: { float throttle = hrt_elapsed_time(&_initialized_time) / (float)2000000 * - _runway_takeoff_throttle.get(); - return throttle < _runway_takeoff_throttle.get() ? + _takeoff_throttle.get(); + return throttle < _takeoff_throttle.get() ? throttle : - _runway_takeoff_throttle.get(); + _takeoff_throttle.get(); } case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - return _runway_takeoff_throttle.get(); + return _takeoff_throttle.get(); default: return tecsThrottle; @@ -221,8 +221,8 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) float RunwayTakeoff::getMaxPitch(float max) { - if (_climbout && _runway_takeoff_max_pitch.get() > 0.1f) { - return _runway_takeoff_max_pitch.get(); + if (_climbout && _max_takeoff_pitch.get() > 0.1f) { + return _max_takeoff_pitch.get(); } else { diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 86b8c01b6e..8e3cd35528 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -83,7 +83,7 @@ public: float getYaw(float navigatorYaw); float getThrottle(float tecsThrottle); bool resetIntegrators(); - float getMinAirspeedScaling() { return _min_airspeed_scaling; }; + float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; float getMinPitch(float sp_min, float climbout_min, float min); float getMaxPitch(float max); @@ -98,17 +98,15 @@ private: float _init_yaw; bool _climbout; - /** magic values **/ - float _min_airspeed_scaling; - float _max_takeoff_roll; - /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; - control::BlockParamInt _runway_takeoff_heading; - control::BlockParamFloat _runway_takeoff_nav_alt; - control::BlockParamFloat _runway_takeoff_throttle; - control::BlockParamFloat _runway_takeoff_pitch_sp; - control::BlockParamFloat _runway_takeoff_max_pitch; + control::BlockParamInt _heading_mode; + control::BlockParamFloat _nav_alt; + control::BlockParamFloat _takeoff_throttle; + control::BlockParamFloat _runway_pitch_sp; + control::BlockParamFloat _max_takeoff_pitch; + control::BlockParamFloat _max_takeoff_roll; + control::BlockParamFloat _min_airspeed_scaling; control::BlockParamFloat _airspeed_min; control::BlockParamFloat _climbout_diff; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index 137732c074..05110dddc2 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -109,3 +109,36 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); + +/** + * Max pitch during takeoff. + * Fixed-wing settings are used if set to 0. Note that there is also a minimum + * pitch of 10 degrees during takeoff, so this must be larger if set. + * + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); + +/** + * Max roll during climbout. + * Roll is limited during climbout to ensure enough lift and prevents aggressive + * navigation before we're on a safe height. + * + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0); + +/** + * Min. airspeed scaling factor for takeoff. + * Pitch up will be commanded when the following airspeed is reached: + * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL + * + * @min 0.0 + * @max 2.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3); From e0cdf65fb446bc40ff032ea88b70ebd3f6a6d1ab Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 8 Oct 2015 22:23:18 +0200 Subject: [PATCH 21/58] use navigator to hold heading --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 53 +++++++++++-------- src/lib/runway_takeoff/RunwayTakeoff.h | 10 +++- .../runway_takeoff/runway_takeoff_params.c | 11 ---- .../fw_pos_control_l1_main.cpp | 26 +++++++-- 4 files changed, 63 insertions(+), 37 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index c04ad780f2..16cbb04e45 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -47,6 +47,7 @@ #include #include #include +#include namespace runwaytakeoff { @@ -58,6 +59,8 @@ RunwayTakeoff::RunwayTakeoff() : _initialized_time(0), _init_yaw(0), _climbout(false), + _start_sp{}, + _target_sp{}, _runway_takeoff_enabled(this, "TKOFF"), _heading_mode(this, "HDG"), _nav_alt(this, "NAV_ALT"), @@ -150,17 +153,11 @@ float RunwayTakeoff::getRoll(float navigatorRoll) return 0.0f; } - // allow some roll during climbout if waypoint heading is targeted + // allow some roll during climbout else if (_state < RunwayTakeoffState::FLY) { - if (_heading_mode.get() == 0) { - // otherwise stay at 0 roll - return 0.0f; - - } else if (_heading_mode.get() == 1) { - return math::constrain(navigatorRoll, - math::radians(-_max_takeoff_roll.get()), - math::radians(_max_takeoff_roll).get()); - } + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll.get()), + math::radians(_max_takeoff_roll.get())); } return navigatorRoll; @@ -168,18 +165,6 @@ float RunwayTakeoff::getRoll(float navigatorRoll) float RunwayTakeoff::getYaw(float navigatorYaw) { - if (_state < RunwayTakeoffState::FLY) { - if (_heading_mode.get() == 0) { - // fix heading in the direction the airframe points - return _init_yaw; - - } else if (_heading_mode.get() == 1) { - // or head into the direction of the takeoff waypoint - // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) - return navigatorYaw; - } - } - return navigatorYaw; } @@ -230,6 +215,30 @@ float RunwayTakeoff::getMaxPitch(float max) } } +math::Vector<2> RunwayTakeoff::getPrevWP() +{ + math::Vector<2> prev_wp; + prev_wp(0) = (float)_start_sp.lat; + prev_wp(1) = (float)_start_sp.lon; + return prev_wp; +} + +math::Vector<2> RunwayTakeoff::getCurrWP(math::Vector<2> sp_curr_wp) +{ + if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::FLY) { + // navigating towards calculated direction if heading mode 0 and as long as we're in climbout + math::Vector<2> curr_wp; + curr_wp(0) = (float)_target_sp.lat; + curr_wp(1) = (float)_target_sp.lon; + return curr_wp; + + } else { + // navigating towards next mission waypoint + return sp_curr_wp; + } + +} + void RunwayTakeoff::reset() { _initialized = false; diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 8e3cd35528..2913b2ad64 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -50,6 +50,7 @@ #include #include #include +#include namespace runwaytakeoff { @@ -75,6 +76,10 @@ public: bool isInitialized() { return _initialized; }; bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; + float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; + position_setpoint_s *getStartSP() { return &_start_sp; }; + position_setpoint_s *getTargetSP() { return &_target_sp; }; + float getInitYaw() { return _init_yaw; }; bool controlYaw(); bool climbout() { return _climbout; }; @@ -83,9 +88,10 @@ public: float getYaw(float navigatorYaw); float getThrottle(float tecsThrottle); bool resetIntegrators(); - float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; float getMinPitch(float sp_min, float climbout_min, float min); float getMaxPitch(float max); + math::Vector<2> getPrevWP(); + math::Vector<2> getCurrWP(math::Vector<2> sp_curr_wp); void reset(); @@ -97,6 +103,8 @@ private: hrt_abstime _initialized_time; float _init_yaw; bool _climbout; + struct position_setpoint_s _start_sp; + struct position_setpoint_s _target_sp; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index 05110dddc2..7c64889e76 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -110,17 +110,6 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); */ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); -/** - * Max pitch during takeoff. - * Fixed-wing settings are used if set to 0. Note that there is also a minimum - * pitch of 10 degrees during takeoff, so this must be larger if set. - * - * @min 0.0 - * @max 60.0 - * @group Runway Takeoff - */ -PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); - /** * Max roll during climbout. * Roll is limited during climbout to ensure enough lift and prevents aggressive diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index b9f24a6eb2..176d4ed11b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1368,12 +1368,32 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; + + // draw a line from takeoff location into the direction the UAV is heading + get_waypoint_heading_distance( + _runway_takeoff.getInitYaw(), + HDG_HOLD_DIST_NEXT, + *_runway_takeoff.getStartSP(), + *_runway_takeoff.getTargetSP(), + true); + mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); } - // update navigation - // FIXME: after reselecting the takeoff wp, prev_wp is set to the next wp which causes wrong navigation behaviour - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + // update takeoff path if we're reaching the end of it + if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _runway_takeoff.getTargetSP()->lat, _runway_takeoff.getTargetSP()->lon) < HDG_HOLD_REACHED_DIST) { + get_waypoint_heading_distance( + _runway_takeoff.getInitYaw(), + HDG_HOLD_DIST_NEXT, + *_runway_takeoff.getStartSP(), + *_runway_takeoff.getTargetSP(), + false); + } + + /* update navigation: _runway_takeoff decides if we target the current WP from setpoint triplet + * or the calculated one through initial heading */ + _l1_control.navigate_waypoints(_runway_takeoff.getPrevWP(), _runway_takeoff.getCurrWP(curr_wp), current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); // update runway takeoff helper From 3b865624f1b302c256bf628de9013043d2df0a18 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 18 Sep 2015 08:42:08 +0200 Subject: [PATCH 22/58] added library for terrain_estimation Conflicts: makefiles/nuttx/config_aerocore_default.mk makefiles/nuttx/config_px4fmu-v1_default.mk makefiles/nuttx/config_px4fmu-v2_default.mk makefiles/nuttx/config_px4fmu-v2_multiplatform.mk makefiles/posix/config_posix_sitl.mk --- src/lib/terrain_estimation/module.mk | 36 ++++ .../terrain_estimation/terrain_estimator.cpp | 188 ++++++++++++++++++ .../terrain_estimation/terrain_estimator.h | 96 +++++++++ .../AttitudePositionEstimatorEKF.h | 3 + .../ekf_att_pos_estimator_main.cpp | 16 +- 5 files changed, 336 insertions(+), 3 deletions(-) create mode 100644 src/lib/terrain_estimation/module.mk create mode 100644 src/lib/terrain_estimation/terrain_estimator.cpp create mode 100644 src/lib/terrain_estimation/terrain_estimator.h diff --git a/src/lib/terrain_estimation/module.mk b/src/lib/terrain_estimation/module.mk new file mode 100644 index 0000000000..3a2047012a --- /dev/null +++ b/src/lib/terrain_estimation/module.mk @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +SRCS = terrain_estimator.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp new file mode 100644 index 0000000000..f522e42047 --- /dev/null +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file terrain_estimator.cpp + * A terrain estimation kalman filter. + */ + +#include "terrain_estimator.h" + +TerrainEstimator::TerrainEstimator() : + _distance_last(0.0f), + _terrain_valid(false), + _time_last_distance(0), + _time_last_gps(0) +{ + _x.zero(); + _u_z = 0.0f; + _P.identity(); +} + +bool TerrainEstimator::is_distance_valid(float distance) { + if (distance > 40.0f || distance < 0.00001f) { + return false; + } else { + return true; + } +} + +void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance) +{ + if (attitude->R_valid) { + math::Matrix<3, 3> R_att(attitude->R); + math::Vector<3> a(&sensor->accelerometer_m_s2[0]); + math::Vector<3> u; + u = R_att * a; + _u_z = u(2) + 9.81f; // compensate for gravity + + } else { + _u_z = 0.0f; + } + + // dynamics matrix + math::Matrix A; + A.zero(); + A(0,1) = 1; + A(1,2) = 1; + + // input matrix + math::Matrix B; + B.zero(); + B(1,0) = 1; + + // input noise variance + float R = 0.135f; + + // process noise convariance + math::Matrix Q; + Q(0,0) = 0; + Q(1,1) = 0; + + // do prediction + math::Vector dx = (A * _x) * dt; + dx(1) += B(1,0) * _u_z * dt; + + // propagate state and covariance matrix + _x += dx; + _P += (A * _P + _P * A.transposed() + + B * R * B.transposed() + Q) * dt; +} + +void TerrainEstimator::measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude) +{ + if (distance->timestamp > _time_last_distance) { + + float d = distance->current_distance; + + math::Matrix<1, n_x> C; + C(0, 0) = -1; // measured altitude, + + float R = 0.009f; + + math::Vector<1> y; + y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch); + + // residual + math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + S_I(0,0) += R; + S_I = S_I.inversed(); + math::Vector<1> r = y - C * _x; + + math::Matrix K = _P * C.transposed() * S_I; + + // some sort of outlayer rejection + if (fabsf(distance->current_distance - _distance_last) < 1.0f) { + _x += K * r; + _P -= K * C * _P; + } + + // if the current and the last range measurement are bad then we consider the terrain + // estimate to be invalid + if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) { + _terrain_valid = false; + } else { + _terrain_valid = true; + } + + _time_last_distance = distance->timestamp; + _distance_last = distance->current_distance; + } + + if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { + math::Matrix<1, n_x> C; + C(0,1) = 1; + + float R = 0.056f; + + math::Vector<1> y; + y(0) = gps->vel_d_m_s; + + // residual + math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + S_I(0,0) += R; + S_I = S_I.inversed(); + math::Vector<1> r = y - C * _x; + + math::Matrix K = _P * C.transposed() * S_I; + _x += K * r; + _P -= K * C * _P; + + _time_last_gps = gps->timestamp_position; + } + + // reinitialise filter if we find bad data + bool reinit = false; + for (int i = 0; i < n_x; i++) { + if (!PX4_ISFINITE(_x(i))) { + reinit = true; + } + } + + for (int i = 0; i < n_x; i++) { + for (int j = 0; j < n_x; j++) { + if (!PX4_ISFINITE(_P(i,j))) { + reinit = true; + } + } + } + + if (reinit) { + _x.zero(); + _P.zero(); + _P(0,0) = _P(1,1) = _P(2,2) = 0.1f; + } + +} diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h new file mode 100644 index 0000000000..981e621236 --- /dev/null +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file terrain_estimator.h + */ + +#include +#include +#include +#include +#include + + +/* +* This class can be used to estimate distance to the ground using a laser range finder. +* It's assumed that the laser points down vertically if the vehicle is in it's neutral pose. +* The predict(...) function will do a state prediciton based on accelerometer inputs. It also +* considers accelerometer bias. +* The measurement_update(...) function does a measurement update based on range finder and gps +* velocity measurements. Both functions should always be called together when there is new +* acceleration data available. +* The is_valid() function provides information wether the estimate is valid. +*/ + +class __EXPORT TerrainEstimator +{ +public: + TerrainEstimator(); + ~TerrainEstimator(); + + bool is_valid() {return _terrain_valid;} + float get_distance_to_ground() {return -_x(0);} + float get_velocity() {return _x(1);}; + + void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance); + void measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude); + +private: + enum {n_x=3}; + + float _distance_last; + bool _terrain_valid; + + // kalman filter variables + math::Vector _x; // state: ground distance, velocity, accel bias in z direction + float _u_z; // acceleration in earth z direction + math::Matrix<3, 3> _P; // covariance matrix + + // timestamps + uint64_t _time_last_distance; + uint64_t _time_last_gps; + + struct { + float var_acc_z; + float var_p_z; + float var_p_vz; + float var_lidar; + float var_gps_vz; + } _params; + + bool is_distance_valid(float distance); + +}; \ No newline at end of file diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 1429979d6d..fc11e0f505 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -69,6 +69,7 @@ #include #include +#include #include #include #include "estimator_22states.h" @@ -278,6 +279,8 @@ private: AttPosEKF *_ekf; + TerrainEstimator *_terrain_estimator; + /* Low pass filter for attitude rates */ math::LowPassFilter2p _LP_att_P; math::LowPassFilter2p _LP_att_Q; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index f4fcf78e25..2193b3e945 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -212,6 +212,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _parameters{}, _parameter_handles{}, _ekf(nullptr), + _terrain_estimator(nullptr), _LP_att_P(250.0f, 20.0f), _LP_att_Q(250.0f, 20.0f), @@ -219,6 +220,8 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : { _voter_mag.set_timeout(200000); + _terrain_estimator = new TerrainEstimator(); + _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS"); @@ -697,6 +700,10 @@ void AttitudePositionEstimatorEKF::task_main() // Run EKF data fusion steps updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); + // Run separate terrain estimator + _terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance); + _terrain_estimator->measurement_update(&_gps, &_distance, &_att); + // Publish attitude estimations publishAttitude(); @@ -997,9 +1004,12 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* terrain altitude */ - _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; - _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && - (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); + if (_terrain_estimator->is_valid()) { + _global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground(); + _global_pos.terrain_alt_valid = true; + } else { + _global_pos.terrain_alt_valid = false; + } _global_pos.yaw = _local_pos.yaw; From b16e6249e4189e077ce9d82973897a7698842181 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 11 Oct 2015 16:52:09 +0200 Subject: [PATCH 23/58] more correct groundspeed scaling for wheel controller --- src/lib/ecl/attitude_fw/ecl_controller.h | 3 ++- .../ecl/attitude_fw/ecl_wheel_controller.cpp | 19 +++++++------------ .../fw_att_control/fw_att_control_main.cpp | 13 ++++++++++--- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h index 0817204ccc..fed7d1d8fb 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_controller.h @@ -75,7 +75,8 @@ struct ECL_ControlData { float airspeed_max; float airspeed; float scaler; - float ground_speed; + float groundspeed; + float groundspeed_scaler; bool lock_integrator; }; diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index d67a46bb1c..76419ef6f7 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -60,7 +60,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da { /* Do not calculate control signal with bad inputs */ if (!(PX4_ISFINITE(ctl_data.yaw_rate) && - PX4_ISFINITE(ctl_data.ground_speed))) { + PX4_ISFINITE(ctl_data.groundspeed) && + PX4_ISFINITE(ctl_data.groundspeed_scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -79,21 +80,13 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da /* input conditioning */ float min_speed = 1.0f; - /* assume minimum speed to prevent oscillations */ - float speed = min_speed; - if (ctl_data.ground_speed > speed) { - speed = ctl_data.ground_speed; - } - - /* only scale a certain amount with speed else the corrections get to small */ - float scaler = 0.7f + 0.3f / speed; /* Calculate body angular rate error */ _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error - if (!lock_integrator && _k_i > 0.0f && speed > min_speed) { + if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) { - float id = _rate_error * dt * scaler; + float id = _rate_error * dt * ctl_data.groundspeed_scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -115,7 +108,9 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler; + _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + + _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + + integrator_constrained; /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/ diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index da91fda9f7..271dc86079 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -837,9 +837,16 @@ FixedwingAttitudeControl::task_main() * * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); + /* Use min airspeed to calculate ground speed scaling region. + * Don't scale below gspd_scaling_trim + */ + float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + + _global_pos.vel_e * _global_pos.vel_e); + float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); + float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); + float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; float yaw_sp = 0.0f; @@ -995,8 +1002,8 @@ FixedwingAttitudeControl::task_main() control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; - control_input.ground_speed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + - _global_pos.vel_e * _global_pos.vel_e); + control_input.groundspeed = groundspeed; + control_input.groundspeed_scaler = groundspeed_scaler; _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); From 62122201138e369459fada1c2aa957a5465c142b Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:46:00 +0200 Subject: [PATCH 24/58] replaced aileron mixer with flaperon mixer --- ROMFS/px4fmu_common/mixers/AERTW.main.mix | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTW.main.mix index af548826dc..a28dd9ec6e 100644 --- a/ROMFS/px4fmu_common/mixers/AERTW.main.mix +++ b/ROMFS/px4fmu_common/mixers/AERTW.main.mix @@ -10,22 +10,23 @@ to output 3 and the wheel to output 4. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust). -Aileron mixer -------------- -Two scalers total (output, roll). +Flaperon mixer (ailerons + flaps) +--------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; depending on the actual configuration it may be necessary to reverse the scaling factors (to reverse the servo movement) and adjust the offset, scaling and endpoints to suit. -As there is only one output, if using two servos adjustments to compensate for -differences between the servos must be made mechanically. To obtain the correct -motion using a Y cable, the servos can be positioned reversed from one another. - -M: 1 +M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 -10000 -10000 0 -10000 10000 Elevator mixer ------------ From 0043c40b463c1e05703fcdadcbd5aba6cc535af6 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:46:51 +0200 Subject: [PATCH 25/58] added more indexing variables --- msg/actuator_controls.msg | 3 +++ 1 file changed, 3 insertions(+) diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index 66e12325d2..a6ebda6aae 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -5,6 +5,9 @@ uint8 INDEX_PITCH = 1 uint8 INDEX_YAW = 2 uint8 INDEX_THROTTLE = 3 uint8 INDEX_FLAPS = 4 +uint8 INDEX_SPOILERS = 5 +uint8 INDEX_AIRBRAKES = 6 +uint8 INDEX_LANDING_GEAR = 7 uint8 GROUP_INDEX_ATTITUDE = 0 uint64 timestamp uint64 timestamp_sample # the timestamp the data this control response is based on was sampled From 18d9c061ba5b78b2c96ac78bd07b1e0ec5cfa06c Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:47:26 +0200 Subject: [PATCH 26/58] added flag for applying flaps --- msg/vehicle_attitude_setpoint.msg | 2 ++ 1 file changed, 2 insertions(+) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 3bbb5efa7b..43ea237b47 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -23,3 +23,5 @@ bool pitch_reset_integral # Reset pitch integral part (navigation logic change bool yaw_reset_integral # Reset yaw integral part (navigation logic change) bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) + +bool apply_flaps From 7fc97ed147222668d996815e0feab887f9abf7ed Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:47:50 +0200 Subject: [PATCH 27/58] implemented use of flaps for auto landings --- .../fw_att_control/fw_att_control_main.cpp | 30 +++++++++++++++++-- .../fw_att_control/fw_att_control_params.c | 18 +++++++++++ .../fw_pos_control_l1_main.cpp | 6 +++- 3 files changed, 50 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 271dc86079..579096268e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -202,6 +202,9 @@ private: float man_roll_max; /**< Max Roll in rad */ float man_pitch_max; /**< Max Pitch in rad */ + float flaps_scale; /**< Scale factor for flaps */ + float flaperon_scale; /**< Scale factor for flaperons */ + int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ } _parameters; /**< local copies of interesting parameters */ @@ -246,6 +249,9 @@ private: param_t man_roll_max; param_t man_pitch_max; + param_t flaps_scale; + param_t flaperon_scale; + param_t vtol_type; } _parameter_handles; /**< handles for interesting parameters */ @@ -414,6 +420,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + _parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL"); + _parameter_handles.flaperon_scale = param_find("FW_FLAPERONS_SCL"); + _parameter_handles.vtol_type = param_find("VT_TYPE"); /* fetch initial parameter values */ @@ -498,6 +507,9 @@ FixedwingAttitudeControl::parameters_update() _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale); + param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale); + param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); /* pitch control parameters */ @@ -810,8 +822,20 @@ FixedwingAttitudeControl::task_main() float flaps_control = 0.0f; /* map flaps by default to manual if valid */ - if (PX4_ISFINITE(_manual.flaps)) { - flaps_control = _manual.flaps; + if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { + flaps_control = _manual.flaps * _parameters.flaps_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { + flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; + } + + /* default flaperon to center */ + float flaperon = 0.0f; + + /* map flaperons by default to manual if valid */ + if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { + flaperon = _manual.aux2 * _parameters.flaperon_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { + flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; } /* decide if in stabilized or full manual control */ @@ -1119,7 +1143,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control; _actuators.control[5] = _manual.aux1; - _actuators.control[6] = _manual.aux2; + _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index f4cfa7fd8d..41ca2edf44 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -434,3 +434,21 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); + +/** + * Scale factor for flaps + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f); + +/** + * Scale factor for flaperons + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 176d4ed11b..0727c718e0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1076,7 +1076,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder - + _att_sp.apply_flaps = false; // by default we don't use flaps float eas2tas = 1.0f; // XXX calculate actual number based on current measurements /* filter speed and altitude for controller */ @@ -1207,6 +1207,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + // apply full flaps for landings. this flag will also trigger the use of flaperons + // if they have been enabled using the corresponding parameter + _att_sp.apply_flaps = true; + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); From 42d03cb07695d832f06abeda448212714fa35dfe Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:59:35 +0200 Subject: [PATCH 28/58] activate wheel controller as soon as plane flares --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0727c718e0..8d8409445d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1291,6 +1291,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; + /* enable direct yaw control using rudder/wheel */ + _att_sp.yaw_body = target_bearing; + _att_sp.fw_control_yaw = true; + if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { From f3e0d91f24ed30b32647a10b30b82f1bda9b29f5 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 14 Oct 2015 07:47:39 +0200 Subject: [PATCH 29/58] added airspeed scale parameter for takeoff and landing --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++++-- .../fw_pos_control_l1/fw_pos_control_l1_params.c | 12 ++++++++++++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 8d8409445d..121eae8af5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -281,6 +281,7 @@ private: float land_flare_pitch_min_deg; float land_flare_pitch_max_deg; int land_use_terrain_estimate; + float land_airspeed_scale; } _parameters; /**< local copies of interesting parameters */ @@ -329,6 +330,7 @@ private: param_t land_flare_pitch_min_deg; param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; + param_t land_airspeed_scale; } _parameter_handles; /**< handles for interesting parameters */ @@ -572,6 +574,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); + _parameter_handles.land_airspeed_scale = param_find("FW_AIRSPD_SCALE"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -675,6 +678,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); + param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -1263,8 +1267,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX this could make a great param float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = 1.3f * _parameters.airspeed_min; - float airspeed_approach = 1.3f * _parameters.airspeed_min; + float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min; + float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min; /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be * equal to _pos_sp_triplet.current.alt */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 5870039c24..14b73038a0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -443,3 +443,15 @@ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); * */ PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f); + +/** + * Takeoff and landing airspeed scale factor + * + * Multiplying this factor with the minimum airspeed of the plane + * gives the target airspeed for takeoff and landing approach. + * + * @min 1.0 + * @max 1.5 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_SCALE, 1.3f); From b30091be00edc5e9994cf12bc08d644d4c0f8d3b Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 14 Oct 2015 11:06:10 +0200 Subject: [PATCH 30/58] minor fixes --- ROMFS/px4fmu_common/init.d/2105_maja | 2 +- ROMFS/px4fmu_common/mixers/AERTW.main.mix | 8 ++++---- src/modules/fw_att_control/fw_att_control_main.cpp | 6 +++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja index af27eca5d9..2cbc11c275 100644 --- a/ROMFS/px4fmu_common/init.d/2105_maja +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -42,7 +42,7 @@ fi set MIXER AERTW # use PWM parameters for throttle channel -set PWM_OUT 4 +set PWM_OUT 5 set PWM_DISARMED p:PWM_DISARMED set PWM_MIN p:PWM_MIN set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTW.main.mix index a28dd9ec6e..49665c5110 100644 --- a/ROMFS/px4fmu_common/mixers/AERTW.main.mix +++ b/ROMFS/px4fmu_common/mixers/AERTW.main.mix @@ -83,14 +83,14 @@ Gimbal / flaps / payload mixer for last three channels, using the payload control group ----------------------------------------------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + M: 1 O: 10000 10000 0 -10000 10000 S: 2 0 10000 10000 0 -10000 10000 -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 1 10000 10000 0 -10000 10000 - M: 1 O: 10000 10000 0 -10000 10000 S: 2 2 10000 10000 0 -10000 10000 diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 579096268e..7600aca22b 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -421,7 +421,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); _parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL"); - _parameter_handles.flaperon_scale = param_find("FW_FLAPERONS_SCL"); + _parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL"); _parameter_handles.vtol_type = param_find("VT_TYPE"); @@ -823,7 +823,7 @@ FixedwingAttitudeControl::task_main() /* map flaps by default to manual if valid */ if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { - flaps_control = _manual.flaps * _parameters.flaps_scale; + flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale; } else if (_vcontrol_mode.flag_control_auto_enabled) { flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; } @@ -833,7 +833,7 @@ FixedwingAttitudeControl::task_main() /* map flaperons by default to manual if valid */ if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { - flaperon = _manual.aux2 * _parameters.flaperon_scale; + flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale; } else if (_vcontrol_mode.flag_control_auto_enabled) { flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; } From 36df3a04996e0d6e69d02e8ae59768ce25aab416 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 14 Oct 2015 12:55:44 +0200 Subject: [PATCH 31/58] fixed minor things from review --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 52 +++++++++++++++++++++--- src/lib/runway_takeoff/RunwayTakeoff.h | 2 +- 2 files changed, 47 insertions(+), 7 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 16cbb04e45..90e3601e69 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -47,7 +47,6 @@ #include #include #include -#include namespace runwaytakeoff { @@ -61,6 +60,7 @@ RunwayTakeoff::RunwayTakeoff() : _climbout(false), _start_sp{}, _target_sp{}, + _throttle_ramp_time(2 * 1e6), _runway_takeoff_enabled(this, "TKOFF"), _heading_mode(this, "HDG"), _nav_alt(this, "NAV_ALT"), @@ -87,7 +87,7 @@ void RunwayTakeoff::init(float yaw) _initialized = true; _state = RunwayTakeoffState::THROTTLE_RAMP; _initialized_time = hrt_absolute_time(); - _climbout = true; + _climbout = true; // this is true until climbout is finished } void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) @@ -95,7 +95,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: - if (hrt_elapsed_time(&_initialized_time) > 1e6) { + if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) { _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; } @@ -131,12 +131,21 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) } } +/* + * Returns true as long as we're below navigation altitude + */ bool RunwayTakeoff::controlYaw() { // keep controlling yaw directly until we start navigation return _state < RunwayTakeoffState::CLIMBOUT; } +/* + * Returns pitch setpoint to use. + * + * Limited (parameter) as long as the plane is on runway. Otherwise + * use the one from TECS + */ float RunwayTakeoff::getPitch(float tecsPitch) { if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { @@ -146,6 +155,9 @@ float RunwayTakeoff::getPitch(float tecsPitch) return tecsPitch; } +/* + * Returns the roll setpoint to use. + */ float RunwayTakeoff::getRoll(float navigatorRoll) { // until we have enough ground clearance, set roll to 0 @@ -163,16 +175,25 @@ float RunwayTakeoff::getRoll(float navigatorRoll) return navigatorRoll; } +/* + * Returns the yaw setpoint to use. + */ float RunwayTakeoff::getYaw(float navigatorYaw) { return navigatorYaw; } +/* + * Returns the throttle setpoint to use. + * + * Ramps up in the beginning, until it lifts off the runway it is set to + * parameter value, then it returns the TECS throttle. + */ float RunwayTakeoff::getThrottle(float tecsThrottle) { switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: { - float throttle = hrt_elapsed_time(&_initialized_time) / (float)2000000 * + float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time * _takeoff_throttle.get(); return throttle < _takeoff_throttle.get() ? throttle : @@ -193,9 +214,16 @@ bool RunwayTakeoff::resetIntegrators() return _state < RunwayTakeoffState::TAKEOFF; } +/* + * Returns the minimum pitch for TECS to use. + * + * In climbout we either want what was set on the waypoint (sp_min) but at least + * the climbtout minimum pitch (parameter). + * Otherwise use the minimum that is enforced generally (parameter). + */ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) { - if (_climbout) { + if (_state < RunwayTakeoffState::FLY) { return math::max(sp_min, climbout_min); } @@ -204,9 +232,15 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) } } +/* + * Returns the maximum pitch for TECS to use. + * + * Limited by parameter (if set) until climbout is done. + */ float RunwayTakeoff::getMaxPitch(float max) { - if (_climbout && _max_takeoff_pitch.get() > 0.1f) { + // use max pitch from parameter if set (> 0.1) + if (_state < RunwayTakeoffState::FLY && _max_takeoff_pitch.get() > 0.1f) { return _max_takeoff_pitch.get(); } @@ -215,6 +249,9 @@ float RunwayTakeoff::getMaxPitch(float max) } } +/* + * Returns the "previous" (start) WP for navigation. + */ math::Vector<2> RunwayTakeoff::getPrevWP() { math::Vector<2> prev_wp; @@ -223,6 +260,9 @@ math::Vector<2> RunwayTakeoff::getPrevWP() return prev_wp; } +/* + * Returns the "current" (target) WP for navigation. + */ math::Vector<2> RunwayTakeoff::getCurrWP(math::Vector<2> sp_curr_wp) { if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::FLY) { diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 2913b2ad64..cf0ab455d8 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -50,7 +50,6 @@ #include #include #include -#include namespace runwaytakeoff { @@ -105,6 +104,7 @@ private: bool _climbout; struct position_setpoint_s _start_sp; struct position_setpoint_s _target_sp; + unsigned _throttle_ramp_time; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; From 1ae722159332da0fa0abf1cbd939ea0670e6b2fe Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 14 Oct 2015 13:56:16 +0200 Subject: [PATCH 32/58] make flaps and flaperons continuous --- .../fw_att_control/fw_att_control_main.cpp | 44 +++++++++++++++++-- 1 file changed, 41 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 7600aca22b..3e7a9f9aa8 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -161,6 +161,10 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ bool _debug; /**< if set to true, print debug output */ + float _flaps_cmd_last; + float _flaperons_cmd_last; + + struct { float tconst; float p_p; @@ -363,7 +367,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ _setpoint_valid(false), - _debug(false) + _debug(false), + _flaps_cmd_last(0), + _flaperons_cmd_last(0) { /* safely initialize structs */ _ctrl_state = {}; @@ -821,6 +827,8 @@ FixedwingAttitudeControl::task_main() /* default flaps to center */ float flaps_control = 0.0f; + static float delta_flaps = 0; + /* map flaps by default to manual if valid */ if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale; @@ -828,9 +836,25 @@ FixedwingAttitudeControl::task_main() flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; } + // move the actual control value continuous with time + static hrt_abstime t_flaps_changed = 0; + if (fabsf(flaps_control - _flaps_cmd_last) > 0.01f) { + t_flaps_changed = hrt_absolute_time(); + delta_flaps = flaps_control - _flaps_cmd_last; + _flaps_cmd_last = flaps_control; + } + + static float flaps_applied = 0.0f; + + if (fabsf(flaps_applied - flaps_control) > 0.01f) { + flaps_applied = (flaps_control - delta_flaps) + (float)hrt_elapsed_time(&t_flaps_changed) * (delta_flaps) / 1000000; + } + /* default flaperon to center */ float flaperon = 0.0f; + static float delta_flaperon = 0.0f; + /* map flaperons by default to manual if valid */ if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale; @@ -838,6 +862,20 @@ FixedwingAttitudeControl::task_main() flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; } + // move the actual control value continuous with time + static hrt_abstime t_flaperons_changed = 0; + if (fabsf(flaperon - _flaperons_cmd_last) > 0.01f) { + t_flaperons_changed = hrt_absolute_time(); + delta_flaperon = flaperon - _flaperons_cmd_last; + _flaperons_cmd_last = flaperon; + } + + static float flaperon_applied = 0.0f; + + if (fabsf(flaperon_applied - flaperon) > 0.01f) { + flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * (delta_flaperon) / 1000000; + } + /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_attitude_enabled) { /* scale around tuning airspeed */ @@ -1141,9 +1179,9 @@ FixedwingAttitudeControl::task_main() _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; } - _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control; + _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_applied; _actuators.control[5] = _manual.aux1; - _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon; + _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon_applied; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ From 11c6ee2b5a9115b0b0e6727bd6f2df574ad447f3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 14 Oct 2015 17:14:38 +0200 Subject: [PATCH 33/58] make terrain estimate invalid after range sensor timeout --- src/lib/terrain_estimation/terrain_estimator.cpp | 9 ++++++++- src/lib/terrain_estimation/terrain_estimator.h | 2 +- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index f522e42047..16ab8de4a0 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -38,6 +38,8 @@ #include "terrain_estimator.h" +#define DISTANCE_TIMEOUT 100000 // time in usec after which laser is considered dead + TerrainEstimator::TerrainEstimator() : _distance_last(0.0f), _terrain_valid(false), @@ -100,9 +102,14 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu B * R * B.transposed() + Q) * dt; } -void TerrainEstimator::measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, +void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, const struct vehicle_attitude_s *attitude) { + // terrain estimate is invalid if we have range sensor timeout + if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) { + _terrain_valid = false; + } + if (distance->timestamp > _time_last_distance) { float d = distance->current_distance; diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index 981e621236..fc10cb8d63 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -65,7 +65,7 @@ public: void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, const struct distance_sensor_s *distance); - void measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, + void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, const struct vehicle_attitude_s *attitude); private: diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 2193b3e945..e4c8eeccde 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -702,7 +702,7 @@ void AttitudePositionEstimatorEKF::task_main() // Run separate terrain estimator _terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance); - _terrain_estimator->measurement_update(&_gps, &_distance, &_att); + _terrain_estimator->measurement_update(hrt_absolute_time(), &_gps, &_distance, &_att); // Publish attitude estimations publishAttitude(); From 5949b6615d52fe694f3f2a8824d19838158fd572 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 14 Oct 2015 18:08:35 +0200 Subject: [PATCH 34/58] don't reset the yaw integrator on takeoff --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 121eae8af5..02a61a20e7 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1443,9 +1443,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); + // reset integrals except yaw (which also counts for the wheel controller) _att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators(); _att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators(); - _att_sp.yaw_reset_integral = _runway_takeoff.resetIntegrators(); /*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body, (double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/ From 4e22d65325cc34423e0259402de3a720d71e78f1 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 14 Oct 2015 22:01:09 +0200 Subject: [PATCH 35/58] don't use virtual line anymore for takeoff but use correct starting point to navigate, updated default parameters for wheel controller --- .../ecl/attitude_fw/ecl_wheel_controller.cpp | 4 +- src/lib/runway_takeoff/RunwayTakeoff.cpp | 58 +++++++++---------- src/lib/runway_takeoff/RunwayTakeoff.h | 12 ++-- .../fw_att_control/fw_att_control_params.c | 4 +- .../fw_pos_control_l1_main.cpp | 32 +++------- 5 files changed, 45 insertions(+), 65 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index 76419ef6f7..7f8a9ca6c7 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -111,8 +111,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + integrator_constrained; - /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f", - (double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/ + /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f", + (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/ return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 90e3601e69..46df5cd744 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -58,9 +58,8 @@ RunwayTakeoff::RunwayTakeoff() : _initialized_time(0), _init_yaw(0), _climbout(false), - _start_sp{}, - _target_sp{}, _throttle_ramp_time(2 * 1e6), + _start_wp(), _runway_takeoff_enabled(this, "TKOFF"), _heading_mode(this, "HDG"), _nav_alt(this, "NAV_ALT"), @@ -81,16 +80,19 @@ RunwayTakeoff::~RunwayTakeoff() } -void RunwayTakeoff::init(float yaw) +void RunwayTakeoff::init(float yaw, double current_lat, double current_lon) { _init_yaw = yaw; _initialized = true; _state = RunwayTakeoffState::THROTTLE_RAMP; _initialized_time = hrt_absolute_time(); _climbout = true; // this is true until climbout is finished + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; } -void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) +void RunwayTakeoff::update(float airspeed, float alt_agl, + double current_lat, double current_lon, int mavlink_fd) { switch (_state) { @@ -112,6 +114,16 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::TAKEOFF: if (alt_agl > _nav_alt.get()) { _state = RunwayTakeoffState::CLIMBOUT; + + /* + * If we started in heading hold mode, move the navigation start WP to the current location now. + * The navigator will take this as starting point to navigate towards the takeoff WP. + */ + if (_heading_mode.get() == 0) { + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; + } + mavlink_log_info(mavlink_fd, "#Climbout"); } @@ -127,7 +139,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; default: - return; + break; } } @@ -177,10 +189,18 @@ float RunwayTakeoff::getRoll(float navigatorRoll) /* * Returns the yaw setpoint to use. + * + * 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 RunwayTakeoff::getYaw(float navigatorYaw) { - return navigatorYaw; + if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) { + return _init_yaw; + + } else { + return navigatorYaw; + } } /* @@ -252,31 +272,9 @@ float RunwayTakeoff::getMaxPitch(float max) /* * Returns the "previous" (start) WP for navigation. */ -math::Vector<2> RunwayTakeoff::getPrevWP() +math::Vector<2> RunwayTakeoff::getStartWP() { - math::Vector<2> prev_wp; - prev_wp(0) = (float)_start_sp.lat; - prev_wp(1) = (float)_start_sp.lon; - return prev_wp; -} - -/* - * Returns the "current" (target) WP for navigation. - */ -math::Vector<2> RunwayTakeoff::getCurrWP(math::Vector<2> sp_curr_wp) -{ - if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::FLY) { - // navigating towards calculated direction if heading mode 0 and as long as we're in climbout - math::Vector<2> curr_wp; - curr_wp(0) = (float)_target_sp.lat; - curr_wp(1) = (float)_target_sp.lon; - return curr_wp; - - } else { - // navigating towards next mission waypoint - return sp_curr_wp; - } - + return _start_wp; } void RunwayTakeoff::reset() diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index cf0ab455d8..0e1c5ed14b 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -68,16 +68,14 @@ public: RunwayTakeoff(); ~RunwayTakeoff(); - void init(float yaw); - void update(float airspeed, float alt_agl, int mavlink_fd); + void init(float yaw, double current_lat, double current_lon); + void update(float airspeed, float alt_agl, double current_lat, double current_lon, int mavlink_fd); RunwayTakeoffState getState() { return _state; }; bool isInitialized() { return _initialized; }; bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; - position_setpoint_s *getStartSP() { return &_start_sp; }; - position_setpoint_s *getTargetSP() { return &_target_sp; }; float getInitYaw() { return _init_yaw; }; bool controlYaw(); @@ -89,8 +87,7 @@ public: bool resetIntegrators(); float getMinPitch(float sp_min, float climbout_min, float min); float getMaxPitch(float max); - math::Vector<2> getPrevWP(); - math::Vector<2> getCurrWP(math::Vector<2> sp_curr_wp); + math::Vector<2> getStartWP(); void reset(); @@ -102,9 +99,8 @@ private: hrt_abstime _initialized_time; float _init_yaw; bool _climbout; - struct position_setpoint_s _start_sp; - struct position_setpoint_s _target_sp; unsigned _throttle_ramp_time; + math::Vector<2> _start_wp; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 41ca2edf44..ce2db9ad5c 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f); /** * Wheel steering rate integrator gain @@ -254,7 +254,7 @@ PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_IMAX, 0.2f); +PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f); /** * Maximum wheel steering rate diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 02a61a20e7..45032d4735 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1375,45 +1375,31 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { - _runway_takeoff.init(_att.yaw); + _runway_takeoff.init(_att.yaw, _global_pos.lat, _global_pos.lon); /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; - // draw a line from takeoff location into the direction the UAV is heading - get_waypoint_heading_distance( - _runway_takeoff.getInitYaw(), - HDG_HOLD_DIST_NEXT, - *_runway_takeoff.getStartSP(), - *_runway_takeoff.getTargetSP(), - true); - mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); } - // update takeoff path if we're reaching the end of it - if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, - _runway_takeoff.getTargetSP()->lat, _runway_takeoff.getTargetSP()->lon) < HDG_HOLD_REACHED_DIST) { - get_waypoint_heading_distance( - _runway_takeoff.getInitYaw(), - HDG_HOLD_DIST_NEXT, - *_runway_takeoff.getStartSP(), - *_runway_takeoff.getTargetSP(), - false); - } - - /* update navigation: _runway_takeoff decides if we target the current WP from setpoint triplet - * or the calculated one through initial heading */ - _l1_control.navigate_waypoints(_runway_takeoff.getPrevWP(), _runway_takeoff.getCurrWP(curr_wp), current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); // update runway takeoff helper _runway_takeoff.update( _airspeed.true_airspeed_m_s, _global_pos.alt - terrain_alt, + _global_pos.lat, + _global_pos.lon, _mavlink_fd); + /* + * Update navigation: _runway_takeoff returns the start WP according to mode and phase. + * If we use the navigator heading or not is decided later. + */ + _l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, ground_speed_2d); + // update tecs float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); From 73b1c186989f2c11426badb7a19e6417688efab1 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 13:49:22 +0200 Subject: [PATCH 36/58] do not stick to terrain estimate if it's not valid --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 45032d4735..b608ea7f9e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -963,7 +963,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ - if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { + if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) { if(!land_useterrain) { mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate"); land_useterrain = true; From 0bd23dd7c5511446451a491424ea58d296b9ca4b Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 13:56:53 +0200 Subject: [PATCH 37/58] only go into heading hold mode after flaring --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index b608ea7f9e..122ce0fdda 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1227,8 +1227,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi wp_distance_save = 0.0f; } - //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { + // we want the plane to keep tracking the desired flight path until we start flaring + // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds + if (land_noreturn_vertical) { /* heading hold, along the line connecting this and the last waypoint */ From 2a2e2a27d321146aa06ae7588c5d2f3cad1375d2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 14:55:32 +0200 Subject: [PATCH 38/58] update comments --- ROMFS/px4fmu_common/mixers/AERTW.main.mix | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTW.main.mix index 49665c5110..2ab5bdb611 100644 --- a/ROMFS/px4fmu_common/mixers/AERTW.main.mix +++ b/ROMFS/px4fmu_common/mixers/AERTW.main.mix @@ -1,16 +1,16 @@ -Aileron/rudder/elevator/throttle/wheel mixer for PX4FMU +Aileron/rudder/elevator/throttle/wheel/flaps mixer for PX4FMU ======================================================= This file defines mixers suitable for controlling a fixed wing aircraft with aileron, rudder, elevator, throttle and steerable wheel controls using PX4FMU. The configuration assumes the aileron servo(s) are connected to PX4FMU servo -output 0, the elevator to output 1, the rudder to output 2, the throttle -to output 3 and the wheel to output 4. +output 0 and 1, the elevator to output 2, the rudder to output 3, the throttle +to output 4 and the wheel to output 5. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch), 2 (yaw) and 3 (thrust). +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). -Flaperon mixer (ailerons + flaps) +Aileron mixer (roll + flaperon) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -79,7 +79,7 @@ O: 10000 10000 0 -10000 10000 S: 0 2 10000 10000 0 -10000 10000 -Gimbal / flaps / payload mixer for last three channels, +Flaps / gimbal / payload mixer for last three channels, using the payload control group ----------------------------------------------------- From 8fa22c2efa7494956d32e8cd27961adc2c9de0c6 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 14:57:34 +0200 Subject: [PATCH 39/58] renamed mixer file --- ROMFS/px4fmu_common/mixers/{AERTW.main.mix => AERTWF.main.mix} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename ROMFS/px4fmu_common/mixers/{AERTW.main.mix => AERTWF.main.mix} (100%) diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTWF.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/AERTW.main.mix rename to ROMFS/px4fmu_common/mixers/AERTWF.main.mix From 5f400946855dc3cac6e1faf133a16018bed85588 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 16:10:33 +0200 Subject: [PATCH 40/58] fixed bug which lead to direct yaw control in stabilized mode --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 3e7a9f9aa8..8772ceb835 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -800,6 +800,10 @@ FixedwingAttitudeControl::task_main() vehicle_status_poll(); + // the position controller will not emit attitude setpoints in some modes + // we need to make sure that this flag is reset + _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; + /* lock integrator until control is started */ bool lock_integrator; From 3d3398e33026bb5c12157d1490805a768313dd0e Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 27 Oct 2015 16:39:33 +0100 Subject: [PATCH 41/58] added code handling aborting landings --- .../fw_pos_control_l1_main.cpp | 73 ++++++++++++++++++- src/modules/navigator/navigator_main.cpp | 11 ++- 2 files changed, 78 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 122ce0fdda..61d29494db 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -105,6 +105,7 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_YAWRATE_THRESH 0.15f // max yawrate at which plane locks yaw for heading hold mode #define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading #define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) +#define T_ALT_TIMEOUT 1 // time after which we abort landing if terrain estimate is not valid static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode @@ -196,6 +197,11 @@ private: bool land_onslope; bool land_useterrain; + // terrain estimation states + float _t_alt_prev_valid; //**< last terrain estimate which was valid */ + hrt_abstime _time_last_t_alt; //*< time at which we had last valid terrain alt */ + hrt_abstime _time_started_landing; //*< time at which landing started */ + bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ @@ -530,6 +536,9 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), land_useterrain(false), + _t_alt_prev_valid(0), + _time_last_t_alt(0), + _time_started_landing(0), _was_in_air(false), _time_went_in_air(0), _runway_takeoff(), @@ -1198,13 +1207,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - if (in_takeoff_situation()) { + float alt_sp; + if (_nav_capabilities.abort_landing == true) { + // if we entered loiter due to an aborted landing, demand + // altitude setpoint well above landing waypoint + alt_sp = _pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff; + } else { + // use altitude given by wapoint + alt_sp = _pos_sp_triplet.current.alt; + } + + if (in_takeoff_situation() || + ((_global_pos.alt < _pos_sp_triplet.current.alt + _parameters.climbout_diff ) && _nav_capabilities.abort_landing == true)) { /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); @@ -1215,6 +1235,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // if they have been enabled using the corresponding parameter _att_sp.apply_flaps = true; + // save time at which we started landing + if (_time_started_landing == 0) { + _time_started_landing = hrt_absolute_time(); + } + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -1273,7 +1298,40 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be * equal to _pos_sp_triplet.current.alt */ - float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos); + float terrain_alt; + if (_parameters.land_use_terrain_estimate) { + if (_global_pos.terrain_alt_valid) { + // all good, have valid terrain altitude + terrain_alt = _global_pos.terrain_alt; + _t_alt_prev_valid = terrain_alt; + _time_last_t_alt = hrt_absolute_time(); + } else if (_time_last_t_alt == 0) { + // we have started landing phase but don't have valid terrain + // wait for some time, maybe we will soon get a valid estimate + // until then just use the altitude of the landing waypoint + if (hrt_elapsed_time(&_time_started_landing) < 5 * 1000 * 1000) { + terrain_alt = _pos_sp_triplet.current.alt; + } else { + // still no valid terrain, abort landing + terrain_alt = _pos_sp_triplet.current.alt; + _nav_capabilities.abort_landing = true; + } + } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) + || land_noreturn_vertical) { + // use previous terrain estimate for some time and hope to recover + // if we are already flaring (land_noreturn_vertical) then just + // go with the old estimate + terrain_alt = _t_alt_prev_valid; + } else { + // terrain alt was not valid for long time, abort landing + terrain_alt = _t_alt_prev_valid; + _nav_capabilities.abort_landing = true; + } + } else { + // no terrain estimation, just use landing waypoint altitude + terrain_alt = _pos_sp_triplet.current.alt; + } + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_altitude_rel = _pos_sp_triplet.previous.valid ? @@ -1720,9 +1778,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /** MANUAL FLIGHT **/ - /* reset hold altitude */ + // reset hold altitude _hold_alt = _global_pos.alt; + // reset terrain estimation relevant values + _time_started_landing = 0; + _time_last_t_alt = 0; + + // reset lading abort state + _nav_capabilities.abort_landing = false; + /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e44b8d0be5..b056e3fd99 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -440,8 +440,15 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_mission; + if (_nav_caps.abort_landing) { + // pos controller aborted landing, requests loiter + // above landing waypoint + _navigation_mode = &_loiter; + _pos_sp_triplet_published_invalid_once = false; + } else { + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_mission; + } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; From 0f03ae12d79f062a65de76ab6690def0fd1ac6f8 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 07:03:45 +0100 Subject: [PATCH 42/58] added field for aborted fw landings --- msg/navigation_capabilities.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/navigation_capabilities.msg b/msg/navigation_capabilities.msg index 235b5df03b..6d12aaaed9 100644 --- a/msg/navigation_capabilities.msg +++ b/msg/navigation_capabilities.msg @@ -3,3 +3,4 @@ float32 turn_distance # the optimal distance to a waypoint to switch to the nex float32 landing_horizontal_slope_displacement float32 landing_slope_angle_rad float32 landing_flare_length +bool abort_landing From 896dff40cfbee4e93bb91d4c988bcceab32d9d27 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 08:02:51 +0100 Subject: [PATCH 43/58] added geo functions to create new waypoints from given setting --- src/lib/geo/geo.c | 25 +++++++++++++++++++++++++ src/lib/geo/geo.h | 6 ++++++ 2 files changed, 31 insertions(+) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index a4b1599a9d..07d765c3bc 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -298,6 +298,31 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou return CONSTANTS_RADIUS_OF_EARTH * c; } +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target) +{ + float heading; + if (fabsf(dist) < FLT_EPSILON) { + *lat_target = lat_A; + *lon_target = lon_A; + } + else if (dist >= FLT_EPSILON) { + heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } else { + heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + heading = _wrap_2pi(heading + 180.0f * M_PI_F); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } +} + +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *lat_end, double *lon_end) +{ + bearing = _wrap_2pi(bearing); + double radius_ratio = (double)(dist / CONSTANTS_RADIUS_OF_EARTH); + + *lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing)); + *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); +} __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { double lat_now_rad = lat_now * M_DEG_TO_RAD; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index f77a8b58a4..f9250fb5a3 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -236,6 +236,12 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al */ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +// TODO put description for both functions and improve naming +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target); + +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *end_lat, double *end_lon); + /** * Returns the bearing to the next waypoint in radians. * From 7f0c3a9b71ada3b422bbd8c339ba7696cd39beb3 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 08:16:30 +0100 Subject: [PATCH 44/58] use virtual setpoint for landing line tracking --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 61d29494db..e08af118e9 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1252,6 +1252,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi wp_distance_save = 0.0f; } + // create virtual waypoint which is on the desired flight path but + // some distance behind landing waypoint. This will make sure that the plane + // will always follow the desired flight path even if we get close or past + // the landing waypoint + math::Vector<2> curr_wp_shifted; + double lat; + double lon; + create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); + curr_wp_shifted(0) = (float)lat; + curr_wp_shifted(1) = (float)lon; // we want the plane to keep tracking the desired flight path until we start flaring // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds if (land_noreturn_vertical) { @@ -1276,7 +1286,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _l1_control.navigate_waypoints(prev_wp, curr_wp_shifted, current_position, ground_speed_2d); } _att_sp.roll_body = _l1_control.nav_roll(); From 6f4c8d45ff5624f341a8ce017faca86bae47607a Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 10:37:30 +0100 Subject: [PATCH 45/58] during flare control pitch setpoint based on distance to ground --- .../fw_pos_control_l1_main.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e08af118e9..7a98859f81 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -197,10 +197,11 @@ private: bool land_onslope; bool land_useterrain; - // terrain estimation states + // landing relevant states float _t_alt_prev_valid; //**< last terrain estimate which was valid */ hrt_abstime _time_last_t_alt; //*< time at which we had last valid terrain alt */ hrt_abstime _time_started_landing; //*< time at which landing started */ + float height_flare; //*< estimated height to ground at which flare started */ bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ @@ -539,6 +540,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _t_alt_prev_valid(0), _time_last_t_alt(0), _time_started_landing(0), + height_flare(0.0f), _was_in_air(false), _time_went_in_air(0), _runway_takeoff(), @@ -1396,10 +1398,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!land_noreturn_vertical) { + // just started with the flaring phase + _att_sp.pitch_body = 0.0f; + height_flare = _global_pos.alt - terrain_alt; mavlink_log_info(_mavlink_fd, "#Landing, flaring"); land_noreturn_vertical = true; + } else { + if (_global_pos.vel_d > 0.1f) { + _att_sp.pitch_body = _parameters.land_flare_pitch_min_deg * (height_flare - (_global_pos.alt - terrain_alt)) / height_flare; + } else { + _att_sp.pitch_body = _att_sp.pitch_body; + } } - //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); flare_curve_alt_rel_last = flare_curve_alt_rel; } else { @@ -1852,7 +1862,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && (launch_detection_state == LAUNCHDETECTION_RES_NONE || - _runway_takeoff.runwayTakeoffEnabled()) + _runway_takeoff.runwayTakeoffEnabled() || + land_noreturn_vertical) )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } From 5b1c7321e732a20e39dc52bc0ce174c23ea858ad Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 28 Oct 2015 14:08:42 +0100 Subject: [PATCH 46/58] reverted navigation on landing back to old heading hold --- src/lib/geo/geo.c | 4 ++-- .../fw_pos_control_l1_main.cpp | 20 +++++++++++-------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 07d765c3bc..0d09e8526b 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -310,7 +310,7 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } else { heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); - heading = _wrap_2pi(heading + 180.0f * M_PI_F); + heading = _wrap_2pi(heading + M_PI_F); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } } @@ -318,7 +318,7 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *lat_end, double *lon_end) { bearing = _wrap_2pi(bearing); - double radius_ratio = (double)(dist / CONSTANTS_RADIUS_OF_EARTH); + double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); *lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing)); *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7a98859f81..080b96c5ce 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1264,9 +1264,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); curr_wp_shifted(0) = (float)lat; curr_wp_shifted(1) = (float)lon; + // we want the plane to keep tracking the desired flight path until we start flaring // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds - if (land_noreturn_vertical) { + //if (land_noreturn_vertical) { + if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ @@ -1288,7 +1290,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, curr_wp_shifted, current_position, ground_speed_2d); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); } _att_sp.roll_body = _l1_control.nav_roll(); @@ -1405,7 +1407,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_noreturn_vertical = true; } else { if (_global_pos.vel_d > 0.1f) { - _att_sp.pitch_body = _parameters.land_flare_pitch_min_deg * (height_flare - (_global_pos.alt - terrain_alt)) / height_flare; + _att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) * + math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f); } else { _att_sp.pitch_body = _att_sp.pitch_body; } @@ -1859,12 +1862,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ - if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && ( + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && (launch_detection_state == LAUNCHDETECTION_RES_NONE || - _runway_takeoff.runwayTakeoffEnabled() || - land_noreturn_vertical) - )) { + _runway_takeoff.runwayTakeoffEnabled())) || + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && + land_noreturn_vertical)) + )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } From bc0fb69189ecf20c13c88fd81e86923ed1d3f345 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 28 Oct 2015 14:52:15 +0100 Subject: [PATCH 47/58] change mixer for maja --- ROMFS/px4fmu_common/init.d/2105_maja | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja index 2cbc11c275..10c5608eae 100644 --- a/ROMFS/px4fmu_common/init.d/2105_maja +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -39,7 +39,7 @@ then param set PWM_DISARMED 1000 fi -set MIXER AERTW +set MIXER AERTWF # use PWM parameters for throttle channel set PWM_OUT 5 From d97ead81aa249ba82f518e6f7c59bd50e5f0d5d3 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 28 Oct 2015 17:06:38 +0100 Subject: [PATCH 48/58] set talt timeout to 10sec --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 080b96c5ce..0b07ceda39 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1323,7 +1323,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // we have started landing phase but don't have valid terrain // wait for some time, maybe we will soon get a valid estimate // until then just use the altitude of the landing waypoint - if (hrt_elapsed_time(&_time_started_landing) < 5 * 1000 * 1000) { + if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) { terrain_alt = _pos_sp_triplet.current.alt; } else { // still no valid terrain, abort landing From 5e4df86091f587ffcbb9fdb66fe87d00aaf2520b Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 2 Nov 2015 17:40:32 +0100 Subject: [PATCH 49/58] added albatross config and mixer --- ROMFS/px4fmu_common/init.d/2105_maja | 12 +-- ROMFS/px4fmu_common/init.d/2106_albatross | 51 +++++++++++ .../{AERTWF.main.mix => AAERTWF.main.mix} | 0 ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix | 84 +++++++++++++++++++ 4 files changed, 142 insertions(+), 5 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/2106_albatross rename ROMFS/px4fmu_common/mixers/{AERTWF.main.mix => AAERTWF.main.mix} (100%) create mode 100644 ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja index 10c5608eae..1f68428f3b 100644 --- a/ROMFS/px4fmu_common/init.d/2105_maja +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -5,10 +5,12 @@ # @type Standard Plane # # @output MAIN1 aileron -# @output MAIN2 elevator -# @output MAIN3 rudder -# @output MAIN4 throttle -# @output MAIN5 wheel +# @output MAIN2 aileron +# @output MAIN3 elevator +# @output MAIN4 rudder +# @output MAIN5 throttle +# @output MAIN6 wheel +# @output MAIN7 flaps # # @output AUX1 feed-through of RC AUX1 channel # @output AUX2 feed-through of RC AUX2 channel @@ -39,7 +41,7 @@ then param set PWM_DISARMED 1000 fi -set MIXER AERTWF +set MIXER AAERTWF # use PWM parameters for throttle channel set PWM_OUT 5 diff --git a/ROMFS/px4fmu_common/init.d/2106_albatross b/ROMFS/px4fmu_common/init.d/2106_albatross new file mode 100644 index 0000000000..79e560fb5f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2106_albatross @@ -0,0 +1,51 @@ +#!nsh +# +# @name Applied Aeronautics Albatross +# +# @type Standard Plane +# +# @output MAIN1 aileron right +# @output MAIN2 aileron left +# @output MAIN3 v-tail right +# @output MAIN4 v-tail left +# @output MAIN5 throttle +# @output MAIN6 wheel +# @output MAIN7 flaps right +# @output MAIN8 flaps left +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Andreas Antener +# + +sh /etc/init.d/rc.fw_defaults + +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 15 + param set FW_AIRSPD_MAX 20 + + param set FW_MAN_P_MAX 55 + param set FW_MAN_R_MAX 55 + param set FW_R_LIM 55 + + param set FW_WR_FF 0.2 + param set FW_WR_I 0.2 + param set FW_WR_IMAX 0.8 + param set FW_WR_P 1 + param set FW_W_RMAX 0 + + # set disarmed value for the ESC + param set PWM_DISARMED 1000 +fi + +set MIXER AAVVTWFF + +# use PWM parameters for throttle channel +set PWM_OUT 5 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/mixers/AERTWF.main.mix b/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/AERTWF.main.mix rename to ROMFS/px4fmu_common/mixers/AAERTWF.main.mix diff --git a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix new file mode 100644 index 0000000000..52146b84f0 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix @@ -0,0 +1,84 @@ +Aileron/v-tail/throttle/wheel/flaps mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, v-tail (rudder, elevator), throttle, steerable wheel and flaps +using PX4FMU. +The configuration assumes the aileron servos are connected to PX4FMU servo +output 0 and 1, the tail servos to output 2 and 3, the throttle +to output 4, the wheel to output 5 and the flaps to output 6 and 7. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). + +Aileron mixer (roll + flaperon) +--------------------------------- + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 -10000 -10000 0 -10000 10000 + +V-tail mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two tail servos are physically reversed, the pitch +input is inverted between the two servos. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 -7000 -7000 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 -7000 -7000 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Wheel mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the wheel servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 -10000 -10000 0 -10000 10000 + +Flaps mixer +------------ +Flap servos are physically reversed. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 5000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 -10000 -5000 0 -10000 10000 + From 3a46487fa4f58889dae818075e6ed6dd73f6106c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 3 Nov 2015 15:35:28 +0100 Subject: [PATCH 50/58] fixed flaps offset to have correct neutral position --- ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix index 52146b84f0..e6520862d7 100644 --- a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix +++ b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix @@ -76,9 +76,9 @@ Flap servos are physically reversed. M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 5000 0 -10000 10000 +S: 0 4 0 5000 -10000 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 -10000 -5000 0 -10000 10000 +S: 0 4 0 -5000 10000 -10000 10000 From 4abff89be0bd022c87e1be0a533c8c914478704e Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 6 Nov 2015 22:48:44 +0100 Subject: [PATCH 51/58] updated terrain estimator and runway takeoff libs to cmake build system --- cmake/configs/nuttx_px4fmu-v1_default.cmake | 2 ++ cmake/configs/nuttx_px4fmu-v2_default.cmake | 2 ++ cmake/configs/posix_eagle_default.cmake | 2 ++ cmake/configs/posix_sitl_simple.cmake | 2 ++ cmake/configs/qurt_eagle_hil.cmake | 2 ++ cmake/configs/qurt_eagle_release.cmake | 2 ++ cmake/configs/qurt_eagle_travis.cmake | 2 ++ src/lib/ecl/CMakeLists.txt | 1 + .../{module.mk => CMakeLists.txt} | 22 ++++++++++--------- .../{module.mk => CMakeLists.txt} | 14 ++++++++---- 10 files changed, 37 insertions(+), 14 deletions(-) rename src/lib/runway_takeoff/{module.mk => CMakeLists.txt} (86%) rename src/lib/terrain_estimation/{module.mk => CMakeLists.txt} (90%) diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake index ed48bd4f34..902447c282 100644 --- a/cmake/configs/nuttx_px4fmu-v1_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake @@ -111,6 +111,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 0c6458cad1..c3a3586f66 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -118,6 +118,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/posix_eagle_default.cmake b/cmake/configs/posix_eagle_default.cmake index fc97e70918..76d9711187 100644 --- a/cmake/configs/posix_eagle_default.cmake +++ b/cmake/configs/posix_eagle_default.cmake @@ -38,6 +38,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff platforms/common platforms/posix/px4_layer diff --git a/cmake/configs/posix_sitl_simple.cmake b/cmake/configs/posix_sitl_simple.cmake index 581b82e827..a7291a04f3 100644 --- a/cmake/configs/posix_sitl_simple.cmake +++ b/cmake/configs/posix_sitl_simple.cmake @@ -56,6 +56,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff ) set(config_extra_builtin_cmds diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake index 4222fff249..87dd29152e 100644 --- a/cmake/configs/qurt_eagle_hil.cmake +++ b/cmake/configs/qurt_eagle_hil.cmake @@ -47,6 +47,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff modules/controllib # diff --git a/cmake/configs/qurt_eagle_release.cmake b/cmake/configs/qurt_eagle_release.cmake index 945e9da199..62a20263e5 100644 --- a/cmake/configs/qurt_eagle_release.cmake +++ b/cmake/configs/qurt_eagle_release.cmake @@ -70,6 +70,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff # # QuRT port diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake index b9fda6ec48..75cb5f2394 100644 --- a/cmake/configs/qurt_eagle_travis.cmake +++ b/cmake/configs/qurt_eagle_travis.cmake @@ -52,6 +52,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/ecl + lib/terrain_estimation + lib/runway_takeoff # # QuRT port diff --git a/src/lib/ecl/CMakeLists.txt b/src/lib/ecl/CMakeLists.txt index 0fd5b5f5e9..9f8e02ba2e 100644 --- a/src/lib/ecl/CMakeLists.txt +++ b/src/lib/ecl/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_module( attitude_fw/ecl_pitch_controller.cpp attitude_fw/ecl_roll_controller.cpp attitude_fw/ecl_yaw_controller.cpp + attitude_fw/ecl_wheel_controller.cpp l1/ecl_l1_pos_controller.cpp validation/data_validator.cpp validation/data_validator_group.cpp diff --git a/src/lib/runway_takeoff/module.mk b/src/lib/runway_takeoff/CMakeLists.txt similarity index 86% rename from src/lib/runway_takeoff/module.mk rename to src/lib/runway_takeoff/CMakeLists.txt index 95b9aea53e..7938279a0e 100644 --- a/src/lib/runway_takeoff/module.mk +++ b/src/lib/runway_takeoff/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. +# Copyright (c) 2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -30,12 +30,14 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -# -# RunwayTakeoff Library -# - -SRCS = RunwayTakeoff.cpp \ - runway_takeoff_params.c - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE lib__runway_takeoff + COMPILE_FLAGS + -Os + SRCS + RunwayTakeoff.cpp + runway_takeoff_params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/terrain_estimation/module.mk b/src/lib/terrain_estimation/CMakeLists.txt similarity index 90% rename from src/lib/terrain_estimation/module.mk rename to src/lib/terrain_estimation/CMakeLists.txt index 3a2047012a..0c1177b96b 100644 --- a/src/lib/terrain_estimation/module.mk +++ b/src/lib/terrain_estimation/CMakeLists.txt @@ -30,7 +30,13 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -SRCS = terrain_estimator.cpp - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE lib__terrain_estimation + COMPILE_FLAGS + -Os + SRCS + terrain_estimator.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : From 2719789b2e773f4d4dfc347649d1bb71bc690b57 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 7 Nov 2015 11:41:15 +0100 Subject: [PATCH 52/58] use matrix lib to enable building for posix --- .../terrain_estimation/terrain_estimator.cpp | 54 +++++++++---------- .../terrain_estimation/terrain_estimator.h | 5 +- 2 files changed, 30 insertions(+), 29 deletions(-) diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index 16ab8de4a0..a92bbb136e 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -46,9 +46,9 @@ TerrainEstimator::TerrainEstimator() : _time_last_distance(0), _time_last_gps(0) { - _x.zero(); + memset(&_x._data[0], 0, sizeof(_x._data)); _u_z = 0.0f; - _P.identity(); + _P.setIdentity(); } bool TerrainEstimator::is_distance_valid(float distance) { @@ -63,9 +63,9 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu const struct distance_sensor_s *distance) { if (attitude->R_valid) { - math::Matrix<3, 3> R_att(attitude->R); - math::Vector<3> a(&sensor->accelerometer_m_s2[0]); - math::Vector<3> u; + matrix::Matrix R_att(attitude->R); + matrix::Vector a(&sensor->accelerometer_m_s2[0]); + matrix::Vector u; u = R_att * a; _u_z = u(2) + 9.81f; // compensate for gravity @@ -74,32 +74,32 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu } // dynamics matrix - math::Matrix A; - A.zero(); + matrix::Matrix A; + A.setZero(); A(0,1) = 1; A(1,2) = 1; // input matrix - math::Matrix B; - B.zero(); + matrix::Matrix B; + B.setZero(); B(1,0) = 1; // input noise variance float R = 0.135f; // process noise convariance - math::Matrix Q; + matrix::Matrix Q; Q(0,0) = 0; Q(1,1) = 0; // do prediction - math::Vector dx = (A * _x) * dt; + matrix::Vector dx = (A * _x) * dt; dx(1) += B(1,0) * _u_z * dt; // propagate state and covariance matrix _x += dx; - _P += (A * _P + _P * A.transposed() + - B * R * B.transposed() + Q) * dt; + _P += (A * _P + _P * A.transpose() + + B * R * B.transpose() + Q) * dt; } void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, @@ -114,21 +114,21 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl float d = distance->current_distance; - math::Matrix<1, n_x> C; + matrix::Matrix C; C(0, 0) = -1; // measured altitude, float R = 0.009f; - math::Vector<1> y; + matrix::Vector y; y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch); // residual - math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + matrix::Matrix S_I = (C * _P * C.transpose()); S_I(0,0) += R; - S_I = S_I.inversed(); - math::Vector<1> r = y - C * _x; + S_I = matrix::inv (S_I); + matrix::Vector r = y - C * _x; - math::Matrix K = _P * C.transposed() * S_I; + matrix::Matrix K = _P * C.transpose() * S_I; // some sort of outlayer rejection if (fabsf(distance->current_distance - _distance_last) < 1.0f) { @@ -149,21 +149,21 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl } if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { - math::Matrix<1, n_x> C; + matrix::Matrix C; C(0,1) = 1; float R = 0.056f; - math::Vector<1> y; + matrix::Vector y; y(0) = gps->vel_d_m_s; // residual - math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + matrix::Matrix S_I = (C * _P * C.transpose()); S_I(0,0) += R; - S_I = S_I.inversed(); - math::Vector<1> r = y - C * _x; + S_I = matrix::inv(S_I); + matrix::Vector r = y - C * _x; - math::Matrix K = _P * C.transposed() * S_I; + matrix::Matrix K = _P * C.transpose() * S_I; _x += K * r; _P -= K * C * _P; @@ -187,8 +187,8 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl } if (reinit) { - _x.zero(); - _P.zero(); + memset(&_x._data[0], 0, sizeof(_x._data)); + _P.setZero(); _P(0,0) = _P(1,1) = _P(2,2) = 0.1f; } diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index fc10cb8d63..697a053b88 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -36,6 +36,7 @@ */ #include +#include "matrix/Matrix.hpp" #include #include #include @@ -75,9 +76,9 @@ private: bool _terrain_valid; // kalman filter variables - math::Vector _x; // state: ground distance, velocity, accel bias in z direction + matrix::Vector _x; // state: ground distance, velocity, accel bias in z direction float _u_z; // acceleration in earth z direction - math::Matrix<3, 3> _P; // covariance matrix + matrix::Matrix _P; // covariance matrix // timestamps uint64_t _time_last_distance; From 120fd9d5225e1e09a19dc51c0f0ccc27b4a98766 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 7 Nov 2015 11:41:56 +0100 Subject: [PATCH 53/58] use control state topic for attitude and airspeed --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0b07ceda39..24cf1f084f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1457,7 +1457,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { - _runway_takeoff.init(_att.yaw, _global_pos.lat, _global_pos.lon); + math::Quaternion q(&_ctrl_state.q[0]); + math::Vector<3> euler = q.to_euler(); + _runway_takeoff.init(euler(2), _global_pos.lat, _global_pos.lon); /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ @@ -1470,7 +1472,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // update runway takeoff helper _runway_takeoff.update( - _airspeed.true_airspeed_m_s, + _ctrl_state.airspeed, _global_pos.alt - terrain_alt, _global_pos.lat, _global_pos.lon, From e48cf53ce83ac7f1e30b3e662e6eae5e568688c3 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 7 Nov 2015 11:43:07 +0100 Subject: [PATCH 54/58] fix code style --- src/lib/geo/geo.c | 15 ++++--- src/lib/geo/geo.h | 6 ++- src/lib/runway_takeoff/RunwayTakeoff.cpp | 2 +- .../terrain_estimation/terrain_estimator.cpp | 40 +++++++++++-------- .../terrain_estimation/terrain_estimator.h | 7 ++-- 5 files changed, 42 insertions(+), 28 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 0d09e8526b..023337ff04 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -298,16 +298,19 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou return CONSTANTS_RADIUS_OF_EARTH * c; } -__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target) +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target) { float heading; + if (fabsf(dist) < FLT_EPSILON) { *lat_target = lat_A; *lon_target = lon_A; - } - else if (dist >= FLT_EPSILON) { + + } else if (dist >= FLT_EPSILON) { heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } else { heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); heading = _wrap_2pi(heading + M_PI_F); @@ -315,13 +318,15 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou } } -__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *lat_end, double *lon_end) +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *lat_end, double *lon_end) { bearing = _wrap_2pi(bearing); double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); *lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing)); - *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); + *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), + cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index f9250fb5a3..8dc0836e34 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -238,9 +238,11 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou // TODO put description for both functions and improve naming -__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target); +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target); -__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *end_lat, double *end_lon); +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *end_lat, double *end_lon); /** * Returns the bearing to the next waypoint in radians. diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 46df5cd744..7089889858 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -92,7 +92,7 @@ void RunwayTakeoff::init(float yaw, double current_lat, double current_lon) } void RunwayTakeoff::update(float airspeed, float alt_agl, - double current_lat, double current_lon, int mavlink_fd) + double current_lat, double current_lon, int mavlink_fd) { switch (_state) { diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index a92bbb136e..64a747f532 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -51,16 +51,19 @@ TerrainEstimator::TerrainEstimator() : _P.setIdentity(); } -bool TerrainEstimator::is_distance_valid(float distance) { +bool TerrainEstimator::is_distance_valid(float distance) +{ if (distance > 40.0f || distance < 0.00001f) { return false; + } else { return true; } } -void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, - const struct distance_sensor_s *distance) +void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, + const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance) { if (attitude->R_valid) { matrix::Matrix R_att(attitude->R); @@ -76,25 +79,25 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu // dynamics matrix matrix::Matrix A; A.setZero(); - A(0,1) = 1; - A(1,2) = 1; + A(0, 1) = 1; + A(1, 2) = 1; // input matrix - matrix::Matrix B; + matrix::Matrix B; B.setZero(); - B(1,0) = 1; + B(1, 0) = 1; // input noise variance float R = 0.135f; // process noise convariance matrix::Matrix Q; - Q(0,0) = 0; - Q(1,1) = 0; + Q(0, 0) = 0; + Q(1, 1) = 0; // do prediction matrix::Vector dx = (A * _x) * dt; - dx(1) += B(1,0) * _u_z * dt; + dx(1) += B(1, 0) * _u_z * dt; // propagate state and covariance matrix _x += dx; @@ -102,8 +105,9 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu B * R * B.transpose() + Q) * dt; } -void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, - const struct vehicle_attitude_s *attitude) +void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude) { // terrain estimate is invalid if we have range sensor timeout if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) { @@ -124,7 +128,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // residual matrix::Matrix S_I = (C * _P * C.transpose()); - S_I(0,0) += R; + S_I(0, 0) += R; S_I = matrix::inv (S_I); matrix::Vector r = y - C * _x; @@ -140,6 +144,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // estimate to be invalid if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) { _terrain_valid = false; + } else { _terrain_valid = true; } @@ -150,7 +155,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { matrix::Matrix C; - C(0,1) = 1; + C(0, 1) = 1; float R = 0.056f; @@ -159,7 +164,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // residual matrix::Matrix S_I = (C * _P * C.transpose()); - S_I(0,0) += R; + S_I(0, 0) += R; S_I = matrix::inv(S_I); matrix::Vector r = y - C * _x; @@ -172,6 +177,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // reinitialise filter if we find bad data bool reinit = false; + for (int i = 0; i < n_x; i++) { if (!PX4_ISFINITE(_x(i))) { reinit = true; @@ -180,7 +186,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl for (int i = 0; i < n_x; i++) { for (int j = 0; j < n_x; j++) { - if (!PX4_ISFINITE(_P(i,j))) { + if (!PX4_ISFINITE(_P(i, j))) { reinit = true; } } @@ -189,7 +195,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl if (reinit) { memset(&_x._data[0], 0, sizeof(_x._data)); _P.setZero(); - _P(0,0) = _P(1,1) = _P(2,2) = 0.1f; + _P(0, 0) = _P(1, 1) = _P(2, 2) = 0.1f; } } diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index 697a053b88..25723f21d8 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -65,12 +65,13 @@ public: float get_velocity() {return _x(1);}; void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, - const struct distance_sensor_s *distance); - void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, + const struct distance_sensor_s *distance); + void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, const struct vehicle_attitude_s *attitude); private: - enum {n_x=3}; + enum {n_x = 3}; float _distance_last; bool _terrain_valid; From c989c21269e70399389af5a48a4222145f9512d6 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 10 Nov 2015 13:33:01 +0100 Subject: [PATCH 55/58] fixed geo functions --- src/lib/geo/geo.c | 15 +++++++++++---- src/lib/geo/geo.h | 25 +++++++++++++++++++++++-- 2 files changed, 34 insertions(+), 6 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 023337ff04..a8eee50435 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -319,14 +319,21 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou } __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, - double *lat_end, double *lon_end) + double *lat_target, double *lon_target) { bearing = _wrap_2pi(bearing); double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); - *lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing)); - *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), - cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); + double lat_start_rad = lat_start / (double)180.0 * M_PI; + double lon_start_rad = lon_start / (double)180.0 * M_PI; + + *lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos(( + double)bearing)); + *lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad), + cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target)); + + *lat_target *= (double)180.0 / M_PI; + *lon_target *= (double)180.0 / M_PI; } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 8dc0836e34..0aa6cf03a8 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -237,12 +237,33 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -// TODO put description for both functions and improve naming +/** + * Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance + * from waypoint A + * + * @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°) + * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°) + * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°) + * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°) + * @param dist distance of target waypoint from waypoint A (can be negative) + * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°) + * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°) + */ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target); +/** + * Creates a waypoint from given waypoint, distance and bearing + * + * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°) + * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°) + * @param bearing + * @param distance + * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°) + * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°) + */ __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, - double *end_lat, double *end_lon); + double *lat_target, double *lon_target); /** * Returns the bearing to the next waypoint in radians. From 154fa07a4647534f2673e86870ae9b5d292f98d3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 16 Nov 2015 12:02:56 +0100 Subject: [PATCH 56/58] fixes after review --- src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp | 2 +- src/lib/geo/geo.c | 14 ++++++-------- src/lib/geo/geo.h | 7 ++++--- src/lib/runway_takeoff/runway_takeoff_params.c | 4 ++++ src/lib/terrain_estimation/terrain_estimator.h | 6 ++++-- .../ekf_att_pos_estimator_main.cpp | 2 +- 6 files changed, 20 insertions(+), 15 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index 7f8a9ca6c7..7e34ff2a61 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -33,7 +33,7 @@ /** * @file ecl_wheel_controller.cpp - * Implementation of a simple orthogonal coordinated turn yaw PID controller. + * Implementation of a simple PID wheel controller for heading tracking. * * Authors and acknowledgements in header. */ diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index a8eee50435..af49bf1504 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -301,18 +301,16 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target) { - float heading; - if (fabsf(dist) < FLT_EPSILON) { *lat_target = lat_A; *lon_target = lon_A; } else if (dist >= FLT_EPSILON) { - heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } else { - heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); heading = _wrap_2pi(heading + M_PI_F); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } @@ -324,16 +322,16 @@ __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_st bearing = _wrap_2pi(bearing); double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); - double lat_start_rad = lat_start / (double)180.0 * M_PI; - double lon_start_rad = lon_start / (double)180.0 * M_PI; + double lat_start_rad = lat_start * M_DEG_TO_RAD; + double lon_start_rad = lon_start * M_DEG_TO_RAD; *lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos(( double)bearing)); *lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad), cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target)); - *lat_target *= (double)180.0 / M_PI; - *lon_target *= (double)180.0 / M_PI; + *lat_target *= M_RAD_TO_DEG; + *lon_target *= M_RAD_TO_DEG; } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 0aa6cf03a8..11451fcf19 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -245,7 +245,7 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°) * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°) * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°) - * @param dist distance of target waypoint from waypoint A (can be negative) + * @param dist distance of target waypoint from waypoint A in meters (can be negative) * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°) * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°) */ @@ -254,11 +254,12 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou /** * Creates a waypoint from given waypoint, distance and bearing + * see http://www.movable-type.co.uk/scripts/latlong.html * * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°) * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°) - * @param bearing - * @param distance + * @param bearing in rad + * @param distance in meters * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°) * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°) */ diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index 7c64889e76..f5a1017f96 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -71,6 +71,7 @@ PARAM_DEFINE_INT32(RWTO_HDG, 0); * rudder is used to keep the heading (see RWTO_HDG). This should be below * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0. * + * @unit m * @min 0.0 * @max 100.0 * @group Runway Takeoff @@ -93,6 +94,7 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); * a little to keep it's wheel on the ground before airspeed * to takeoff is reached. * + * @unit deg * @min 0.0 * @max 20.0 * @group Runway Takeoff @@ -104,6 +106,7 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); * Fixed-wing settings are used if set to 0. Note that there is also a minimum * pitch of 10 degrees during takeoff, so this must be larger if set. * + * @unit deg * @min 0.0 * @max 60.0 * @group Runway Takeoff @@ -115,6 +118,7 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); * Roll is limited during climbout to ensure enough lift and prevents aggressive * navigation before we're on a safe height. * + * @unit deg * @min 0.0 * @max 60.0 * @group Runway Takeoff diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index 25723f21d8..ed9e9741f3 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -51,14 +51,14 @@ * The measurement_update(...) function does a measurement update based on range finder and gps * velocity measurements. Both functions should always be called together when there is new * acceleration data available. -* The is_valid() function provides information wether the estimate is valid. +* The is_valid() function provides information whether the estimate is valid. */ class __EXPORT TerrainEstimator { public: TerrainEstimator(); - ~TerrainEstimator(); + ~TerrainEstimator() {}; bool is_valid() {return _terrain_valid;} float get_distance_to_ground() {return -_x(0);} @@ -85,6 +85,7 @@ private: uint64_t _time_last_distance; uint64_t _time_last_gps; + /* struct { float var_acc_z; float var_p_z; @@ -92,6 +93,7 @@ private: float var_lidar; float var_gps_vz; } _params; + */ bool is_distance_valid(float distance); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e4c8eeccde..49f08f3cab 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -270,7 +270,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() } } while (_estimator_task != -1); } - + delete _terrain_estimator; delete _ekf; estimator::g_estimator = nullptr; From c721d565a97c7f14a17211c715092192bc8e53e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Nov 2015 11:21:08 +0100 Subject: [PATCH 57/58] Upgrade XCode version to what is working locally for Travis CI --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index e6b5239344..3745cc867a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -9,7 +9,7 @@ matrix: - os: linux sudo: false - os: osx - osx_image: beta-xcode6.3 + osx_image: xcode7 sudo: true cache: From 3c1c2ba34e600cad3d463847322a9123ffda9eb6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Nov 2015 11:57:01 +0100 Subject: [PATCH 58/58] Travis CI: Install CMake --- .travis.yml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 3745cc867a..2aafa122e1 100644 --- a/.travis.yml +++ b/.travis.yml @@ -60,11 +60,9 @@ before_install: elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then brew tap PX4/homebrew-px4 && brew update - && brew install astyle - && brew install gcc-arm-none-eabi + && brew install cmake ninja astyle gcc-arm-none-eabi && brew install genromfs && brew install kconfig-frontends - && brew install ninja && sudo easy_install pip && sudo pip install pyserial empy ;