mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-25 16:56:25 +08:00
L1 position controller: use double precision floating point for all lat/lon
This commit is contained in:
Daniel Agar
committed by
Lorenz Meier
Lorenz Meier
parent
31a6edff07
commit
1981519aad
@@ -46,6 +46,7 @@
|
||||
|
||||
#include <float.h>
|
||||
|
||||
using matrix::Vector2d;
|
||||
using matrix::Vector2f;
|
||||
using matrix::wrap_pi;
|
||||
|
||||
@@ -72,17 +73,15 @@ float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
|
||||
}
|
||||
|
||||
void
|
||||
ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B,
|
||||
const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector)
|
||||
ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector2d &vector_B,
|
||||
const Vector2d &vector_curr_position, const Vector2f &ground_speed_vector)
|
||||
{
|
||||
/* this follows the logic presented in [1] */
|
||||
float eta = 0.0f;
|
||||
float xtrack_vel = 0.0f;
|
||||
float ltrack_vel = 0.0f;
|
||||
|
||||
/* get the direction between the last (visited) and next waypoint */
|
||||
_target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1),
|
||||
(double)vector_B(0), (double)vector_B(1));
|
||||
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0),
|
||||
vector_B(1));
|
||||
|
||||
/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
|
||||
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
|
||||
@@ -132,11 +131,14 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
|
||||
|
||||
/* unit vector from waypoint A to current position */
|
||||
Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
|
||||
|
||||
/* velocity across / orthogonal to line */
|
||||
xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
|
||||
float xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
|
||||
|
||||
/* velocity along line */
|
||||
ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
|
||||
float ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
|
||||
eta = atan2f(xtrack_vel, ltrack_vel);
|
||||
|
||||
/* bearing from current position to L1 point */
|
||||
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
|
||||
|
||||
@@ -158,33 +160,38 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
|
||||
/* calculate eta to fly to waypoint B */
|
||||
|
||||
/* velocity across / orthogonal to line */
|
||||
xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
|
||||
float xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
|
||||
|
||||
/* velocity along line */
|
||||
ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
|
||||
float ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
|
||||
eta = atan2f(xtrack_vel, ltrack_vel);
|
||||
|
||||
/* bearing from current position to L1 point */
|
||||
_nav_bearing = atan2f(-vector_B_to_P_unit(1), -vector_B_to_P_unit(0));
|
||||
|
||||
} else {
|
||||
|
||||
/* calculate eta to fly along the line between A and B */
|
||||
|
||||
/* velocity across / orthogonal to line */
|
||||
xtrack_vel = ground_speed_vector % vector_AB;
|
||||
float xtrack_vel = ground_speed_vector % vector_AB;
|
||||
|
||||
/* velocity along line */
|
||||
ltrack_vel = ground_speed_vector * vector_AB;
|
||||
float ltrack_vel = ground_speed_vector * vector_AB;
|
||||
|
||||
/* calculate eta2 (angle of velocity vector relative to line) */
|
||||
float eta2 = atan2f(xtrack_vel, ltrack_vel);
|
||||
|
||||
/* calculate eta1 (angle to L1 point) */
|
||||
float xtrackErr = vector_A_to_airplane % vector_AB;
|
||||
float sine_eta1 = xtrackErr / math::max(_L1_distance, 0.1f);
|
||||
|
||||
/* limit output to 45 degrees */
|
||||
sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
|
||||
float eta1 = asinf(sine_eta1);
|
||||
eta = eta1 + eta2;
|
||||
|
||||
/* bearing from current position to L1 point */
|
||||
_nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;
|
||||
|
||||
}
|
||||
|
||||
/* limit angle to +-90 degrees */
|
||||
@@ -201,7 +208,7 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
|
||||
}
|
||||
|
||||
void
|
||||
ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius,
|
||||
ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d &vector_curr_position, float radius,
|
||||
int8_t loiter_direction, const Vector2f &ground_speed_vector)
|
||||
{
|
||||
/* the complete guidance logic in this section was proposed by [2] */
|
||||
@@ -212,8 +219,8 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
|
||||
float K_velocity = 2.0f * _L1_damping * omega;
|
||||
|
||||
/* update bearing to next waypoint */
|
||||
_target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1),
|
||||
(double)vector_A(0), (double)vector_A(1));
|
||||
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0),
|
||||
vector_A(1));
|
||||
|
||||
/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
|
||||
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
|
||||
@@ -356,13 +363,17 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
|
||||
update_roll_setpoint();
|
||||
}
|
||||
|
||||
Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2f &origin, const Vector2f &target) const
|
||||
Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2d &origin, const Vector2d &target) const
|
||||
{
|
||||
/* this is an approximation for small angles, proposed by [2] */
|
||||
Vector2f out(math::radians((target(0) - origin(0))),
|
||||
math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
|
||||
const double x_angle = math::radians(target(0) - origin(0));
|
||||
const double y_angle = math::radians(target(1) - origin(1));
|
||||
const double x_origin_cos = cos(math::radians(origin(0)));
|
||||
|
||||
return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
|
||||
return Vector2f{
|
||||
static_cast<float>(x_angle * CONSTANTS_RADIUS_OF_EARTH),
|
||||
static_cast<float>(y_angle *x_origin_cos * CONSTANTS_RADIUS_OF_EARTH),
|
||||
};
|
||||
}
|
||||
|
||||
void ECL_L1_Pos_Controller::set_l1_period(float period)
|
||||
|
||||
@@ -143,8 +143,8 @@ public:
|
||||
*
|
||||
* @return sets _lateral_accel setpoint
|
||||
*/
|
||||
void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B,
|
||||
const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed);
|
||||
void navigate_waypoints(const matrix::Vector2d &vector_A, const matrix::Vector2d &vector_B,
|
||||
const matrix::Vector2d &vector_curr_position, const matrix::Vector2f &ground_speed);
|
||||
|
||||
/**
|
||||
* Navigate on an orbit around a loiter waypoint.
|
||||
@@ -154,7 +154,7 @@ public:
|
||||
*
|
||||
* @return sets _lateral_accel setpoint
|
||||
*/
|
||||
void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius,
|
||||
void navigate_loiter(const matrix::Vector2d &vector_A, const matrix::Vector2d &vector_curr_position, float radius,
|
||||
int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector);
|
||||
|
||||
/**
|
||||
@@ -235,7 +235,7 @@ private:
|
||||
* @param wp The point to convert to into the local coordinates, in WGS84 coordinates
|
||||
* @return The vector in meters pointing from the reference position to the coordinates
|
||||
*/
|
||||
matrix::Vector2f get_local_planar_vector(const matrix::Vector2f &origin, const matrix::Vector2f &target) const;
|
||||
matrix::Vector2f get_local_planar_vector(const matrix::Vector2d &origin, const matrix::Vector2d &target) const;
|
||||
|
||||
/**
|
||||
* Update roll angle setpoint. This will also apply slew rate limits if set.
|
||||
|
||||
+1
-1
Submodule src/lib/matrix updated: 977cf52322...3679f7fd51
@@ -35,6 +35,19 @@
|
||||
|
||||
#include <vtol_att_control/vtol_type.h>
|
||||
|
||||
using math::constrain;
|
||||
using math::max;
|
||||
using math::min;
|
||||
using math::radians;
|
||||
|
||||
using matrix::Dcmf;
|
||||
using matrix::Eulerf;
|
||||
using matrix::Quatf;
|
||||
using matrix::Vector2f;
|
||||
using matrix::Vector2d;
|
||||
using matrix::Vector3f;
|
||||
using matrix::wrap_pi;
|
||||
|
||||
FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
|
||||
@@ -587,7 +600,7 @@ FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_lim
|
||||
}
|
||||
|
||||
bool
|
||||
FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2f &curr_pos,
|
||||
FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
|
||||
{
|
||||
@@ -662,8 +675,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
|
||||
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
|
||||
_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
|
||||
|
||||
Vector2f curr_wp{0.0f, 0.0f};
|
||||
Vector2f prev_wp{0.0f, 0.0f};
|
||||
Vector2d curr_wp{0, 0};
|
||||
Vector2d prev_wp{0, 0};
|
||||
|
||||
if (_vehicle_status.in_transition_to_fw) {
|
||||
|
||||
@@ -674,8 +687,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
|
||||
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition,
|
||||
&lon_transition);
|
||||
|
||||
_transition_waypoint(0) = (float)lat_transition;
|
||||
_transition_waypoint(1) = (float)lon_transition;
|
||||
_transition_waypoint(0) = lat_transition;
|
||||
_transition_waypoint(1) = lon_transition;
|
||||
}
|
||||
|
||||
|
||||
@@ -683,24 +696,25 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
|
||||
|
||||
} else {
|
||||
/* current waypoint (the one currently heading for) */
|
||||
curr_wp = Vector2f((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
|
||||
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
|
||||
|
||||
if (pos_sp_prev.valid) {
|
||||
prev_wp(0) = (float)pos_sp_prev.lat;
|
||||
prev_wp(1) = (float)pos_sp_prev.lon;
|
||||
prev_wp(0) = pos_sp_prev.lat;
|
||||
prev_wp(1) = 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) = pos_sp_curr.lat;
|
||||
prev_wp(1) = pos_sp_curr.lon;
|
||||
}
|
||||
|
||||
|
||||
/* reset transition waypoint, will be set upon entering front transition */
|
||||
_transition_waypoint.setNaN();
|
||||
_transition_waypoint(0) = static_cast<double>(NAN);
|
||||
_transition_waypoint(1) = static_cast<double>(NAN);
|
||||
}
|
||||
|
||||
/* Initialize attitude controller integrator reset flags to 0 */
|
||||
@@ -1007,8 +1021,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
|
||||
get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, 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};
|
||||
Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_wp.lon};
|
||||
Vector2d curr_wp{_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon};
|
||||
|
||||
/* populate l1 control setpoint */
|
||||
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
|
||||
@@ -1157,24 +1171,24 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f &curr_pos,
|
||||
FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
||||
{
|
||||
/* current waypoint (the one currently heading for) */
|
||||
Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
|
||||
Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */
|
||||
Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
|
||||
Vector2d prev_wp{0, 0}; /* previous waypoint */
|
||||
|
||||
if (pos_sp_prev.valid) {
|
||||
prev_wp(0) = (float)pos_sp_prev.lat;
|
||||
prev_wp(1) = (float)pos_sp_prev.lon;
|
||||
prev_wp(0) = pos_sp_prev.lat;
|
||||
prev_wp(1) = 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) = pos_sp_curr.lat;
|
||||
prev_wp(1) = pos_sp_curr.lon;
|
||||
}
|
||||
|
||||
// apply flaps for takeoff according to the corresponding scale factor set
|
||||
@@ -1326,24 +1340,24 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2f &curr_pos,
|
||||
FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
||||
{
|
||||
/* current waypoint (the one currently heading for) */
|
||||
Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
|
||||
Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */
|
||||
Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
|
||||
Vector2d prev_wp{0, 0}; /* previous waypoint */
|
||||
|
||||
if (pos_sp_prev.valid) {
|
||||
prev_wp(0) = (float)pos_sp_prev.lat;
|
||||
prev_wp(1) = (float)pos_sp_prev.lon;
|
||||
prev_wp(0) = pos_sp_prev.lat;
|
||||
prev_wp(1) = 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) = pos_sp_curr.lat;
|
||||
prev_wp(1) = pos_sp_curr.lon;
|
||||
}
|
||||
|
||||
// apply full flaps for landings. this flag will also trigger the use of flaperons
|
||||
@@ -1392,8 +1406,8 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2f
|
||||
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;
|
||||
curr_wp(0) = lat;
|
||||
curr_wp(1) = lon;
|
||||
}
|
||||
|
||||
// we want the plane to keep tracking the desired flight path until we start flaring
|
||||
@@ -1693,7 +1707,7 @@ FixedwingPositionControl::Run()
|
||||
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
|
||||
Vector2f curr_pos((float)_current_latitude, (float)_current_longitude);
|
||||
Vector2d curr_pos(_current_latitude, _current_longitude);
|
||||
Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
|
||||
|
||||
//Convert Local setpoints to global setpoints
|
||||
|
||||
@@ -89,22 +89,13 @@
|
||||
#include <uORB/uORB.h>
|
||||
#include <vtol_att_control/vtol_type.h>
|
||||
|
||||
using math::constrain;
|
||||
using math::max;
|
||||
using math::min;
|
||||
using math::radians;
|
||||
|
||||
using matrix::Dcmf;
|
||||
using matrix::Eulerf;
|
||||
using matrix::Quatf;
|
||||
using matrix::Vector2f;
|
||||
using matrix::Vector3f;
|
||||
using matrix::wrap_pi;
|
||||
|
||||
using namespace launchdetection;
|
||||
using namespace runwaytakeoff;
|
||||
using namespace time_literals;
|
||||
|
||||
using matrix::Vector2d;
|
||||
using matrix::Vector2f;
|
||||
|
||||
static constexpr float HDG_HOLD_DIST_NEXT =
|
||||
3000.0f; // initial distance of waypoint in front of plane in heading hold mode
|
||||
static constexpr float HDG_HOLD_REACHED_DIST =
|
||||
@@ -248,7 +239,7 @@ private:
|
||||
|
||||
bool _vtol_tailsitter{false};
|
||||
|
||||
Vector2f _transition_waypoint{NAN, NAN};
|
||||
matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
|
||||
|
||||
// estimator reset counters
|
||||
uint8_t _pos_reset_counter{0}; ///< captures the number of times the estimator has reset the horizontal position
|
||||
@@ -324,13 +315,13 @@ private:
|
||||
*/
|
||||
void update_desired_altitude(float dt);
|
||||
|
||||
bool control_position(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed,
|
||||
bool control_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_prev,
|
||||
const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
|
||||
void control_takeoff(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed,
|
||||
void control_takeoff(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_prev,
|
||||
const position_setpoint_s &pos_sp_curr);
|
||||
void control_landing(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed,
|
||||
void control_landing(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_prev,
|
||||
const position_setpoint_s &pos_sp_curr);
|
||||
|
||||
|
||||
@@ -46,7 +46,6 @@
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
using matrix::Vector2f;
|
||||
using namespace time_literals;
|
||||
|
||||
namespace runwaytakeoff
|
||||
@@ -69,8 +68,8 @@ void RunwayTakeoff::init(const hrt_abstime &now, float yaw, double current_lat,
|
||||
_state = RunwayTakeoffState::THROTTLE_RAMP;
|
||||
_initialized_time = now;
|
||||
_climbout = true; // this is true until climbout is finished
|
||||
_start_wp(0) = (float)current_lat;
|
||||
_start_wp(1) = (float)current_lon;
|
||||
_start_wp(0) = current_lat;
|
||||
_start_wp(1) = current_lon;
|
||||
}
|
||||
|
||||
void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl,
|
||||
@@ -103,8 +102,8 @@ void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl
|
||||
* The navigator will take this as starting point to navigate towards the takeoff WP.
|
||||
*/
|
||||
if (_param_rwto_hdg.get() == 0) {
|
||||
_start_wp(0) = (float)current_lat;
|
||||
_start_wp(1) = (float)current_lon;
|
||||
_start_wp(0) = current_lat;
|
||||
_start_wp(1) = current_lon;
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_log_pub, "#Climbout");
|
||||
@@ -248,14 +247,6 @@ float RunwayTakeoff::getMaxPitch(float max)
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Returns the "previous" (start) WP for navigation.
|
||||
*/
|
||||
Vector2f RunwayTakeoff::getStartWP()
|
||||
{
|
||||
return _start_wp;
|
||||
}
|
||||
|
||||
void RunwayTakeoff::reset()
|
||||
{
|
||||
_initialized = false;
|
||||
|
||||
@@ -88,7 +88,7 @@ public:
|
||||
bool resetIntegrators();
|
||||
float getMinPitch(float climbout_min, float min);
|
||||
float getMaxPitch(float max);
|
||||
matrix::Vector2f getStartWP();
|
||||
const matrix::Vector2d &getStartWP() const { return _start_wp; };
|
||||
|
||||
void reset();
|
||||
|
||||
@@ -99,7 +99,7 @@ private:
|
||||
hrt_abstime _initialized_time{0};
|
||||
float _init_yaw{0.f};
|
||||
bool _climbout{false};
|
||||
matrix::Vector2f _start_wp;
|
||||
matrix::Vector2d _start_wp;
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamBool<px4::params::RWTO_TKOFF>) _param_rwto_tkoff,
|
||||
|
||||
@@ -149,7 +149,7 @@ RoverPositionControl::vehicle_attitude_poll()
|
||||
}
|
||||
|
||||
bool
|
||||
RoverPositionControl::control_position(const matrix::Vector2f ¤t_position,
|
||||
RoverPositionControl::control_position(const matrix::Vector2d ¤t_position,
|
||||
const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
|
||||
{
|
||||
float dt = 0.01; // Using non zero value to a avoid division by zero
|
||||
@@ -172,14 +172,14 @@ RoverPositionControl::control_position(const matrix::Vector2f ¤t_position,
|
||||
//bool was_circle_mode = _gnd_control.circle_mode();
|
||||
|
||||
/* current waypoint (the one currently heading for) */
|
||||
matrix::Vector2f curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
|
||||
matrix::Vector2d curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
|
||||
|
||||
/* previous waypoint */
|
||||
matrix::Vector2f prev_wp = curr_wp;
|
||||
matrix::Vector2d prev_wp = curr_wp;
|
||||
|
||||
if (pos_sp_triplet.previous.valid) {
|
||||
prev_wp(0) = (float)pos_sp_triplet.previous.lat;
|
||||
prev_wp(1) = (float)pos_sp_triplet.previous.lon;
|
||||
prev_wp(0) = pos_sp_triplet.previous.lat;
|
||||
prev_wp(1) = pos_sp_triplet.previous.lon;
|
||||
}
|
||||
|
||||
matrix::Vector2f ground_speed_2d(ground_speed);
|
||||
@@ -433,7 +433,7 @@ RoverPositionControl::run()
|
||||
_pos_reset_counter = _global_pos.lat_lon_reset_counter;
|
||||
|
||||
matrix::Vector3f ground_speed(_local_pos.vx, _local_pos.vy, _local_pos.vz);
|
||||
matrix::Vector2f current_position((float)_global_pos.lat, (float)_global_pos.lon);
|
||||
matrix::Vector2d current_position(_global_pos.lat, _global_pos.lon);
|
||||
matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz);
|
||||
|
||||
if (!manual_mode && _control_mode.flag_control_position_enabled) {
|
||||
|
||||
@@ -150,7 +150,7 @@ private:
|
||||
} _pos_ctrl_state {STOPPING}; /// Position control state machine
|
||||
|
||||
/* previous waypoint */
|
||||
matrix::Vector2f _prev_wp{0.0f, 0.0f};
|
||||
matrix::Vector2d _prev_wp{0, 0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::GND_L1_PERIOD>) _param_l1_period,
|
||||
@@ -190,7 +190,7 @@ private:
|
||||
/**
|
||||
* Control position.
|
||||
*/
|
||||
bool control_position(const matrix::Vector2f &global_pos, const matrix::Vector3f &ground_speed,
|
||||
bool control_position(const matrix::Vector2d &global_pos, const matrix::Vector3f &ground_speed,
|
||||
const position_setpoint_triplet_s &_pos_sp_triplet);
|
||||
void control_velocity(const matrix::Vector3f ¤t_velocity, const position_setpoint_triplet_s &pos_sp_triplet);
|
||||
void control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);
|
||||
|
||||
Reference in New Issue
Block a user