dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_pos_control: reduce scope of attitude setpoint 

No need for a gloabl state since it gets produced freshly
by the controller and then published.
This commit is contained in:
Matthias Grob 2019-10-22 17:09:12 +02:00
parent 79334958a9
commit 40edd33078
1 changed files with 14 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -107,12 +107,11 @@ public:
private:
Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */
orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */
orb_id_t _attitude_setpoint_id{nullptr}; ///< orb metadata to publish attitude setpoint dependent if VTOL or not
orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; ///< attitude setpoint publication handle
uORB::PublicationQueued<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command publication */
orb_advert_t _mavlink_log_pub{nullptr};
orb_id_t _attitude_setpoint_id{nullptr};
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */
uORB::Publication<vehicle_local_position_setpoint_s> _traj_sp_pub{ORB_ID(trajectory_setpoint)}; /**< trajectory setpoints publication */
@ -141,7 +140,6 @@ private:
.landed = true,
};
vehicle_attitude_setpoint_s _att_sp{}; /**< vehicle attitude setpoint */
vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */
vehicle_local_position_s _local_pos{}; /**< vehicle local position */
home_position_s _home_pos{}; /**< home position */
@ -193,6 +191,7 @@ private:
systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */
WeatherVane *_wv_controller{nullptr};
Vector3f _wv_dcm_z_sp_prev{0, 0, 1};
perf_counter_t _cycle_perf;
@ -555,7 +554,7 @@ MulticopterPositionControl::Run()
}
}
_wv_controller->update(Quatf(_att_sp.q_d).dcm_z(), _states.yaw);
_wv_controller->update(_wv_dcm_z_sp_prev, _states.yaw);
}
// an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes
@ -696,17 +695,22 @@ MulticopterPositionControl::Run()
}
// Fill attitude setpoint. Attitude is computed from yaw and thrust setpoint.
_att_sp = ControlMath::thrustToAttitude(matrix::Vector3f(local_pos_sp.thrust), local_pos_sp.yaw);
_att_sp.yaw_sp_move_rate = _control.getYawspeedSetpoint();
_att_sp.fw_control_yaw = false;
_att_sp.apply_flaps = false;
vehicle_attitude_setpoint_s attitude_setpoint{};
attitude_setpoint = ControlMath::thrustToAttitude(matrix::Vector3f(local_pos_sp.thrust), local_pos_sp.yaw);
attitude_setpoint.yaw_sp_move_rate = _control.getYawspeedSetpoint();
attitude_setpoint.fw_control_yaw = false;
attitude_setpoint.apply_flaps = false;
// publish attitude setpoint
// Note: this requires review. The reason for not sending
// an attitude setpoint is because for non-flighttask modes
// the attitude septoint should come from another source, otherwise
// they might conflict with each other such as in offboard attitude control.
publish_attitude();
if (_attitude_setpoint_id != nullptr) {
orb_publish_auto(_attitude_setpoint_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
}
_wv_dcm_z_sp_prev = Quatf(attitude_setpoint.q_d).dcm_z();
// if there's any change in landing gear setpoint publish it
if (gear.landing_gear != _old_landing_gear_position
@ -719,18 +723,6 @@ MulticopterPositionControl::Run()
}
_old_landing_gear_position = gear.landing_gear;
} else {
// no flighttask is active: set attitude setpoint to idle
_att_sp.roll_body = _att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _states.yaw;
_att_sp.yaw_sp_move_rate = 0.0f;
_att_sp.fw_control_yaw = false;
_att_sp.apply_flaps = false;
matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
q_sp.copyTo(_att_sp.q_d);
_att_sp.q_d_valid = true;
_att_sp.thrust_body[2] = 0.0f;
}
}
@ -1031,19 +1023,6 @@ MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoin
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
}
void
MulticopterPositionControl::publish_attitude()
{
_att_sp.timestamp = hrt_absolute_time();
if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
}
void MulticopterPositionControl::check_failure(bool task_failure, uint8_t nav_state)
{
if (!task_failure) {