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

generic position controller status/feedback message

Browse Source
This commit is contained in:
Daniel Agar
2018-08-31 14:43:51 -04:00
parent 223dacee64
commit 5207c420c3
16 changed files with 303 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_high_latency2.cpp
+10 -11
View File
@@ -39,17 +39,16 @@
#include "mavlink_high_latency2.h"
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <lib/ecl/geo/geo.h>
#include <commander/px4_custom_mode.h>
#include <lib/ecl/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -75,8 +74,8 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink
_battery_time(0),
_estimator_status_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))),
_estimator_status_time(0),
_fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))),
_fw_pos_ctrl_status_time(0),
_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(position_controller_status))),
_pos_ctrl_status_time(0),
_geofence_sub(_mavlink->add_orb_subscription(ORB_ID(geofence_result))),
_geofence_time(0),
_global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
@@ -283,13 +282,13 @@ bool MavlinkStreamHighLatency2::write_estimator_status(mavlink_high_latency2_t *
bool MavlinkStreamHighLatency2::write_fw_ctrl_status(mavlink_high_latency2_t *msg)
{
struct fw_pos_ctrl_status_s fw_pos_ctrl_status;
position_controller_status_s pos_ctrl_status = {};
const bool updated = _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_time, &fw_pos_ctrl_status);
const bool updated = _pos_ctrl_status_sub->update(&_pos_ctrl_status_time, &pos_ctrl_status);
if (_fw_pos_ctrl_status_time > 0) {
if (_pos_ctrl_status_time > 0) {
uint16_t target_distance;
convert_limit_safe(fw_pos_ctrl_status.wp_dist * 0.1f, target_distance);
convert_limit_safe(pos_ctrl_status.wp_dist * 0.1f, target_distance);
msg->target_distance = target_distance;
}
src/modules/mavlink/mavlink_high_latency2.h
+2 -2
View File
@@ -101,8 +101,8 @@ private:
MavlinkOrbSubscription *_estimator_status_sub;
uint64_t _estimator_status_time;
MavlinkOrbSubscription *_fw_pos_ctrl_status_sub;
uint64_t _fw_pos_ctrl_status_time;
MavlinkOrbSubscription *_pos_ctrl_status_sub;
uint64_t _pos_ctrl_status_time;
MavlinkOrbSubscription *_geofence_sub;
uint64_t _geofence_time;
src/modules/mavlink/mavlink_messages.cpp
+19 -19
View File
@@ -49,8 +49,8 @@
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_rc_input.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
#include <px4_time.h>
#include <systemlib/mavlink_log.h>
@@ -69,13 +69,13 @@
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_bias.h>
@@ -3743,15 +3743,15 @@ public:
unsigned get_size()
{
return (_fw_pos_ctrl_status_sub->is_published()) ?
return (_pos_ctrl_status_sub->is_published()) ?
MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
MavlinkOrbSubscription *_fw_pos_ctrl_status_sub;
MavlinkOrbSubscription *_pos_ctrl_status_sub;
MavlinkOrbSubscription *_tecs_status_sub;
uint64_t _fw_pos_ctrl_status_timestamp{0};
uint64_t _pos_ctrl_status_timestamp{0};
uint64_t _tecs_status_timestamp{0};
/* do not allow top copying this class */
@@ -3760,30 +3760,30 @@ private:
protected:
explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink),
_fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))),
_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(position_controller_status))),
_tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status)))
{}
bool send(const hrt_abstime t)
{
fw_pos_ctrl_status_s _fw_pos_ctrl_status = {};
tecs_status_s _tecs_status = {};
position_controller_status_s pos_ctrl_status = {};
tecs_status_s tecs_status = {};
bool updated = false;
updated |= _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_timestamp, &_fw_pos_ctrl_status);
updated |= _tecs_status_sub->update(&_tecs_status_timestamp, &_tecs_status);
updated |= _pos_ctrl_status_sub->update(&_pos_ctrl_status_timestamp, &pos_ctrl_status);
updated |= _tecs_status_sub->update(&_tecs_status_timestamp, &tecs_status);
if (updated) {
mavlink_nav_controller_output_t msg = {};
msg.nav_roll = math::degrees(_fw_pos_ctrl_status.nav_roll);
msg.nav_pitch = math::degrees(_fw_pos_ctrl_status.nav_pitch);
msg.nav_bearing = (int16_t)math::degrees(_fw_pos_ctrl_status.nav_bearing);
msg.target_bearing = (int16_t)math::degrees(_fw_pos_ctrl_status.target_bearing);
msg.wp_dist = (uint16_t)_fw_pos_ctrl_status.wp_dist;
msg.xtrack_error = _fw_pos_ctrl_status.xtrack_error;
msg.alt_error = _tecs_status.altitude_filtered - _tecs_status.altitude_sp;
msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeed_sp;
msg.nav_roll = math::degrees(pos_ctrl_status.nav_roll);
msg.nav_pitch = math::degrees(pos_ctrl_status.nav_pitch);
msg.nav_bearing = (int16_t)math::degrees(pos_ctrl_status.nav_bearing);
msg.target_bearing = (int16_t)math::degrees(pos_ctrl_status.target_bearing);
msg.wp_dist = (uint16_t)pos_ctrl_status.wp_dist;
msg.xtrack_error = pos_ctrl_status.xtrack_error;
msg.alt_error = tecs_status.altitude_filtered - tecs_status.altitude_sp;
msg.aspd_error = tecs_status.airspeed_filtered - tecs_status.airspeed_sp;
mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg);