fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 03:03: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;
}