|
|
|
|
@ -122,6 +122,7 @@ private:
@@ -122,6 +122,7 @@ private:
|
|
|
|
|
int _control_task; /**< task handle for task */ |
|
|
|
|
int _mavlink_fd; /**< mavlink fd */ |
|
|
|
|
|
|
|
|
|
int _vehicle_status_sub; /**< vehicle status subscription */ |
|
|
|
|
int _att_sub; /**< vehicle attitude subscription */ |
|
|
|
|
int _att_sp_sub; /**< vehicle attitude setpoint */ |
|
|
|
|
int _control_mode_sub; /**< vehicle control mode subscription */ |
|
|
|
|
@ -137,6 +138,7 @@ private:
@@ -137,6 +138,7 @@ private:
|
|
|
|
|
orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ |
|
|
|
|
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ |
|
|
|
|
|
|
|
|
|
struct vehicle_status_s _vehicle_status; /**< vehicle status */ |
|
|
|
|
struct vehicle_attitude_s _att; /**< vehicle attitude */ |
|
|
|
|
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ |
|
|
|
|
struct manual_control_setpoint_s _manual; /**< r/c channel data */ |
|
|
|
|
@ -319,6 +321,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
@@ -319,6 +321,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
|
|
|
|
|
_reset_alt_sp(true), |
|
|
|
|
_mode_auto(false) |
|
|
|
|
{ |
|
|
|
|
memset(&_vehicle_status, 0, sizeof(_vehicle_status)); |
|
|
|
|
memset(&_att, 0, sizeof(_att)); |
|
|
|
|
memset(&_att_sp, 0, sizeof(_att_sp)); |
|
|
|
|
memset(&_manual, 0, sizeof(_manual)); |
|
|
|
|
@ -474,6 +477,12 @@ MulticopterPositionControl::poll_subscriptions()
@@ -474,6 +477,12 @@ MulticopterPositionControl::poll_subscriptions()
|
|
|
|
|
{ |
|
|
|
|
bool updated; |
|
|
|
|
|
|
|
|
|
orb_check(_vehicle_status_sub, &updated); |
|
|
|
|
|
|
|
|
|
if (updated) { |
|
|
|
|
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
orb_check(_att_sub, &updated); |
|
|
|
|
|
|
|
|
|
if (updated) { |
|
|
|
|
@ -903,6 +912,7 @@ MulticopterPositionControl::task_main()
@@ -903,6 +912,7 @@ MulticopterPositionControl::task_main()
|
|
|
|
|
/*
|
|
|
|
|
* do subscriptions |
|
|
|
|
*/ |
|
|
|
|
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); |
|
|
|
|
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); |
|
|
|
|
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); |
|
|
|
|
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); |
|
|
|
|
@ -1373,44 +1383,44 @@ MulticopterPositionControl::task_main()
@@ -1373,44 +1383,44 @@ MulticopterPositionControl::task_main()
|
|
|
|
|
reset_int_xy = true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// generate attitude setpoint from manual controls
|
|
|
|
|
/* generate attitude setpoint from manual controls */ |
|
|
|
|
if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { |
|
|
|
|
|
|
|
|
|
// reset yaw setpoint to current position if needed
|
|
|
|
|
/* reset yaw setpoint to current position if needed */ |
|
|
|
|
if (reset_yaw_sp) { |
|
|
|
|
reset_yaw_sp = false; |
|
|
|
|
_att_sp.yaw_body = _att.yaw; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// do not move yaw while arming
|
|
|
|
|
/* do not move yaw while arming */ |
|
|
|
|
else if (_manual.z > 0.1f) |
|
|
|
|
{ |
|
|
|
|
const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p; |
|
|
|
|
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; |
|
|
|
|
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); |
|
|
|
|
float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw); |
|
|
|
|
if (yaw_offs < - YAW_OFFSET_MAX) { |
|
|
|
|
_att_sp.yaw_body = _wrap_pi(_att.yaw - YAW_OFFSET_MAX); |
|
|
|
|
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); |
|
|
|
|
float yaw_offs = _wrap_pi(yaw_target - _att.yaw); |
|
|
|
|
|
|
|
|
|
} else if (yaw_offs > YAW_OFFSET_MAX) { |
|
|
|
|
_att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX); |
|
|
|
|
// If the yaw offset became too big for the system to track stop
|
|
|
|
|
// shifting it
|
|
|
|
|
if (fabsf(yaw_offs) < yaw_offset_max) { |
|
|
|
|
_att_sp.yaw_body = yaw_target; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
//Control roll and pitch directly if we no aiding velocity controller is active
|
|
|
|
|
/* control roll and pitch directly if we no aiding velocity controller is active */ |
|
|
|
|
if (!_control_mode.flag_control_velocity_enabled) { |
|
|
|
|
_att_sp.roll_body = _manual.y * _params.man_roll_max; |
|
|
|
|
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
//Control climb rate directly if no aiding altitude controller is active
|
|
|
|
|
/* control throttle directly if no climb rate controller is active */ |
|
|
|
|
if (!_control_mode.flag_control_climb_rate_enabled) { |
|
|
|
|
_att_sp.thrust = math::min(_manual.z, _params.thr_max); |
|
|
|
|
_att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
//Construct attitude setpoint rotation matrix
|
|
|
|
|
/* 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); |
|
|
|
|
memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); |
|
|
|
|
@ -1435,10 +1445,9 @@ MulticopterPositionControl::task_main()
@@ -1435,10 +1445,9 @@ MulticopterPositionControl::task_main()
|
|
|
|
|
if (!(_control_mode.flag_control_offboard_enabled && |
|
|
|
|
!(_control_mode.flag_control_position_enabled || |
|
|
|
|
_control_mode.flag_control_velocity_enabled))) { |
|
|
|
|
if (_att_sp_pub != nullptr) { |
|
|
|
|
if (_att_sp_pub != nullptr && _vehicle_status.is_rotary_wing) { |
|
|
|
|
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
} else if (_att_sp_pub <= 0 && _vehicle_status.is_rotary_wing){ |
|
|
|
|
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
Lorenz Meier
10 years ago