mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-29 22:24:07 +08:00
Merge branch 'master' of github.com:PX4/Firmware into driver_framework
This commit is contained in:
Lorenz Meier
commit
c6510bd2de
@ -200,6 +200,7 @@ for index,m in enumerate(messages[1:]):
|
||||
print("\t} else {")
|
||||
print("\t\t printf(\" Topic did not match any known topics\\n\");")
|
||||
print("\t}")
|
||||
print("\t\torb_unsubscribe(sub);")
|
||||
print("\t return 0;")
|
||||
|
||||
print("}")
|
||||
|
||||
@ -474,8 +474,9 @@ PWMSim::task_main()
|
||||
}
|
||||
|
||||
/* do mixing */
|
||||
actuator_outputs_s outputs;
|
||||
actuator_outputs_s outputs = {};
|
||||
num_outputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
|
||||
outputs.noutputs = num_outputs;
|
||||
outputs.timestamp = hrt_absolute_time();
|
||||
|
||||
/* disable unused ports by setting their output to NaN */
|
||||
|
||||
@ -894,8 +894,10 @@ void AttitudePositionEstimatorEKF::publishControlState()
|
||||
_ctrl_state.q[3] = _ekf->states[3];
|
||||
|
||||
/* Airspeed (Groundspeed - Windspeed) */
|
||||
_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
|
||||
|
||||
//_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
|
||||
// the line above was introduced by the control state PR. The airspeed it gives is totally wrong and leads to horrible flight performance in SITL
|
||||
// and in outdoor tests
|
||||
_ctrl_state.airspeed = _airspeed.true_airspeed_m_s;
|
||||
/* Attitude Rates */
|
||||
_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
|
||||
_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
|
||||
|
||||
@ -279,8 +279,8 @@ private:
|
||||
*/
|
||||
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
|
||||
@ -321,7 +321,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
|
||||
_control_task(-1),
|
||||
_mavlink_fd(-1),
|
||||
|
||||
/* subscriptions */
|
||||
/* subscriptions */
|
||||
_ctrl_state_sub(-1),
|
||||
_att_sp_sub(-1),
|
||||
_control_mode_sub(-1),
|
||||
@ -332,7 +332,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
|
||||
_pos_sp_triplet_sub(-1),
|
||||
_global_vel_sp_sub(-1),
|
||||
|
||||
/* publications */
|
||||
/* publications */
|
||||
_att_sp_pub(nullptr),
|
||||
_local_pos_sp_pub(nullptr),
|
||||
_global_vel_sp_pub(nullptr),
|
||||
@ -517,7 +517,7 @@ MulticopterPositionControl::parameters_update(bool force)
|
||||
_params.man_roll_max = math::radians(_params.man_roll_max);
|
||||
_params.man_pitch_max = math::radians(_params.man_pitch_max);
|
||||
_params.man_yaw_max = math::radians(_params.man_yaw_max);
|
||||
param_get(_params_handles.mc_att_yaw_p,&v);
|
||||
param_get(_params_handles.mc_att_yaw_p, &v);
|
||||
_params.mc_att_yaw_p = v;
|
||||
}
|
||||
|
||||
@ -626,9 +626,9 @@ MulticopterPositionControl::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.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0)
|
||||
- _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0);
|
||||
- _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0);
|
||||
_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1)
|
||||
- _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1);
|
||||
- _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1);
|
||||
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
|
||||
}
|
||||
}
|
||||
@ -703,7 +703,8 @@ MulticopterPositionControl::control_manual(float dt)
|
||||
/* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */
|
||||
math::Matrix<3, 3> R_yaw_sp;
|
||||
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
|
||||
math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(_params.vel_max); // in NED and scaled to actual velocity
|
||||
math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(
|
||||
_params.vel_max); // in NED and scaled to actual velocity
|
||||
|
||||
/*
|
||||
* assisted velocity mode: user controls velocity, but if velocity is small enough, position
|
||||
@ -711,20 +712,20 @@ MulticopterPositionControl::control_manual(float dt)
|
||||
*/
|
||||
|
||||
/* horizontal axes */
|
||||
if (_control_mode.flag_control_position_enabled)
|
||||
{
|
||||
if (_control_mode.flag_control_position_enabled) {
|
||||
/* check for pos. hold */
|
||||
if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz)
|
||||
{
|
||||
if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) {
|
||||
if (!_pos_hold_engaged) {
|
||||
if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)) {
|
||||
if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy
|
||||
&& fabsf(_vel(1)) < _params.hold_max_xy)) {
|
||||
_pos_hold_engaged = true;
|
||||
|
||||
} else {
|
||||
_pos_hold_engaged = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
|
||||
} else {
|
||||
_pos_hold_engaged = false;
|
||||
}
|
||||
|
||||
@ -739,25 +740,27 @@ MulticopterPositionControl::control_manual(float dt)
|
||||
}
|
||||
|
||||
/* vertical axis */
|
||||
if (_control_mode.flag_control_altitude_enabled)
|
||||
{
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
/* check for pos. hold */
|
||||
if (fabsf(req_vel_sp(2)) < _params.hold_z_dz)
|
||||
{
|
||||
if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z))
|
||||
{
|
||||
_alt_hold_engaged = true;
|
||||
if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) {
|
||||
if (!_alt_hold_engaged) {
|
||||
if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) {
|
||||
_alt_hold_engaged = true;
|
||||
|
||||
} else {
|
||||
_alt_hold_engaged = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
|
||||
} else {
|
||||
_alt_hold_engaged = false;
|
||||
_pos_sp(2) = _pos(2);
|
||||
}
|
||||
|
||||
/* set requested velocity setpoint */
|
||||
if (!_alt_hold_engaged) {
|
||||
_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
|
||||
_vel_sp(2) = req_vel_sp_scaled(2);
|
||||
_pos_sp(2) = _pos(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -777,6 +780,7 @@ MulticopterPositionControl::control_offboard(float dt)
|
||||
/* control position */
|
||||
_pos_sp(0) = _pos_sp_triplet.current.x;
|
||||
_pos_sp(1) = _pos_sp_triplet.current.y;
|
||||
|
||||
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
|
||||
/* control velocity */
|
||||
/* reset position setpoint to current position if needed */
|
||||
@ -791,6 +795,7 @@ MulticopterPositionControl::control_offboard(float dt)
|
||||
|
||||
if (_pos_sp_triplet.current.yaw_valid) {
|
||||
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
|
||||
|
||||
} else if (_pos_sp_triplet.current.yawspeed_valid) {
|
||||
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
|
||||
}
|
||||
@ -798,6 +803,7 @@ MulticopterPositionControl::control_offboard(float dt)
|
||||
if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) {
|
||||
/* Control altitude */
|
||||
_pos_sp(2) = _pos_sp_triplet.current.z;
|
||||
|
||||
} else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) {
|
||||
/* reset alt setpoint to current altitude if needed */
|
||||
reset_alt_sp();
|
||||
@ -807,6 +813,7 @@ MulticopterPositionControl::control_offboard(float dt)
|
||||
|
||||
_run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */
|
||||
}
|
||||
|
||||
} else {
|
||||
reset_pos_sp();
|
||||
reset_alt_sp();
|
||||
@ -814,8 +821,8 @@ MulticopterPositionControl::control_offboard(float dt)
|
||||
}
|
||||
|
||||
bool
|
||||
MulticopterPositionControl::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)
|
||||
MulticopterPositionControl::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 */
|
||||
@ -850,13 +857,14 @@ void MulticopterPositionControl::control_auto(float dt)
|
||||
//Poll position setpoint
|
||||
bool updated;
|
||||
orb_check(_pos_sp_triplet_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
|
||||
|
||||
//Make sure that the position setpoint is valid
|
||||
if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) ||
|
||||
!PX4_ISFINITE(_pos_sp_triplet.current.lon) ||
|
||||
!PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
|
||||
!PX4_ISFINITE(_pos_sp_triplet.current.lon) ||
|
||||
!PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
|
||||
_pos_sp_triplet.current.valid = false;
|
||||
}
|
||||
}
|
||||
@ -876,21 +884,21 @@ void MulticopterPositionControl::control_auto(float dt)
|
||||
curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
|
||||
|
||||
if (PX4_ISFINITE(curr_sp(0)) &&
|
||||
PX4_ISFINITE(curr_sp(1)) &&
|
||||
PX4_ISFINITE(curr_sp(2))) {
|
||||
PX4_ISFINITE(curr_sp(1)) &&
|
||||
PX4_ISFINITE(curr_sp(2))) {
|
||||
current_setpoint_valid = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (_pos_sp_triplet.previous.valid) {
|
||||
map_projection_project(&_ref_pos,
|
||||
_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
|
||||
&prev_sp.data[0], &prev_sp.data[1]);
|
||||
_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
|
||||
&prev_sp.data[0], &prev_sp.data[1]);
|
||||
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
|
||||
|
||||
if (PX4_ISFINITE(prev_sp(0)) &&
|
||||
PX4_ISFINITE(prev_sp(1)) &&
|
||||
PX4_ISFINITE(prev_sp(2))) {
|
||||
PX4_ISFINITE(prev_sp(1)) &&
|
||||
PX4_ISFINITE(prev_sp(2))) {
|
||||
previous_setpoint_valid = true;
|
||||
}
|
||||
}
|
||||
@ -920,14 +928,15 @@ void MulticopterPositionControl::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 unit radius */
|
||||
/* check next waypoint and use it to avoid slowing down when passing via waypoint */
|
||||
if (_pos_sp_triplet.next.valid) {
|
||||
math::Vector<3> next_sp;
|
||||
map_projection_project(&_ref_pos,
|
||||
_pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
|
||||
&next_sp.data[0], &next_sp.data[1]);
|
||||
_pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
|
||||
&next_sp.data[0], &next_sp.data[1]);
|
||||
next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt);
|
||||
|
||||
if ((next_sp - curr_sp).length() > MIN_DIST) {
|
||||
@ -945,6 +954,7 @@ void MulticopterPositionControl::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 unit radius, limit it */
|
||||
if (curr_next_s_len > 1.0f) {
|
||||
cos_a_curr_next /= curr_next_s_len;
|
||||
@ -952,8 +962,8 @@ void MulticopterPositionControl::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;
|
||||
}
|
||||
}
|
||||
@ -961,6 +971,7 @@ void MulticopterPositionControl::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) {
|
||||
/* unit sphere crosses trajectory */
|
||||
|
||||
@ -988,6 +999,7 @@ void MulticopterPositionControl::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);
|
||||
}
|
||||
@ -1145,7 +1157,8 @@ MulticopterPositionControl::task_main()
|
||||
control_auto(dt);
|
||||
}
|
||||
|
||||
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
|
||||
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
|
||||
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
|
||||
/* idle state, don't run controller and set zero thrust */
|
||||
R.identity();
|
||||
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
|
||||
@ -1178,18 +1191,20 @@ MulticopterPositionControl::task_main()
|
||||
}
|
||||
|
||||
/* 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));
|
||||
if (vel_norm_xy > _params.vel_max(0)) {
|
||||
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.flag_control_position_enabled) {
|
||||
@ -1210,7 +1225,8 @@ MulticopterPositionControl::task_main()
|
||||
}
|
||||
|
||||
/* use constant descend rate when landing, ignore altitude setpoint */
|
||||
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
|
||||
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
|
||||
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
|
||||
_vel_sp(2) = _params.land_speed;
|
||||
}
|
||||
|
||||
@ -1300,7 +1316,7 @@ MulticopterPositionControl::task_main()
|
||||
|
||||
/* adjust limits for landing mode */
|
||||
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
|
||||
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
|
||||
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
|
||||
/* limit max tilt and min lift when landing */
|
||||
tilt_max = _params.tilt_max_land;
|
||||
|
||||
@ -1510,6 +1526,7 @@ MulticopterPositionControl::task_main()
|
||||
/* publish local position setpoint */
|
||||
if (_local_pos_sp_pub != nullptr) {
|
||||
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
|
||||
|
||||
} else {
|
||||
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
|
||||
}
|
||||
@ -1524,7 +1541,7 @@ MulticopterPositionControl::task_main()
|
||||
}
|
||||
|
||||
/* generate attitude setpoint from manual controls */
|
||||
if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
|
||||
if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
|
||||
|
||||
/* reset yaw setpoint to current position if needed */
|
||||
if (reset_yaw_sp) {
|
||||
@ -1533,8 +1550,7 @@ MulticopterPositionControl::task_main()
|
||||
}
|
||||
|
||||
/* do not move yaw while arming */
|
||||
else if (_manual.z > 0.1f)
|
||||
{
|
||||
else if (_manual.z > 0.1f) {
|
||||
const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p;
|
||||
|
||||
_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
|
||||
@ -1565,8 +1581,8 @@ MulticopterPositionControl::task_main()
|
||||
}
|
||||
|
||||
/* construct attitude setpoint rotation matrix */
|
||||
math::Matrix<3,3> R_sp;
|
||||
R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body);
|
||||
math::Matrix<3, 3> R_sp;
|
||||
R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
|
||||
memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
|
||||
|
||||
/* copy quaternion setpoint to attitude setpoint topic */
|
||||
@ -1574,8 +1590,8 @@ MulticopterPositionControl::task_main()
|
||||
q_sp.from_dcm(R_sp);
|
||||
memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
|
||||
_att_sp.timestamp = hrt_absolute_time();
|
||||
}
|
||||
else {
|
||||
|
||||
} else {
|
||||
reset_yaw_sp = true;
|
||||
}
|
||||
|
||||
@ -1587,17 +1603,19 @@ MulticopterPositionControl::task_main()
|
||||
* in this case the attitude setpoint is published by the mavlink app
|
||||
*/
|
||||
if (!(_control_mode.flag_control_offboard_enabled &&
|
||||
!(_control_mode.flag_control_position_enabled ||
|
||||
_control_mode.flag_control_velocity_enabled))) {
|
||||
!(_control_mode.flag_control_position_enabled ||
|
||||
_control_mode.flag_control_velocity_enabled))) {
|
||||
if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) {
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
|
||||
|
||||
} else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) {
|
||||
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
|
||||
}
|
||||
}
|
||||
|
||||
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
|
||||
reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled;
|
||||
reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled
|
||||
&& !_control_mode.flag_control_climb_rate_enabled;
|
||||
}
|
||||
|
||||
mavlink_log_info(_mavlink_fd, "[mpc] stopped");
|
||||
@ -1612,11 +1630,11 @@ MulticopterPositionControl::start()
|
||||
|
||||
/* start the task */
|
||||
_control_task = px4_task_spawn_cmd("mc_pos_control",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
1500,
|
||||
(px4_main_t)&MulticopterPositionControl::task_main_trampoline,
|
||||
nullptr);
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
1500,
|
||||
(px4_main_t)&MulticopterPositionControl::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
if (_control_task < 0) {
|
||||
warn("task start failed");
|
||||
|
||||
@ -523,8 +523,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
||||
switch (command_id) {
|
||||
case 1: {
|
||||
// Param save request
|
||||
_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
|
||||
param_opcode(get_next_dirty_node_id(1));
|
||||
int node_id;
|
||||
node_id = get_next_dirty_node_id(1);
|
||||
if (node_id < 128) {
|
||||
_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
|
||||
param_opcode(node_id);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 2: {
|
||||
@ -575,7 +579,8 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
|
||||
if (result.isSuccessful()) {
|
||||
uavcan::protocol::param::GetSet::Response resp = result.getResponse();
|
||||
if (resp.name.size()) {
|
||||
_param_counts[node_id] = _count_index++;
|
||||
_count_index++;
|
||||
_param_counts[node_id] = _count_index;
|
||||
|
||||
uavcan::protocol::param::GetSet::Request req;
|
||||
req.index = _count_index;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user