diff --git a/src/modules/mc_att_control_multiplatform/CMakeLists.txt b/src/examples/mc_att_control_multiplatform/CMakeLists.txt similarity index 100% rename from src/modules/mc_att_control_multiplatform/CMakeLists.txt rename to src/examples/mc_att_control_multiplatform/CMakeLists.txt diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/examples/mc_att_control_multiplatform/mc_att_control.cpp similarity index 83% rename from src/modules/mc_att_control_multiplatform/mc_att_control.cpp rename to src/examples/mc_att_control_multiplatform/mc_att_control.cpp index 98435a08f0..54da404f05 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/examples/mc_att_control_multiplatform/mc_att_control.cpp @@ -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(0); _v_control_mode = _n.subscribe(0); _parameter_update = _n.subscribe( - &MulticopterAttitudeControlMultiplatform::handle_parameter_update, this, 1000); + &MulticopterAttitudeControlMultiplatform::handle_parameter_update, this, 1000); _manual_control_sp = _n.subscribe(0); _armed = _n.subscribe(0); _v_status = _n.subscribe(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? diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/examples/mc_att_control_multiplatform/mc_att_control.h similarity index 100% rename from src/modules/mc_att_control_multiplatform/mc_att_control.h rename to src/examples/mc_att_control_multiplatform/mc_att_control.h diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp similarity index 100% rename from src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp rename to src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h b/src/examples/mc_att_control_multiplatform/mc_att_control_base.h similarity index 100% rename from src/modules/mc_att_control_multiplatform/mc_att_control_base.h rename to src/examples/mc_att_control_multiplatform/mc_att_control_base.h diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/examples/mc_att_control_multiplatform/mc_att_control_main.cpp similarity index 100% rename from src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp rename to src/examples/mc_att_control_multiplatform/mc_att_control_main.cpp diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/examples/mc_att_control_multiplatform/mc_att_control_params.c similarity index 100% rename from src/modules/mc_att_control_multiplatform/mc_att_control_params.c rename to src/examples/mc_att_control_multiplatform/mc_att_control_params.c diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h b/src/examples/mc_att_control_multiplatform/mc_att_control_params.h similarity index 100% rename from src/modules/mc_att_control_multiplatform/mc_att_control_params.h rename to src/examples/mc_att_control_multiplatform/mc_att_control_params.h diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp b/src/examples/mc_att_control_multiplatform/mc_att_control_sim.cpp similarity index 100% rename from src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp rename to src/examples/mc_att_control_multiplatform/mc_att_control_sim.cpp diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h b/src/examples/mc_att_control_multiplatform/mc_att_control_sim.h similarity index 100% rename from src/modules/mc_att_control_multiplatform/mc_att_control_sim.h rename to src/examples/mc_att_control_multiplatform/mc_att_control_sim.h diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/examples/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp similarity index 94% rename from src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp rename to src/examples/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp index a0e6926cf4..6afd5271f1 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp +++ b/src/examples/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -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; } diff --git a/src/modules/mc_pos_control_multiplatform/CMakeLists.txt b/src/examples/mc_pos_control_multiplatform/CMakeLists.txt similarity index 100% rename from src/modules/mc_pos_control_multiplatform/CMakeLists.txt rename to src/examples/mc_pos_control_multiplatform/CMakeLists.txt diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp similarity index 86% rename from src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp rename to src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp index 3802247cd9..a281e7b6e3 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -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(&MulticopterPositionControlMultiplatform::handle_vehicle_attitude, this, 0); _control_mode = _n.subscribe(0); _parameter_update = _n.subscribe( - &MulticopterPositionControlMultiplatform::handle_parameter_update, this, 1000); + &MulticopterPositionControlMultiplatform::handle_parameter_update, this, 1000); _manual_control_sp = _n.subscribe(0); _armed = _n.subscribe(0); _local_pos = _n.subscribe(0); - _pos_sp_triplet = _n.subscribe(&MulticopterPositionControlMultiplatform::handle_position_setpoint_triplet, this, 0); + _pos_sp_triplet = _n.subscribe + (&MulticopterPositionControlMultiplatform::handle_position_setpoint_triplet, this, 0); _local_pos_sp = _n.subscribe(0); _global_vel_sp = _n.subscribe(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; } diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/examples/mc_pos_control_multiplatform/mc_pos_control.h similarity index 96% rename from src/modules/mc_pos_control_multiplatform/mc_pos_control.h rename to src/examples/mc_pos_control_multiplatform/mc_pos_control.h index 58acf377b9..51b93432d7 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/examples/mc_pos_control_multiplatform/mc_pos_control.h @@ -88,7 +88,8 @@ protected: Publisher *_att_sp_pub; /**< attitude setpoint publication */ Publisher *_local_pos_sp_pub; /**< vehicle local position setpoint publication */ - Publisher *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + Publisher + *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ Subscriber *_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 diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp b/src/examples/mc_pos_control_multiplatform/mc_pos_control_main.cpp similarity index 100% rename from src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp rename to src/examples/mc_pos_control_multiplatform/mc_pos_control_main.cpp diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/examples/mc_pos_control_multiplatform/mc_pos_control_params.c similarity index 100% rename from src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c rename to src/examples/mc_pos_control_multiplatform/mc_pos_control_params.c diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h b/src/examples/mc_pos_control_multiplatform/mc_pos_control_params.h similarity index 100% rename from src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h rename to src/examples/mc_pos_control_multiplatform/mc_pos_control_params.h diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/examples/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp similarity index 94% rename from src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp rename to src/examples/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp index 85d0dad3ff..36af460b5d 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp +++ b/src/examples/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -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; }