mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-05 10:38:52 +08:00
Multiplatform controllers: Move to examples and fix code style
This commit is contained in:
Lorenz Meier
+25
-22
@@ -72,28 +72,29 @@ MulticopterAttitudeControlMultiplatform::MulticopterAttitudeControlMultiplatform
|
||||
_n(_appState),
|
||||
|
||||
/* parameters */
|
||||
_params_handles({
|
||||
.roll_p = px4::ParameterFloat("MP_ROLL_P", PARAM_MP_ROLL_P_DEFAULT),
|
||||
.roll_rate_p = px4::ParameterFloat("MP_ROLLRATE_P", PARAM_MP_ROLLRATE_P_DEFAULT),
|
||||
.roll_rate_i = px4::ParameterFloat("MP_ROLLRATE_I", PARAM_MP_ROLLRATE_I_DEFAULT),
|
||||
_params_handles(
|
||||
{
|
||||
.roll_p = px4::ParameterFloat("MP_ROLL_P", PARAM_MP_ROLL_P_DEFAULT),
|
||||
.roll_rate_p = px4::ParameterFloat("MP_ROLLRATE_P", PARAM_MP_ROLLRATE_P_DEFAULT),
|
||||
.roll_rate_i = px4::ParameterFloat("MP_ROLLRATE_I", PARAM_MP_ROLLRATE_I_DEFAULT),
|
||||
.roll_rate_d = px4::ParameterFloat("MP_ROLLRATE_D", PARAM_MP_ROLLRATE_D_DEFAULT),
|
||||
.pitch_p = px4::ParameterFloat("MP_PITCH_P", PARAM_MP_PITCH_P_DEFAULT),
|
||||
.pitch_rate_p = px4::ParameterFloat("MP_PITCHRATE_P", PARAM_MP_PITCHRATE_P_DEFAULT),
|
||||
.pitch_rate_i = px4::ParameterFloat("MP_PITCHRATE_I", PARAM_MP_PITCHRATE_I_DEFAULT),
|
||||
.pitch_rate_d = px4::ParameterFloat("MP_PITCHRATE_D", PARAM_MP_PITCHRATE_D_DEFAULT),
|
||||
.yaw_p = px4::ParameterFloat("MP_YAW_P", PARAM_MP_YAW_P_DEFAULT),
|
||||
.yaw_rate_p = px4::ParameterFloat("MP_YAWRATE_P", PARAM_MP_YAWRATE_P_DEFAULT),
|
||||
.yaw_rate_i = px4::ParameterFloat("MP_YAWRATE_I", PARAM_MP_YAWRATE_I_DEFAULT),
|
||||
.yaw_rate_d = px4::ParameterFloat("MP_YAWRATE_D", PARAM_MP_YAWRATE_D_DEFAULT),
|
||||
.yaw_ff = px4::ParameterFloat("MP_YAW_FF", PARAM_MP_YAW_FF_DEFAULT),
|
||||
.yaw_rate_max = px4::ParameterFloat("MP_YAWRATE_MAX", PARAM_MP_YAWRATE_MAX_DEFAULT),
|
||||
.acro_roll_max = px4::ParameterFloat("MP_ACRO_R_MAX", PARAM_MP_ACRO_R_MAX_DEFAULT),
|
||||
.acro_pitch_max = px4::ParameterFloat("MP_ACRO_P_MAX", PARAM_MP_ACRO_P_MAX_DEFAULT),
|
||||
.acro_yaw_max = px4::ParameterFloat("MP_ACRO_Y_MAX", PARAM_MP_ACRO_Y_MAX_DEFAULT)
|
||||
}),
|
||||
.pitch_p = px4::ParameterFloat("MP_PITCH_P", PARAM_MP_PITCH_P_DEFAULT),
|
||||
.pitch_rate_p = px4::ParameterFloat("MP_PITCHRATE_P", PARAM_MP_PITCHRATE_P_DEFAULT),
|
||||
.pitch_rate_i = px4::ParameterFloat("MP_PITCHRATE_I", PARAM_MP_PITCHRATE_I_DEFAULT),
|
||||
.pitch_rate_d = px4::ParameterFloat("MP_PITCHRATE_D", PARAM_MP_PITCHRATE_D_DEFAULT),
|
||||
.yaw_p = px4::ParameterFloat("MP_YAW_P", PARAM_MP_YAW_P_DEFAULT),
|
||||
.yaw_rate_p = px4::ParameterFloat("MP_YAWRATE_P", PARAM_MP_YAWRATE_P_DEFAULT),
|
||||
.yaw_rate_i = px4::ParameterFloat("MP_YAWRATE_I", PARAM_MP_YAWRATE_I_DEFAULT),
|
||||
.yaw_rate_d = px4::ParameterFloat("MP_YAWRATE_D", PARAM_MP_YAWRATE_D_DEFAULT),
|
||||
.yaw_ff = px4::ParameterFloat("MP_YAW_FF", PARAM_MP_YAW_FF_DEFAULT),
|
||||
.yaw_rate_max = px4::ParameterFloat("MP_YAWRATE_MAX", PARAM_MP_YAWRATE_MAX_DEFAULT),
|
||||
.acro_roll_max = px4::ParameterFloat("MP_ACRO_R_MAX", PARAM_MP_ACRO_R_MAX_DEFAULT),
|
||||
.acro_pitch_max = px4::ParameterFloat("MP_ACRO_P_MAX", PARAM_MP_ACRO_P_MAX_DEFAULT),
|
||||
.acro_yaw_max = px4::ParameterFloat("MP_ACRO_Y_MAX", PARAM_MP_ACRO_Y_MAX_DEFAULT)
|
||||
}),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
|
||||
|
||||
{
|
||||
/* fetch initial parameter values */
|
||||
@@ -107,7 +108,7 @@ MulticopterAttitudeControlMultiplatform::MulticopterAttitudeControlMultiplatform
|
||||
_v_rates_sp = _n.subscribe<px4_vehicle_rates_setpoint>(0);
|
||||
_v_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
|
||||
_parameter_update = _n.subscribe<px4_parameter_update>(
|
||||
&MulticopterAttitudeControlMultiplatform::handle_parameter_update, this, 1000);
|
||||
&MulticopterAttitudeControlMultiplatform::handle_parameter_update, this, 1000);
|
||||
_manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
|
||||
_armed = _n.subscribe<px4_actuator_armed>(0);
|
||||
_v_status = _n.subscribe<px4_vehicle_status>(0);
|
||||
@@ -156,7 +157,8 @@ void MulticopterAttitudeControlMultiplatform::handle_parameter_update(const px4_
|
||||
parameters_update();
|
||||
}
|
||||
|
||||
void MulticopterAttitudeControlMultiplatform::handle_vehicle_attitude(const px4_vehicle_attitude &msg) {
|
||||
void MulticopterAttitudeControlMultiplatform::handle_vehicle_attitude(const px4_vehicle_attitude &msg)
|
||||
{
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
@@ -185,6 +187,7 @@ void MulticopterAttitudeControlMultiplatform::handle_vehicle_attitude(const px4
|
||||
|
||||
if (_v_rates_sp_pub != nullptr) {
|
||||
_v_rates_sp_pub->publish(_v_rates_sp_mod);
|
||||
|
||||
} else {
|
||||
if (_v_status->data().is_vtol) {
|
||||
//XXX add a second publisher?
|
||||
+5
-5
@@ -70,11 +70,11 @@ int mc_att_control_m_main(int argc, char *argv[])
|
||||
mc_att_control_task_should_exit = false;
|
||||
warnx("ok now btak running");
|
||||
mc_att_control_daemon_task = px4_task_spawn_cmd("mc_att_control_m",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
1900,
|
||||
mc_att_control_start_main,
|
||||
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
1900,
|
||||
mc_att_control_start_main,
|
||||
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
|
||||
|
||||
return 0;
|
||||
}
|
||||
+87
-70
@@ -68,37 +68,38 @@ MulticopterPositionControlMultiplatform::MulticopterPositionControlMultiplatform
|
||||
_n(_appState),
|
||||
|
||||
/* parameters */
|
||||
_params_handles({
|
||||
.thr_min = px4::ParameterFloat("MPP_THR_MIN", PARAM_MPP_THR_MIN_DEFAULT),
|
||||
_params_handles(
|
||||
{
|
||||
.thr_min = px4::ParameterFloat("MPP_THR_MIN", PARAM_MPP_THR_MIN_DEFAULT),
|
||||
.thr_max = px4::ParameterFloat("MPP_THR_MAX", PARAM_MPP_THR_MAX_DEFAULT),
|
||||
.z_p = px4::ParameterFloat("MPP_Z_P", PARAM_MPP_Z_P_DEFAULT),
|
||||
.z_vel_p = px4::ParameterFloat("MPP_Z_VEL_P", PARAM_MPP_Z_VEL_P_DEFAULT),
|
||||
.z_vel_i = px4::ParameterFloat("MPP_Z_VEL_I", PARAM_MPP_Z_VEL_I_DEFAULT),
|
||||
.z_vel_d = px4::ParameterFloat("MPP_Z_VEL_D", PARAM_MPP_Z_VEL_D_DEFAULT),
|
||||
.z_vel_max = px4::ParameterFloat("MPP_Z_VEL_MAX", PARAM_MPP_Z_VEL_MAX_DEFAULT),
|
||||
.z_ff = px4::ParameterFloat("MPP_Z_FF", PARAM_MPP_Z_FF_DEFAULT),
|
||||
.xy_p = px4::ParameterFloat("MPP_XY_P", PARAM_MPP_XY_P_DEFAULT),
|
||||
.xy_vel_p = px4::ParameterFloat("MPP_XY_VEL_P", PARAM_MPP_XY_VEL_P_DEFAULT),
|
||||
.xy_vel_i = px4::ParameterFloat("MPP_XY_VEL_I", PARAM_MPP_XY_VEL_I_DEFAULT),
|
||||
.xy_vel_d = px4::ParameterFloat("MPP_XY_VEL_D", PARAM_MPP_XY_VEL_D_DEFAULT),
|
||||
.xy_vel_max = px4::ParameterFloat("MPP_XY_VEL_MAX", PARAM_MPP_XY_VEL_MAX_DEFAULT),
|
||||
.xy_ff = px4::ParameterFloat("MPP_XY_FF", PARAM_MPP_XY_FF_DEFAULT),
|
||||
.tilt_max_air = px4::ParameterFloat("MPP_TILTMAX_AIR", PARAM_MPP_TILTMAX_AIR_DEFAULT),
|
||||
.land_speed = px4::ParameterFloat("MPP_LAND_SPEED", PARAM_MPP_LAND_SPEED_DEFAULT),
|
||||
.tilt_max_land = px4::ParameterFloat("MPP_TILTMAX_LND", PARAM_MPP_TILTMAX_LND_DEFAULT),
|
||||
.man_roll_max = px4::ParameterFloat("MPP_MAN_R_MAX", PARAM_MPP_MAN_R_MAX_DEFAULT),
|
||||
.man_pitch_max = px4::ParameterFloat("MPP_MAN_P_MAX", PARAM_MPP_MAN_P_MAX_DEFAULT),
|
||||
.man_yaw_max = px4::ParameterFloat("MPP_MAN_Y_MAX", PARAM_MPP_MAN_Y_MAX_DEFAULT),
|
||||
.mc_att_yaw_p = px4::ParameterFloat("MP_YAW_P", PARAM_MP_YAW_P_DEFAULT)
|
||||
}),
|
||||
_ref_alt(0.0f),
|
||||
_ref_timestamp(0),
|
||||
.z_p = px4::ParameterFloat("MPP_Z_P", PARAM_MPP_Z_P_DEFAULT),
|
||||
.z_vel_p = px4::ParameterFloat("MPP_Z_VEL_P", PARAM_MPP_Z_VEL_P_DEFAULT),
|
||||
.z_vel_i = px4::ParameterFloat("MPP_Z_VEL_I", PARAM_MPP_Z_VEL_I_DEFAULT),
|
||||
.z_vel_d = px4::ParameterFloat("MPP_Z_VEL_D", PARAM_MPP_Z_VEL_D_DEFAULT),
|
||||
.z_vel_max = px4::ParameterFloat("MPP_Z_VEL_MAX", PARAM_MPP_Z_VEL_MAX_DEFAULT),
|
||||
.z_ff = px4::ParameterFloat("MPP_Z_FF", PARAM_MPP_Z_FF_DEFAULT),
|
||||
.xy_p = px4::ParameterFloat("MPP_XY_P", PARAM_MPP_XY_P_DEFAULT),
|
||||
.xy_vel_p = px4::ParameterFloat("MPP_XY_VEL_P", PARAM_MPP_XY_VEL_P_DEFAULT),
|
||||
.xy_vel_i = px4::ParameterFloat("MPP_XY_VEL_I", PARAM_MPP_XY_VEL_I_DEFAULT),
|
||||
.xy_vel_d = px4::ParameterFloat("MPP_XY_VEL_D", PARAM_MPP_XY_VEL_D_DEFAULT),
|
||||
.xy_vel_max = px4::ParameterFloat("MPP_XY_VEL_MAX", PARAM_MPP_XY_VEL_MAX_DEFAULT),
|
||||
.xy_ff = px4::ParameterFloat("MPP_XY_FF", PARAM_MPP_XY_FF_DEFAULT),
|
||||
.tilt_max_air = px4::ParameterFloat("MPP_TILTMAX_AIR", PARAM_MPP_TILTMAX_AIR_DEFAULT),
|
||||
.land_speed = px4::ParameterFloat("MPP_LAND_SPEED", PARAM_MPP_LAND_SPEED_DEFAULT),
|
||||
.tilt_max_land = px4::ParameterFloat("MPP_TILTMAX_LND", PARAM_MPP_TILTMAX_LND_DEFAULT),
|
||||
.man_roll_max = px4::ParameterFloat("MPP_MAN_R_MAX", PARAM_MPP_MAN_R_MAX_DEFAULT),
|
||||
.man_pitch_max = px4::ParameterFloat("MPP_MAN_P_MAX", PARAM_MPP_MAN_P_MAX_DEFAULT),
|
||||
.man_yaw_max = px4::ParameterFloat("MPP_MAN_Y_MAX", PARAM_MPP_MAN_Y_MAX_DEFAULT),
|
||||
.mc_att_yaw_p = px4::ParameterFloat("MP_YAW_P", PARAM_MP_YAW_P_DEFAULT)
|
||||
}),
|
||||
_ref_alt(0.0f),
|
||||
_ref_timestamp(0),
|
||||
|
||||
_reset_pos_sp(true),
|
||||
_reset_alt_sp(true),
|
||||
_mode_auto(false),
|
||||
_thrust_int(),
|
||||
_R()
|
||||
_reset_pos_sp(true),
|
||||
_reset_alt_sp(true),
|
||||
_mode_auto(false),
|
||||
_thrust_int(),
|
||||
_R()
|
||||
{
|
||||
memset(&_ref_pos, 0, sizeof(_ref_pos));
|
||||
|
||||
@@ -108,11 +109,12 @@ MulticopterPositionControlMultiplatform::MulticopterPositionControlMultiplatform
|
||||
_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterPositionControlMultiplatform::handle_vehicle_attitude, this, 0);
|
||||
_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
|
||||
_parameter_update = _n.subscribe<px4_parameter_update>(
|
||||
&MulticopterPositionControlMultiplatform::handle_parameter_update, this, 1000);
|
||||
&MulticopterPositionControlMultiplatform::handle_parameter_update, this, 1000);
|
||||
_manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
|
||||
_armed = _n.subscribe<px4_actuator_armed>(0);
|
||||
_local_pos = _n.subscribe<px4_vehicle_local_position>(0);
|
||||
_pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(&MulticopterPositionControlMultiplatform::handle_position_setpoint_triplet, this, 0);
|
||||
_pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>
|
||||
(&MulticopterPositionControlMultiplatform::handle_position_setpoint_triplet, this, 0);
|
||||
_local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0);
|
||||
_global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(0);
|
||||
|
||||
@@ -226,10 +228,12 @@ MulticopterPositionControlMultiplatform::reset_pos_sp()
|
||||
if (_reset_pos_sp) {
|
||||
_reset_pos_sp = false;
|
||||
/* shift position setpoint to make attitude setpoint continuous */
|
||||
_pos_sp(0) = _pos(0); //+ (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0)
|
||||
// - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
|
||||
_pos_sp(1) = _pos(1); //+ (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1)
|
||||
// - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
|
||||
_pos_sp(0) = _pos(
|
||||
0); //+ (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0)
|
||||
// - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
|
||||
_pos_sp(1) = _pos(
|
||||
1); //+ (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1)
|
||||
// - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
|
||||
|
||||
//XXX: port this once a mavlink like interface is available
|
||||
// mavlink_log_info(&_mavlink_log_pub, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
|
||||
@@ -350,6 +354,7 @@ MulticopterPositionControlMultiplatform::control_offboard(float dt)
|
||||
/* control position */
|
||||
_pos_sp(0) = _pos_sp_triplet->data().current.x;
|
||||
_pos_sp(1) = _pos_sp_triplet->data().current.y;
|
||||
|
||||
} else if (_control_mode->data().flag_control_velocity_enabled && _pos_sp_triplet->data().current.velocity_valid) {
|
||||
/* control velocity */
|
||||
/* reset position setpoint to current position if needed */
|
||||
@@ -362,6 +367,7 @@ MulticopterPositionControlMultiplatform::control_offboard(float dt)
|
||||
|
||||
if (_pos_sp_triplet->data().current.yaw_valid) {
|
||||
_att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw;
|
||||
|
||||
} else if (_pos_sp_triplet->data().current.yawspeed_valid) {
|
||||
_att_sp_msg.data().yaw_body = _att_sp_msg.data().yaw_body + _pos_sp_triplet->data().current.yawspeed * dt;
|
||||
}
|
||||
@@ -369,6 +375,7 @@ MulticopterPositionControlMultiplatform::control_offboard(float dt)
|
||||
if (_control_mode->data().flag_control_altitude_enabled && _pos_sp_triplet->data().current.position_valid) {
|
||||
/* Control altitude */
|
||||
_pos_sp(2) = _pos_sp_triplet->data().current.z;
|
||||
|
||||
} else if (_control_mode->data().flag_control_climb_rate_enabled && _pos_sp_triplet->data().current.velocity_valid) {
|
||||
/* reset alt setpoint to current altitude if needed */
|
||||
reset_alt_sp();
|
||||
@@ -390,8 +397,8 @@ MulticopterPositionControlMultiplatform::control_offboard(float dt)
|
||||
}
|
||||
|
||||
bool
|
||||
MulticopterPositionControlMultiplatform::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
|
||||
const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res)
|
||||
MulticopterPositionControlMultiplatform::cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r,
|
||||
const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res)
|
||||
{
|
||||
/* project center of sphere on line */
|
||||
/* normalized AB */
|
||||
@@ -445,12 +452,13 @@ MulticopterPositionControlMultiplatform::control_auto(float dt)
|
||||
/* by default use current setpoint as is */
|
||||
math::Vector<3> pos_sp_s = curr_sp_s;
|
||||
|
||||
if (_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_POSITION && _pos_sp_triplet->data().previous.valid) {
|
||||
if (_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_POSITION
|
||||
&& _pos_sp_triplet->data().previous.valid) {
|
||||
/* follow "previous - current" line */
|
||||
math::Vector<3> prev_sp;
|
||||
map_projection_project(&_ref_pos,
|
||||
_pos_sp_triplet->data().previous.lat, _pos_sp_triplet->data().previous.lon,
|
||||
&prev_sp.data[0], &prev_sp.data[1]);
|
||||
_pos_sp_triplet->data().previous.lat, _pos_sp_triplet->data().previous.lon,
|
||||
&prev_sp.data[0], &prev_sp.data[1]);
|
||||
prev_sp(2) = -(_pos_sp_triplet->data().previous.alt - _ref_alt);
|
||||
|
||||
if ((curr_sp - prev_sp).length() > MIN_DIST) {
|
||||
@@ -461,14 +469,15 @@ MulticopterPositionControlMultiplatform::control_auto(float dt)
|
||||
math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
|
||||
math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
|
||||
float curr_pos_s_len = curr_pos_s.length();
|
||||
|
||||
if (curr_pos_s_len < 1.0f) {
|
||||
/* copter is closer to waypoint than L1 radius */
|
||||
/* check next waypoint and use it to avoid slowing down when passing via waypoint */
|
||||
if (_pos_sp_triplet->data().next.valid) {
|
||||
math::Vector<3> next_sp;
|
||||
map_projection_project(&_ref_pos,
|
||||
_pos_sp_triplet->data().next.lat, _pos_sp_triplet->data().next.lon,
|
||||
&next_sp.data[0], &next_sp.data[1]);
|
||||
_pos_sp_triplet->data().next.lat, _pos_sp_triplet->data().next.lon,
|
||||
&next_sp.data[0], &next_sp.data[1]);
|
||||
next_sp(2) = -(_pos_sp_triplet->data().next.alt - _ref_alt);
|
||||
|
||||
if ((next_sp - curr_sp).length() > MIN_DIST) {
|
||||
@@ -486,6 +495,7 @@ MulticopterPositionControlMultiplatform::control_auto(float dt)
|
||||
|
||||
if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
|
||||
float curr_next_s_len = curr_next_s.length();
|
||||
|
||||
/* if curr - next distance is larger than L1 radius, limit it */
|
||||
if (curr_next_s_len > 1.0f) {
|
||||
cos_a_curr_next /= curr_next_s_len;
|
||||
@@ -493,8 +503,8 @@ MulticopterPositionControlMultiplatform::control_auto(float dt)
|
||||
|
||||
/* feed forward position setpoint offset */
|
||||
math::Vector<3> pos_ff = prev_curr_s_norm *
|
||||
cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
|
||||
(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
|
||||
cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
|
||||
(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
|
||||
pos_sp_s += pos_ff;
|
||||
}
|
||||
}
|
||||
@@ -502,6 +512,7 @@ MulticopterPositionControlMultiplatform::control_auto(float dt)
|
||||
|
||||
} else {
|
||||
bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
|
||||
|
||||
if (near) {
|
||||
/* L1 sphere crosses trajectory */
|
||||
|
||||
@@ -529,6 +540,7 @@ MulticopterPositionControlMultiplatform::control_auto(float dt)
|
||||
/* difference between current and desired position setpoints, 1 = max speed */
|
||||
math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
|
||||
float d_pos_m_len = d_pos_m.length();
|
||||
|
||||
if (d_pos_m_len > dt) {
|
||||
pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
|
||||
}
|
||||
@@ -555,8 +567,8 @@ void MulticopterPositionControlMultiplatform::handle_position_setpoint_triplet(c
|
||||
{
|
||||
/* Make sure that the position setpoint is valid */
|
||||
if (!PX4_ISFINITE(_pos_sp_triplet->data().current.lat) ||
|
||||
!PX4_ISFINITE(_pos_sp_triplet->data().current.lon) ||
|
||||
!PX4_ISFINITE(_pos_sp_triplet->data().current.alt)) {
|
||||
!PX4_ISFINITE(_pos_sp_triplet->data().current.lon) ||
|
||||
!PX4_ISFINITE(_pos_sp_triplet->data().current.alt)) {
|
||||
_pos_sp_triplet->data().current.valid = false;
|
||||
}
|
||||
}
|
||||
@@ -589,9 +601,9 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
|
||||
update_ref();
|
||||
|
||||
if (_control_mode->data().flag_control_altitude_enabled ||
|
||||
_control_mode->data().flag_control_position_enabled ||
|
||||
_control_mode->data().flag_control_climb_rate_enabled ||
|
||||
_control_mode->data().flag_control_velocity_enabled) {
|
||||
_control_mode->data().flag_control_position_enabled ||
|
||||
_control_mode->data().flag_control_climb_rate_enabled ||
|
||||
_control_mode->data().flag_control_velocity_enabled) {
|
||||
|
||||
_pos(0) = _local_pos->data().x;
|
||||
_pos(1) = _local_pos->data().y;
|
||||
@@ -620,7 +632,8 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
|
||||
control_auto(dt);
|
||||
}
|
||||
|
||||
if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) {
|
||||
if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid
|
||||
&& _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) {
|
||||
/* idle state, don't run controller and set zero thrust */
|
||||
_R.identity();
|
||||
memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body));
|
||||
@@ -648,18 +661,20 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
|
||||
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
|
||||
|
||||
/* make sure velocity setpoint is saturated in xy*/
|
||||
float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) +
|
||||
_vel_sp(1)*_vel_sp(1));
|
||||
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
|
||||
_vel_sp(1) * _vel_sp(1));
|
||||
|
||||
if (vel_norm_xy > _params.vel_max(0)) {
|
||||
/* note assumes vel_max(0) == vel_max(1) */
|
||||
_vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy;
|
||||
_vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy;
|
||||
_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
|
||||
_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
|
||||
}
|
||||
|
||||
/* make sure velocity setpoint is saturated in z*/
|
||||
float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2));
|
||||
float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2));
|
||||
|
||||
if (vel_norm_z > _params.vel_max(2)) {
|
||||
_vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z;
|
||||
_vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z;
|
||||
}
|
||||
|
||||
if (!_control_mode->data().flag_control_altitude_enabled) {
|
||||
@@ -674,7 +689,8 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
|
||||
}
|
||||
|
||||
/* use constant descend rate when landing, ignore altitude setpoint */
|
||||
if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) {
|
||||
if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid
|
||||
&& _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) {
|
||||
_vel_sp(2) = _params.land_speed;
|
||||
}
|
||||
|
||||
@@ -761,7 +777,7 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
|
||||
|
||||
/* adjust limits for landing mode */
|
||||
if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid &&
|
||||
_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) {
|
||||
_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) {
|
||||
/* limit max tilt and min lift when landing */
|
||||
tilt_max = _params.tilt_max_land;
|
||||
|
||||
@@ -984,7 +1000,7 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
|
||||
}
|
||||
|
||||
/* generate attitude setpoint from manual controls */
|
||||
if(_control_mode->data().flag_control_manual_enabled && _control_mode->data().flag_control_attitude_enabled) {
|
||||
if (_control_mode->data().flag_control_manual_enabled && _control_mode->data().flag_control_attitude_enabled) {
|
||||
|
||||
/* reset yaw setpoint to current position if needed */
|
||||
if (reset_yaw_sp) {
|
||||
@@ -993,13 +1009,13 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
|
||||
}
|
||||
|
||||
/* do not move yaw while arming */
|
||||
else if (_manual_control_sp->data().z > 0.1f)
|
||||
{
|
||||
else if (_manual_control_sp->data().z > 0.1f) {
|
||||
const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p;
|
||||
|
||||
_att_sp_msg.data().yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
|
||||
_att_sp_msg.data().yaw_body = _wrap_pi(_att_sp_msg.data().yaw_body + _att_sp_msg.data().yaw_sp_move_rate * dt);
|
||||
float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw);
|
||||
|
||||
if (yaw_offs < - YAW_OFFSET_MAX) {
|
||||
_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - YAW_OFFSET_MAX);
|
||||
|
||||
@@ -1009,24 +1025,24 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
|
||||
}
|
||||
|
||||
/* Control roll and pitch directly if we no aiding velocity controller is active */
|
||||
if(!_control_mode->data().flag_control_velocity_enabled) {
|
||||
if (!_control_mode->data().flag_control_velocity_enabled) {
|
||||
_att_sp_msg.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
|
||||
_att_sp_msg.data().pitch_body = -_manual_control_sp->data().x * _params.man_pitch_max;
|
||||
}
|
||||
|
||||
/* Control climb rate directly if no aiding altitude controller is active */
|
||||
if(!_control_mode->data().flag_control_climb_rate_enabled) {
|
||||
if (!_control_mode->data().flag_control_climb_rate_enabled) {
|
||||
_att_sp_msg.data().thrust = _manual_control_sp->data().z;
|
||||
}
|
||||
|
||||
/* Construct attitude setpoint rotation matrix */
|
||||
math::Matrix<3,3> R_sp;
|
||||
R_sp.from_euler(_att_sp_msg.data().roll_body,_att_sp_msg.data().pitch_body,_att_sp_msg.data().yaw_body);
|
||||
math::Matrix<3, 3> R_sp;
|
||||
R_sp.from_euler(_att_sp_msg.data().roll_body, _att_sp_msg.data().pitch_body, _att_sp_msg.data().yaw_body);
|
||||
_att_sp_msg.data().R_valid = true;
|
||||
memcpy(&_att_sp_msg.data().R_body[0], R_sp.data, sizeof(_att_sp_msg.data().R_body));
|
||||
_att_sp_msg.data().timestamp = get_time_micros();
|
||||
}
|
||||
else {
|
||||
|
||||
} else {
|
||||
reset_yaw_sp = true;
|
||||
}
|
||||
|
||||
@@ -1035,8 +1051,8 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
|
||||
* is published by the mavlink app
|
||||
*/
|
||||
if (!(_control_mode->data().flag_control_offboard_enabled &&
|
||||
!(_control_mode->data().flag_control_position_enabled ||
|
||||
_control_mode->data().flag_control_velocity_enabled))) {
|
||||
!(_control_mode->data().flag_control_position_enabled ||
|
||||
_control_mode->data().flag_control_velocity_enabled))) {
|
||||
if (_att_sp_pub != nullptr) {
|
||||
_att_sp_pub->publish(_att_sp_msg);
|
||||
|
||||
@@ -1046,5 +1062,6 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
|
||||
}
|
||||
|
||||
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
|
||||
reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled;
|
||||
reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled
|
||||
&& !_control_mode->data().flag_control_climb_rate_enabled;
|
||||
}
|
||||
+4
-3
@@ -88,7 +88,8 @@ protected:
|
||||
|
||||
Publisher<px4_vehicle_attitude_setpoint> *_att_sp_pub; /**< attitude setpoint publication */
|
||||
Publisher<px4_vehicle_local_position_setpoint> *_local_pos_sp_pub; /**< vehicle local position setpoint publication */
|
||||
Publisher<px4_vehicle_global_velocity_setpoint> *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
|
||||
Publisher<px4_vehicle_global_velocity_setpoint>
|
||||
*_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
|
||||
|
||||
|
||||
Subscriber<px4_vehicle_attitude> *_att; /**< vehicle attitude */
|
||||
@@ -213,8 +214,8 @@ protected:
|
||||
*/
|
||||
void control_offboard(float dt);
|
||||
|
||||
bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
|
||||
const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res);
|
||||
bool cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r,
|
||||
const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res);
|
||||
|
||||
/**
|
||||
* Set position setpoint for AUTO
|
||||
+5
-5
@@ -69,11 +69,11 @@ int mc_pos_control_m_main(int argc, char *argv[])
|
||||
mc_pos_control_task_should_exit = false;
|
||||
|
||||
mc_pos_control_daemon_task = px4_task_spawn_cmd("mc_pos_control_m",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2500,
|
||||
mc_pos_control_start_main,
|
||||
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2500,
|
||||
mc_pos_control_start_main,
|
||||
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user