diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index 29a83ed715e9..c8dc4b54d1a6 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -39,7 +39,6 @@ bool yawspeed_valid # true if yawspeed setpoint valid float32 loiter_radius # loiter radius (only for fixed wing), in m int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW -float32 pitch_min # minimal pitch angle for fixed wing takeoff waypoints float32 a_x # acceleration x setpoint float32 a_y # acceleration y setpoint diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 8306b7b00d19..72573ef5e1e3 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -258,8 +258,9 @@ FixedwingPositionControl::vehicle_command_poll() orb_check(_vehicle_command_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &_vehicle_command); - handle_command(); + vehicle_command_s vehicle_command; + orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command); + handle_command(vehicle_command); } } @@ -372,7 +373,12 @@ FixedwingPositionControl::position_setpoint_triplet_poll() orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated); if (pos_sp_triplet_updated) { - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + position_setpoint_triplet_s pos_sp_triplet; + + if (orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &pos_sp_triplet) == PX4_OK) { + _pos_sp_curr = pos_sp_triplet.current; + _pos_sp_prev = pos_sp_triplet.previous; + } } } @@ -434,7 +440,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const Vector2f &curr_pos const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { - if (pos_sp_curr.valid && !_l1_control.circle_mode()) { + if (_pos_sp_curr.valid && !_l1_control.circle_mode()) { /* rotate ground speed vector with current attitude */ Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); @@ -444,12 +450,12 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const Vector2f &curr_pos float distance = 0.0f; float delta_altitude = 0.0f; - if (pos_sp_prev.valid) { - distance = get_distance_to_next_waypoint(pos_sp_prev.lat, pos_sp_prev.lon, pos_sp_curr.lat, pos_sp_curr.lon); + if (_pos_sp_prev.valid) { + distance = get_distance_to_next_waypoint(_pos_sp_prev.lat, _pos_sp_prev.lon, _pos_sp_curr.lat, _pos_sp_curr.lon); delta_altitude = pos_sp_curr.alt - pos_sp_prev.alt; } else { - distance = get_distance_to_next_waypoint(curr_pos(0), curr_pos(1), pos_sp_curr.lat, pos_sp_curr.lon); + distance = get_distance_to_next_waypoint(curr_pos(0), curr_pos(1), _pos_sp_curr.lat, _pos_sp_curr.lon); delta_altitude = pos_sp_curr.alt - _global_pos.alt; } @@ -623,8 +629,7 @@ FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_lim } bool -FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vector2f &ground_speed, - const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) +FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vector2f &ground_speed) { float dt = 0.01f; @@ -645,7 +650,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _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 - calculate_gndspeed_undershoot(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); + calculate_gndspeed_undershoot(curr_pos, ground_speed, _pos_sp_prev, _pos_sp_curr); // l1 navigation logic breaks down when wind speed exceeds max airspeed // compute 2D groundspeed from airspeed-heading projection @@ -684,7 +689,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _tecs.reset_state(); } - if (_control_mode.flag_control_auto_enabled && pos_sp_curr.valid) { + if (_control_mode.flag_control_auto_enabled && _pos_sp_curr.valid) { /* AUTONOMOUS FLIGHT */ _control_mode_current = FW_POSCTRL_MODE_AUTO; @@ -702,7 +707,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _tecs.set_speed_weight(_parameters.speed_weight); /* current waypoint (the one currently heading for) */ - Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); + Vector2f curr_wp((float)_pos_sp_curr.lat, (float)_pos_sp_curr.lon); /* Initialize attitude controller integrator reset flags to 0 */ _att_sp.roll_reset_integral = false; @@ -712,47 +717,47 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto /* previous waypoint */ Vector2f prev_wp{0.0f, 0.0f}; - if (pos_sp_prev.valid) { - prev_wp(0) = (float)pos_sp_prev.lat; - prev_wp(1) = (float)pos_sp_prev.lon; + if (_pos_sp_prev.valid) { + prev_wp(0) = (float)_pos_sp_prev.lat; + prev_wp(1) = (float)_pos_sp_prev.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp(0) = (float)pos_sp_curr.lat; - prev_wp(1) = (float)pos_sp_curr.lon; + prev_wp(0) = (float)_pos_sp_curr.lat; + prev_wp(1) = (float)_pos_sp_curr.lon; } float mission_airspeed = _parameters.airspeed_trim; - if (PX4_ISFINITE(pos_sp_curr.cruising_speed) && - pos_sp_curr.cruising_speed > 0.1f) { + if (PX4_ISFINITE(_pos_sp_curr.cruising_speed) && + _pos_sp_curr.cruising_speed > 0.1f) { - mission_airspeed = pos_sp_curr.cruising_speed; + mission_airspeed = _pos_sp_curr.cruising_speed; } float mission_throttle = _parameters.throttle_cruise; - if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) && - pos_sp_curr.cruising_throttle > 0.01f) { + if (PX4_ISFINITE(_pos_sp_curr.cruising_throttle) && + _pos_sp_curr.cruising_throttle > 0.01f) { - mission_throttle = pos_sp_curr.cruising_throttle; + mission_throttle = _pos_sp_curr.cruising_throttle; } - if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + if (_pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; - } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + } else if (_pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - tecs_update_pitch_throttle(pos_sp_curr.alt, + tecs_update_pitch_throttle(_pos_sp_curr.alt, calculate_target_airspeed(mission_airspeed), radians(_parameters.pitch_limit_min) - _parameters.pitchsp_offset_rad, radians(_parameters.pitch_limit_max) - _parameters.pitchsp_offset_rad, @@ -762,15 +767,14 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto false, radians(_parameters.pitch_limit_min)); - } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + } else if (_pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(curr_wp, curr_pos, pos_sp_curr.loiter_radius, - pos_sp_curr.loiter_direction, nav_speed_2d); + _l1_control.navigate_loiter(curr_wp, curr_pos, _pos_sp_curr.loiter_radius, _pos_sp_curr.loiter_direction, nav_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - float alt_sp = pos_sp_curr.alt; + float alt_sp = _pos_sp_curr.alt; if (in_takeoff_situation()) { alt_sp = max(alt_sp, _takeoff_ground_alt + _parameters.climbout_diff); @@ -778,7 +782,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto } if (_fw_pos_ctrl_status.abort_landing) { - if (pos_sp_curr.alt - _global_pos.alt < _parameters.climbout_diff) { + if (_pos_sp_curr.alt - _global_pos.alt < _parameters.climbout_diff) { // aborted landing complete, normal loiter over landing point _fw_pos_ctrl_status.abort_landing = false; @@ -798,20 +802,20 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto false, radians(_parameters.pitch_limit_min)); - } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - control_landing(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); + } else if (_pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + control_landing(curr_pos, ground_speed); - } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - control_takeoff(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); + } else if (_pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + control_takeoff(curr_pos, ground_speed); } /* reset landing state */ - if (pos_sp_curr.type != position_setpoint_s::SETPOINT_TYPE_LAND) { - reset_landing_state(); + if (_pos_sp_curr.type != position_setpoint_s::SETPOINT_TYPE_LAND) { + reset_landing_state(_pos_sp_curr.type); } /* reset takeoff/launch state */ - if (pos_sp_curr.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + if (_pos_sp_curr.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -895,19 +899,18 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _hdg_hold_enabled = true; _hdg_hold_yaw = _yaw; - get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); + get_waypoint_heading_distance(_hdg_hold_yaw, _pos_sp_prev, _pos_sp_curr, true); } /* we have a valid heading hold position, are we too close? */ - float dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _hdg_hold_curr_wp.lat, - _hdg_hold_curr_wp.lon); + float dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _pos_sp_curr.lat, _pos_sp_curr.lon); if (dist < HDG_HOLD_REACHED_DIST) { - get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); + get_waypoint_heading_distance(_hdg_hold_yaw, _pos_sp_prev, _pos_sp_curr, false); } - Vector2f prev_wp{(float)_hdg_hold_prev_wp.lat, (float)_hdg_hold_prev_wp.lon}; - Vector2f curr_wp{(float)_hdg_hold_curr_wp.lat, (float)_hdg_hold_curr_wp.lon}; + Vector2f prev_wp{(float)_pos_sp_prev.lat, (float)_pos_sp_prev.lon}; + Vector2f curr_wp{(float)_pos_sp_curr.lat, (float)_pos_sp_curr.lon}; /* populate l1 control setpoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); @@ -985,14 +988,14 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto /* reset landing and takeoff state */ if (!_last_manual) { - reset_landing_state(); + reset_landing_state(_pos_sp_curr.type); reset_takeoff_state(); } } /* Copy thrust output for publication */ if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto - pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + _pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && !_runway_takeoff.runwayTakeoffEnabled()) { @@ -1002,13 +1005,13 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _att_sp.thrust = _parameters.throttle_idle; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + _pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _runway_takeoff.runwayTakeoffEnabled()) { _att_sp.thrust = _runway_takeoff.getThrottle(min(get_tecs_thrust(), throttle_max)); } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + _pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; @@ -1032,11 +1035,11 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto // auto runway takeoff use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + _pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && (_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled())); // flaring during landing - use_tecs_pitch &= !(pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _land_noreturn_vertical); + use_tecs_pitch &= !(_pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _land_noreturn_vertical); // manual attitude control use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_OTHER); @@ -1056,24 +1059,23 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto } void -FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector2f &ground_speed, - const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) +FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector2f &ground_speed) { /* current waypoint (the one currently heading for) */ - Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); + Vector2f curr_wp((float)_pos_sp_curr.lat, (float)_pos_sp_curr.lon); Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */ - if (pos_sp_prev.valid) { - prev_wp(0) = (float)pos_sp_prev.lat; - prev_wp(1) = (float)pos_sp_prev.lon; + if (_pos_sp_prev.valid) { + prev_wp(0) = (float)_pos_sp_prev.lat; + prev_wp(1) = (float)_pos_sp_prev.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp(0) = (float)pos_sp_curr.lat; - prev_wp(1) = (float)pos_sp_curr.lon; + prev_wp(0) = (float)_pos_sp_curr.lat; + prev_wp(1) = (float)_pos_sp_curr.lon; } // continuously reset launch detection and runway takeoff until armed @@ -1110,7 +1112,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector // update tecs const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); - tecs_update_pitch_throttle(pos_sp_curr.alt, + tecs_update_pitch_throttle(_pos_sp_curr.alt, calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), radians(_parameters.pitch_limit_min), radians(takeoff_pitch_max_deg), @@ -1118,7 +1120,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? _parameters.throttle_cruise, _runway_takeoff.climbout(), - radians(_runway_takeoff.getMinPitch(pos_sp_curr.pitch_min, 10.0f, _parameters.pitch_limit_min)), + radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _parameters.pitch_limit_min)), tecs_status_s::TECS_MODE_TAKEOFF); // assign values @@ -1176,12 +1178,12 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector /* select maximum pitch: the launchdetector may impose another limit for the pitch * depending on the state of the launch */ const float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_parameters.pitch_limit_max); - const float altitude_error = pos_sp_curr.alt - _global_pos.alt; + const float altitude_error = _pos_sp_curr.alt - _global_pos.alt; /* apply minimum pitch and limit roll if target altitude is not within climbout_diff meters */ if (_parameters.climbout_diff > 0.0f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - tecs_update_pitch_throttle(pos_sp_curr.alt, + tecs_update_pitch_throttle(_pos_sp_curr.alt, _parameters.airspeed_trim, radians(_parameters.pitch_limit_min), radians(takeoff_pitch_max_deg), @@ -1189,14 +1191,14 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector takeoff_throttle, _parameters.throttle_cruise, true, - max(radians(pos_sp_curr.pitch_min), radians(10.0f)), + radians(_takeoff_pitch_min.get()), tecs_status_s::TECS_MODE_TAKEOFF); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f)); } else { - tecs_update_pitch_throttle(pos_sp_curr.alt, + tecs_update_pitch_throttle(_pos_sp_curr.alt, calculate_target_airspeed(_parameters.airspeed_trim), radians(_parameters.pitch_limit_min), radians(_parameters.pitch_limit_max), @@ -1216,30 +1218,29 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector /* Set default roll and pitch setpoints during detection phase */ _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = max(radians(pos_sp_curr.pitch_min), radians(10.0f)); + _att_sp.pitch_body = radians(_takeoff_pitch_min.get()); } } } void -FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector2f &ground_speed, - const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) +FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector2f &ground_speed) { /* current waypoint (the one currently heading for) */ - Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); + Vector2f curr_wp((float)_pos_sp_curr.lat, (float)_pos_sp_curr.lon); Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */ - if (pos_sp_prev.valid) { - prev_wp(0) = (float)pos_sp_prev.lat; - prev_wp(1) = (float)pos_sp_prev.lon; + if (_pos_sp_prev.valid) { + prev_wp(0) = (float)_pos_sp_prev.lat; + prev_wp(1) = (float)_pos_sp_prev.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp(0) = (float)pos_sp_curr.lat; - prev_wp(1) = (float)pos_sp_curr.lon; + prev_wp(0) = (float)_pos_sp_curr.lat; + prev_wp(1) = (float)_pos_sp_curr.lon; } // apply full flaps for landings. this flag will also trigger the use of flaperons @@ -1271,12 +1272,12 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector // 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 - if (pos_sp_prev.valid) { - double lat = pos_sp_curr.lat; - double lon = pos_sp_curr.lon; + if (_pos_sp_prev.valid) { + double lat = _pos_sp_curr.lat; + double lon = _pos_sp_curr.lon; - create_waypoint_from_line_and_dist(pos_sp_curr.lat, pos_sp_curr.lon, - pos_sp_prev.lat, pos_sp_prev.lon, -1000.0f, &lat, &lon); + create_waypoint_from_line_and_dist(_pos_sp_curr.lat, _pos_sp_curr.lon, + _pos_sp_prev.lat, _pos_sp_prev.lon, -1000.0f, &lat, &lon); curr_wp(0) = (float)lat; curr_wp(1) = (float)lon; @@ -1287,7 +1288,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector if ((_parameters.land_heading_hold_horizontal_distance > 0.0f) && !_land_noreturn_horizontal && ((wp_distance < _parameters.land_heading_hold_horizontal_distance) || _land_noreturn_vertical)) { - if (pos_sp_prev.valid) { + if (_pos_sp_prev.valid) { /* heading hold, along the line connecting this and the last waypoint */ _target_bearing = bearing_lastwp_currwp; @@ -1320,7 +1321,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // default to no terrain estimation, just use landing waypoint altitude - float terrain_alt = pos_sp_curr.alt; + float terrain_alt = _pos_sp_curr.alt; if (_parameters.land_use_terrain_estimate == 1) { if (_global_pos.terrain_alt_valid) { @@ -1334,11 +1335,11 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector // 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) < 10 * 1000 * 1000) { - terrain_alt = pos_sp_curr.alt; + terrain_alt = _pos_sp_curr.alt; } else { // still no valid terrain, abort landing - terrain_alt = pos_sp_curr.alt; + terrain_alt = _pos_sp_curr.alt; _fw_pos_ctrl_status.abort_landing = true; } @@ -1359,8 +1360,8 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_altitude_rel = 0.0f; - if (pos_sp_prev.valid) { - L_altitude_rel = pos_sp_prev.alt - terrain_alt; + if (_pos_sp_prev.valid) { + L_altitude_rel = _pos_sp_prev.alt - terrain_alt; } float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, @@ -1462,7 +1463,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector } else { /* continue horizontally */ - if (pos_sp_prev.valid) { + if (_pos_sp_prev.valid) { altitude_desired_rel = L_altitude_rel; } else { @@ -1507,13 +1508,11 @@ FixedwingPositionControl::get_tecs_thrust() } void -FixedwingPositionControl::handle_command() +FixedwingPositionControl::handle_command(const vehicle_command_s &vcmd) { - if (_vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) { + if (vcmd.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) { // only abort landing before point of no return (horizontal and vertical) - if (_control_mode.flag_control_auto_enabled && - _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + if (_control_mode.flag_control_auto_enabled && (_time_started_landing != 0)) { _fw_pos_ctrl_status.abort_landing = true; mavlink_log_critical(&_mavlink_log_pub, "Landing aborted"); @@ -1642,7 +1641,7 @@ FixedwingPositionControl::run() * Attempt to control position, on success (= sensors present and not in manual mode), * publish setpoint. */ - if (control_position(curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current)) { + if (control_position(curr_pos, ground_speed)) { _att_sp.timestamp = hrt_absolute_time(); // add attitude setpoint offsets @@ -1692,9 +1691,8 @@ FixedwingPositionControl::run() _fw_pos_ctrl_status.target_bearing = _l1_control.target_bearing(); _fw_pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error(); - Vector2f curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon); - - _fw_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(curr_pos(0), curr_pos(1), curr_wp(0), curr_wp(1)); + _fw_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(curr_pos(0), curr_pos(1), _pos_sp_curr.lat, + _pos_sp_curr.lon); fw_pos_ctrl_status_publish(); } @@ -1723,7 +1721,7 @@ FixedwingPositionControl::reset_takeoff_state() } void -FixedwingPositionControl::reset_landing_state() +FixedwingPositionControl::reset_landing_state(const uint8_t pos_sp_type) { _time_started_landing = 0; @@ -1738,7 +1736,7 @@ FixedwingPositionControl::reset_landing_state() // reset abort land, unless loitering after an abort if (_fw_pos_ctrl_status.abort_landing - && _pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER) { + && (pos_sp_type != position_setpoint_s::SETPOINT_TYPE_LOITER)) { _fw_pos_ctrl_status.abort_landing = false; } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 63b0820ce136..90bd79092e4e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -170,10 +170,8 @@ class FixedwingPositionControl final : public ModuleBase) _takeoff_pitch_min + ) /** * Update our local parameter cache. @@ -436,12 +437,9 @@ class FixedwingPositionControl final : public ModuleBasealtitude_is_relative = true; } - /* this field is shared with pitch_min (and circle_radius for geofence) in memory and - * exclusive in the MAVLink spec. Set it to 0 first - * and then set minimum pitch later only for the - * corresponding item - */ - mission_item->time_inside = 0.0f; - switch (mavlink_mission_item->command) { case MAV_CMD_NAV_WAYPOINT: mission_item->nav_cmd = NAV_CMD_WAYPOINT; @@ -1332,7 +1325,6 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_NAV_TAKEOFF: mission_item->nav_cmd = NAV_CMD_TAKEOFF; - mission_item->pitch_min = mavlink_mission_item->param1; mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); break; @@ -1598,7 +1590,6 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * break; case NAV_CMD_TAKEOFF: - mavlink_mission_item->param1 = mission_item->pitch_min; mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; break; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index fd32d46a4b62..730f019dabd9 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -554,11 +554,6 @@ Mission::set_mission_items() _mission_item.nav_cmd = NAV_CMD_WAYPOINT; /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ _mission_item.yaw = NAN; - /* since _mission_item.time_inside and and _mission_item.pitch_min build a union, we need to set time_inside to zero - * since in NAV_CMD_TAKEOFF mode there is currently no time_inside. - * Note also that resetting time_inside to zero will cause pitch_min to be zero as well. - */ - _mission_item.time_inside = 0.0f; } else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && _work_item_type == WORK_ITEM_TYPE_DEFAULT @@ -585,10 +580,6 @@ Mission::set_mission_items() _mission_item.nav_cmd = NAV_CMD_WAYPOINT; /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ _mission_item.yaw = NAN; - /* since _mission_item.time_inside and and _mission_item.pitch_min build a union, we need to set time_inside to zero - * since in NAV_CMD_TAKEOFF mode there is currently no time_inside. - */ - _mission_item.time_inside = 0.0f; } /* if we just did a VTOL takeoff, prepare transition */ diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 044ce7efa47d..dc29daee720b 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -523,9 +523,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi } else { sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; - - // set pitch and ensure that the hold time is zero - sp->pitch_min = item.pitch_min; } break; @@ -606,7 +603,7 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) } void -MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch) +MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude) { item->nav_cmd = NAV_CMD_TAKEOFF; @@ -619,7 +616,6 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude, item->altitude_is_relative = false; item->loiter_radius = _navigator->get_loiter_radius(); - item->pitch_min = min_pitch; item->autocontinue = false; item->origin = ORIGIN_ONBOARD; } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 8eb9f8ea43e4..9b4f09a78976 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -95,7 +95,7 @@ class MissionBlock : public NavigatorMode /** * Set a takeoff mission item */ - void set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch = 0.0f); + void set_takeoff_item(struct mission_item_s *item, float abs_altitude); /** * Set a land mission item diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 7cb7bf2ed79a..befe62d1f7e9 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -139,7 +139,6 @@ struct mission_item_s { struct { union { float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ - float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ float circle_radius; /**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */ }; float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8a0037ecc7c4..427d94ed2f01 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1807,7 +1807,6 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPSP.type = buf.triplet.current.type; log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; LOGBUFFER_WRITE_AND_COUNT(GPSP); } } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 60b58d4fc170..812bcbedf2d4 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -255,7 +255,6 @@ struct log_GPSP_s { uint8_t type; float loiter_radius; int8_t loiter_direction; - float pitch_min; }; /* --- ESC - ESC STATE --- */ @@ -609,8 +608,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "BffffffLLHhB", "ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"), LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), - LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), - LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir"), + LOG_FORMAT(ESC, "HBBBHHffiff", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "fffffffB", "V,VFilt,C,CFilt,Discharged,Remaining,Scale,Warning"), LOG_FORMAT(DIST, "BBBff", "Id,Type,Orientation,Distance,Covariance"),