fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 19:33: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/gnd_pos_control/GroundRoverPositionControl.cpp
+21 -36
View File
@@ -150,12 +150,6 @@ GroundRoverPositionControl::parameters_update()
_parameters.speed_imax,
_parameters.gndspeed_max);
/* Update and publish the navigation capabilities */
_gnd_pos_ctrl_status.landing_slope_angle_rad = 0;
_gnd_pos_ctrl_status.landing_horizontal_slope_displacement = 0;
_gnd_pos_ctrl_status.landing_flare_length = 0;
gnd_pos_ctrl_status_publish();
return OK;
}
@@ -192,18 +186,6 @@ GroundRoverPositionControl::position_setpoint_triplet_poll()
}
}
void GroundRoverPositionControl::gnd_pos_ctrl_status_publish()
{
_gnd_pos_ctrl_status.timestamp = hrt_absolute_time();
if (_gnd_pos_ctrl_status_pub != nullptr) {
orb_publish(ORB_ID(fw_pos_ctrl_status), _gnd_pos_ctrl_status_pub, &_gnd_pos_ctrl_status);
} else {
_gnd_pos_ctrl_status_pub = orb_advertise(ORB_ID(fw_pos_ctrl_status), &_gnd_pos_ctrl_status);
}
}
bool
GroundRoverPositionControl::control_position(const matrix::Vector2f &current_position,
const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
@@ -440,31 +422,34 @@ GroundRoverPositionControl::task_main()
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
}
/* XXX check if radius makes sense here */
float turn_distance = _parameters.l1_distance; //_gnd_control.switch_distance(100.0f);
/* XXX check if radius makes sense here */
float turn_distance = _parameters.l1_distance; //_gnd_control.switch_distance(100.0f);
/* lazily publish navigation capabilities */
if ((hrt_elapsed_time(&_gnd_pos_ctrl_status.timestamp) > 1000000)
|| (fabsf(turn_distance - _gnd_pos_ctrl_status.turn_distance) > FLT_EPSILON
&& turn_distance > 0)) {
// publish status
position_controller_status_s pos_ctrl_status = {};
/* set new turn distance */
_gnd_pos_ctrl_status.turn_distance = turn_distance;
pos_ctrl_status.nav_roll = _gnd_control.get_roll_setpoint();
pos_ctrl_status.nav_pitch = 0.0f;
pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing();
_gnd_pos_ctrl_status.nav_roll = _gnd_control.get_roll_setpoint();
_gnd_pos_ctrl_status.nav_pitch = 0.0f;
_gnd_pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing();
pos_ctrl_status.target_bearing = _gnd_control.target_bearing();
pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error();
_gnd_pos_ctrl_status.target_bearing = _gnd_control.target_bearing();
_gnd_pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error();
pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
matrix::Vector2f curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon);
_gnd_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint((double)current_position(0), (double)current_position(1),
(double)curr_wp(0), (double)curr_wp(1));
pos_ctrl_status.acceptance_radius = turn_distance;
pos_ctrl_status.yaw_acceptance = NAN;
gnd_pos_ctrl_status_publish();
pos_ctrl_status.timestamp = hrt_absolute_time();
if (_pos_ctrl_status_pub != nullptr) {
orb_publish(ORB_ID(position_controller_status), _pos_ctrl_status_pub, &pos_ctrl_status);
} else {
_pos_ctrl_status_pub = orb_advertise(ORB_ID(position_controller_status), &pos_ctrl_status);
}
}
}
src/modules/gnd_pos_control/GroundRoverPositionControl.hpp
+10 -17
View File
@@ -41,23 +41,22 @@
* @author Marco Zorzi <mzorzi@student.ethz.ch>
*/
#include <cfloat>
#include <drivers/drv_hrt.h>
#include <lib/ecl/geo/geo.h>
#include <lib/ecl/l1/ecl_l1_pos_controller.h>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <lib/pid/pid.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <cfloat>
#include <drivers/drv_hrt.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <perf/perf_counter.h>
#include <pid/pid.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -94,7 +93,7 @@ public:
private:
orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint */
orb_advert_t _gnd_pos_ctrl_status_pub{nullptr}; /**< navigation capabilities publication */
orb_advert_t _pos_ctrl_status_pub{nullptr}; /**< navigation capabilities publication */
bool _task_should_exit{false}; /**< if true, sensor task should exit */
bool _task_running{false}; /**< if true, task is running in its mainloop */
@@ -105,7 +104,6 @@ private:
int _params_sub{-1}; /**< notification of parameter updates */
int _pos_sp_triplet_sub{-1};
fw_pos_ctrl_status_s _gnd_pos_ctrl_status{}; /**< navigation capabilities */
manual_control_setpoint_s _manual{}; /**< r/c channel data */
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */
vehicle_attitude_setpoint_s _att_sp{}; /**< vehicle attitude setpoint */
@@ -188,11 +186,6 @@ private:
void position_setpoint_triplet_poll();
void vehicle_control_mode_poll();
/**
* Publish navigation capabilities
*/
void gnd_pos_ctrl_status_publish();
/**
* Control position.
*/