fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 06:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into dev_ros

Browse Source 
Conflicts:
	src/examples/subscriber/subscriber_params.c
	src/modules/mc_att_control/mc_att_control_main.cpp
	src/modules/uORB/topics/vehicle_attitude.h
	src/modules/uORB/topics/vehicle_attitude_setpoint.h
	src/platforms/px4_middleware.h
This commit is contained in:
Thomas Gubler
2014-12-18 12:11:05 +01:00
parent 4402d7106b 6e874bed50
commit 16618f1ada
34 changed files with 1609 additions and 334 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control.cpp
+27 -5
View File
@@ -97,9 +97,17 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX);
_params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX);
_params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX);
_params_handles.autostart_id = PX4_PARAM_INIT(SYS_AUTOSTART);
/* fetch initial parameter values */
parameters_update();
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/
_is_vtol = true;
}
else {
_is_vtol = false;
}
/*
* do subscriptions
@@ -108,9 +116,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_v_att_sp = PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0);
_v_rates_sp = PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0);
_v_control_mode = PX4_SUBSCRIBE(_n, vehicle_control_mode, 0);
PX4_SUBSCRIBE(_n, parameter_update, MulticopterAttitudeControl::handle_parameter_update, this, 1000);
_parameter_update = PX4_SUBSCRIBE(_n, parameter_update,
MulticopterAttitudeControl::handle_parameter_update, this, 1000);
_manual_control_sp = PX4_SUBSCRIBE(_n, manual_control_setpoint, 0);
_armed = PX4_SUBSCRIBE(_n, actuator_armed, 0);
_v_status = PX4_SUBSCRIBE(_n, vehicle_status, 0);
}
@@ -204,7 +214,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
control_attitude(dt);
/* publish the attitude setpoint if needed */
if (_publish_att_sp) {
if (_publish_att_sp && _v_status->get().is_rotary_wing) {
_v_att_sp_mod.timestamp = px4::get_time_micros();
if (_att_sp_pub != nullptr) {
@@ -225,7 +235,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
if (_v_rates_sp_pub != nullptr) {
_v_rates_sp_pub->publish(_v_rates_sp_mod);
} else {
_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
if (_is_vtol) {
_v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint);
} else {
_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
}
}
} else {
@@ -250,7 +264,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
_v_rates_sp_pub->publish(_v_rates_sp_mod);
} else {
_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
if (_is_vtol) {
_v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint);
} else {
_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
}
}
} else {
@@ -277,7 +295,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
_actuators_0_pub->publish(_actuators);
} else {
_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0);
if (_is_vtol) {
_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_virtual_mc);
} else {
_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0);
}
}
}
}
src/modules/mc_att_control/mc_att_control.h
+4
View File
@@ -93,6 +93,8 @@ private:
px4::Publisher * _v_rates_sp_pub; /**< rate setpoint publication */
px4::Publisher * _actuators_0_pub; /**< attitude actuator controls publication */
bool _is_vtol; /**< true if vehicle is vtol, to be replaced with global API */
px4::NodeHandle _n;
struct {
@@ -117,6 +119,8 @@ private:
px4_param_t acro_roll_max;
px4_param_t acro_pitch_max;
px4_param_t acro_yaw_max;
px4_param_t autostart_id;
} _params_handles; /**< handles for interesting parameters */
perf_counter_t _loop_perf; /**< loop performance counter */
src/modules/mc_att_control/mc_att_control_base.cpp
+1 -1
View File
@@ -266,7 +266,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed->get().armed) {
if (!_armed->get().armed || !_v_status->get().is_rotary_wing) {
_rates_int.zero();
}
src/modules/mc_att_control/mc_att_control_base.h
+7 -3
View File
@@ -87,11 +87,13 @@ public:
protected:
px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att; /**< vehicle attitude */
px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */
px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp; /**< vehicle attitude setpoint */
px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp; /**< vehicle rates setpoint */
px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode; /**< vehicle control mode */
px4::PX4_SUBSCRIBER(parameter_update) *_parameter_update; /**< parameter update */
px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */
px4::PX4_SUBSCRIBER(actuator_armed) *_armed; /**< actuator arming status */
px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp; /**< vehicle attitude setpoint */
px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp; /**< vehicle rates setpoint */
px4::PX4_SUBSCRIBER(vehicle_status) *_v_status; /**< vehicle status */
PX4_TOPIC_T(vehicle_attitude_setpoint) _v_att_sp_mod; /**< modified vehicle attitude setpoint
that gets published eventually */
@@ -121,6 +123,8 @@ protected:
float man_pitch_max;
float man_yaw_max;
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
int32_t autostart_id;
} _params;
bool _publish_att_sp;