diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 8076f75b94..eaa3e82b1d 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -116,8 +116,58 @@ RoverPositionControl::vehicle_control_mode_poll() void RoverPositionControl::manual_control_setpoint_poll() { - if (_manual_control_setpoint_sub.updated()) { - _manual_control_setpoint_sub.copy(&_manual_control_setpoint); + if (_control_mode.flag_control_manual_enabled) { + if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { + float dt = math::constrain(hrt_elapsed_time(&_manual_setpoint_last_called) * 1e-6f, 0.0002f, 0.04f); + + if (!_control_mode.flag_control_climb_rate_enabled && + !_control_mode.flag_control_offboard_enabled) { + + if (_control_mode.flag_control_attitude_enabled) { + // STABILIZED mode generate the attitude setpoint from manual user inputs + _att_sp.roll_body = 0.0; + _att_sp.pitch_body = 0.0; + + /* reset yaw setpoint to current position if needed */ + if (_reset_yaw_sp) { + const float vehicle_yaw = Eulerf(Quatf(_vehicle_att.q)).psi(); + _manual_yaw_sp = vehicle_yaw; + _reset_yaw_sp = false; + + } else { + const float yaw_rate = math::radians(_param_gnd_man_y_max.get()); + _att_sp.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate; + _manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt); + } + + _att_sp.yaw_body = _manual_yaw_sp; + _att_sp.thrust_body[0] = _manual_control_setpoint.z; + + Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + q.copyTo(_att_sp.q_d); + + _att_sp.timestamp = hrt_absolute_time(); + + + _attitude_sp_pub.publish(_att_sp); + + } else { + _act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.y; + _act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x; + // Set heading from the manual roll input channel + _act_controls.control[actuator_controls_s::INDEX_YAW] = + _manual_control_setpoint.y; // Nominally yaw: _manual_control_setpoint.r; + // Set throttle from the manual throttle channel + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z; + _reset_yaw_sp = true; + } + + } else { + _reset_yaw_sp = true; + } + + _manual_setpoint_last_called = hrt_absolute_time(); + } } } @@ -352,8 +402,6 @@ RoverPositionControl::Run() /* update parameters from storage */ parameters_update(); - bool manual_mode = _control_mode.flag_control_manual_enabled; - /* only run controller if position changed */ if (_local_pos_sub.update(&_local_pos)) { @@ -382,7 +430,7 @@ RoverPositionControl::Run() 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) { + if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) { if (control_position(current_position, ground_speed, _pos_sp_triplet)) { @@ -412,7 +460,7 @@ RoverPositionControl::Run() } - } else if (!manual_mode && _control_mode.flag_control_velocity_enabled) { + } else if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_velocity_enabled) { control_velocity(current_velocity, _pos_sp_triplet); @@ -420,30 +468,18 @@ RoverPositionControl::Run() } // Respond to an attitude update and run the attitude controller if enabled - if (!manual_mode && _control_mode.flag_control_attitude_enabled + if (_control_mode.flag_control_attitude_enabled && !_control_mode.flag_control_position_enabled && !_control_mode.flag_control_velocity_enabled) { - control_attitude(_vehicle_att, _att_sp); } - // Manual pass-through if no other control mode is enabled - if (manual_mode) { - _act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.y; - _act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x; - // Set heading from the manual roll input channel - _act_controls.control[actuator_controls_s::INDEX_YAW] = - _manual_control_setpoint.y; // Nominally yaw: _manual_control_setpoint.r; - // Set throttle from the manual throttle channel - _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z; - } - - // publish the controller output + /* Only publish if any of the proper modes are enabled */ if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_attitude_enabled || _control_mode.flag_control_position_enabled || - manual_mode) { + _control_mode.flag_control_manual_enabled) { // timestamp and publish controls _act_controls.timestamp = hrt_absolute_time(); _actuator_controls_pub.publish(_act_controls); diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 0d853add1b..e80c53f99d 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -102,6 +102,7 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Publication _attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; /**< navigation capabilities publication */ uORB::Publication _actuator_controls_pub{ORB_ID(actuator_controls_0)}; /**< actuator controls publication */ @@ -126,6 +127,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ hrt_abstime _control_position_last_called{0}; /**) _param_l1_period, (ParamFloat) _param_l1_damping, @@ -171,6 +176,7 @@ private: (ParamFloat) _param_wheel_base, (ParamFloat) _param_max_turn_angle, + (ParamFloat) _param_gnd_man_y_max, (ParamFloat) _param_nav_loiter_rad /**< loiter radius for Rover */ ) diff --git a/src/modules/rover_pos_control/rover_pos_control_params.c b/src/modules/rover_pos_control/rover_pos_control_params.c index bdc82044d8..b3120f50a7 100644 --- a/src/modules/rover_pos_control/rover_pos_control_params.c +++ b/src/modules/rover_pos_control/rover_pos_control_params.c @@ -273,3 +273,14 @@ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f); * @group Rover Position Control */ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f); + +/** + * Max manual yaw rate + * + * @unit deg/s + * @min 0.0 + * @max 400 + * @decimal 1 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_MAN_Y_MAX, 150.0f);