fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 16:18:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw_att_control: update orb_publish to uORB::Publication<>

Browse Source
This commit is contained in:
Daniel Agar
2019-09-02 16:20:44 -04:00
parent b0f82ee8ac
commit e83026f106
2 changed files with 14 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+4 -26
View File
@@ -331,14 +331,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
_rates_sp.yaw = _manual.r * _parameters.acro_max_z_rate_rad;
_rates_sp.thrust_body[0] = _manual.z;
if (_rate_sp_pub != nullptr) {
/* publish the attitude rates setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp);
} else {
/* advertise the attitude rates setpoint */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
}
_rate_sp_pub.publish(_rates_sp);
} else {
/* manual/direct control */
@@ -748,14 +741,7 @@ void FixedwingAttitudeControl::Run()
_rates_sp.timestamp = hrt_absolute_time();
if (_rate_sp_pub != nullptr) {
/* publish the attitude rates setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp);
} else {
/* advertise the attitude rates setpoint */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
}
_rate_sp_pub.publish(_rates_sp);
} else {
vehicle_rates_setpoint_poll();
@@ -784,8 +770,7 @@ void FixedwingAttitudeControl::Run()
rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator();
rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator();
int instance;
orb_publish_auto(ORB_ID(rate_ctrl_status), &_rate_ctrl_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT);
_rate_ctrl_status_pub.publish(rate_ctrl_status);
}
// Add feed-forward from roll control output to yaw control output
@@ -817,14 +802,7 @@ void FixedwingAttitudeControl::Run()
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
if (_actuators_2_pub != nullptr) {
/* publish the actuator controls*/
orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
} else {
/* advertise and publish */
_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
}
_actuators_2_pub.publish(_actuators_airframe);
}
}
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+10 -7
View File
@@ -47,6 +47,8 @@
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
@@ -108,14 +110,15 @@ private:
uORB::SubscriptionData<airspeed_s> _airspeed_sub{ORB_ID(airspeed)};
orb_advert_t _rate_sp_pub{nullptr}; /**< rate setpoint publication */
orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint point */
orb_advert_t _actuators_0_pub{nullptr}; /**< actuator control group 0 setpoint */
orb_advert_t _actuators_2_pub{nullptr}; /**< actuator control group 1 setpoint (Airframe) */
orb_advert_t _rate_ctrl_status_pub{nullptr}; /**< rate controller status publication */
uORB::Publication<actuator_controls_s> _actuators_2_pub{ORB_ID(actuator_controls_2)}; /**< actuator control group 1 setpoint (Airframe) */
uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; /**< rate controller status publication */
orb_id_t _actuators_id{nullptr}; // pointer to correct actuator controls0 uORB metadata structure
orb_id_t _attitude_setpoint_id{nullptr};
orb_id_t _attitude_setpoint_id{nullptr};
orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint point */
orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */
orb_advert_t _actuators_0_pub{nullptr}; /**< actuator control group 0 setpoint */
actuator_controls_s _actuators {}; /**< actuator control inputs */
actuator_controls_s _actuators_airframe {}; /**< actuator control inputs */