fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

vehicle trajectory limit unnecessary updates and reduce logging (#10347)

Browse Source 
- fixes #10345
This commit is contained in:
Daniel Agar 2018-08-30 09:21:13 -04:00 committed by GitHub
parent 94d953eef2
commit d131fea899
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 23 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
View File

@ -194,13 +194,13 @@ protected:
* Vehicle constraints.
* The constraints can vary with tasks.
*/
vehicle_constraints_s _constraints;
vehicle_constraints_s _constraints{};
/**
* Desired waypoints.
* Goals set by the FCU to be sent to the obstacle avoidance system.
*/
vehicle_trajectory_waypoint_s _desired_waypoint;
vehicle_trajectory_waypoint_s _desired_waypoint{};
DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) MPC_XY_VEL_MAX,

4
src/modules/logger/logger.cpp
View File

@ -643,8 +643,8 @@ void Logger::add_default_topics()
add_topic("vehicle_rates_setpoint", 30);
add_topic("vehicle_status", 200);
add_topic("vehicle_status_flags");
add_topic("vehicle_trajectory_waypoint");
add_topic("vehicle_trajectory_waypoint_desired");
add_topic("vehicle_trajectory_waypoint", 200);
add_topic("vehicle_trajectory_waypoint_desired", 200);
add_topic("vehicle_vision_attitude");
add_topic("vehicle_vision_position");
add_topic("vtol_vehicle_status", 200);

41
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -129,9 +129,8 @@ private:
vehicle_local_position_s _local_pos{}; /**< vehicle local position */
vehicle_local_position_setpoint_s _local_pos_sp{}; /**< vehicle local position setpoint */
home_position_s _home_pos{}; /**< home position */
vehicle_trajectory_waypoint_s _traj_wp_avoidance; /**< trajectory waypoint */
vehicle_trajectory_waypoint_s
_traj_wp_avoidance_desired; /**< desired waypoints, inputs to an obstacle avoidance module */
vehicle_trajectory_waypoint_s _traj_wp_avoidance{}; /**< trajectory waypoint */
vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */
@ -275,7 +274,6 @@ private:
*/
void publish_avoidance_desired_waypoint();
/**
* Shim for calling task_main from task_create.
*/
@ -739,8 +737,6 @@ MulticopterPositionControl::task_main()
// they might conflict with each other such as in offboard attitude control.
publish_attitude();
publish_avoidance_desired_waypoint();
} else {
// no flighttask is active: set attitude setpoint to idle
_att_sp.roll_body = _att_sp.pitch_body = 0.0f;
@ -1009,17 +1005,24 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint
void
MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlStates &states)
{
_traj_wp_avoidance_desired = _flight_tasks.getAvoidanceWaypoint();
if (MPC_OBS_AVOID.get()) {
const vehicle_trajectory_waypoint_s traj_wp_desired_new = _flight_tasks.getAvoidanceWaypoint();
_traj_wp_avoidance_desired.timestamp = hrt_absolute_time();
_traj_wp_avoidance_desired.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
if (traj_wp_desired_new.timestamp > _traj_wp_avoidance_desired.timestamp) {
_traj_wp_avoidance_desired = traj_wp_desired_new;
_traj_wp_avoidance_desired.timestamp = hrt_absolute_time();
_traj_wp_avoidance_desired.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].acceleration);
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = states.yaw;
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed = NAN;
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].acceleration);
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = states.yaw;
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed = NAN;
_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
publish_avoidance_desired_waypoint();
}
}
}
void
@ -1057,7 +1060,7 @@ MulticopterPositionControl::use_obstacle_avoidance()
void
MulticopterPositionControl::publish_avoidance_desired_waypoint()
{
/* publish desired waypoint*/
// publish desired waypoint
if (_traj_wp_avoidance_desired_pub != nullptr) {
orb_publish(ORB_ID(vehicle_trajectory_waypoint_desired), _traj_wp_avoidance_desired_pub, &_traj_wp_avoidance_desired);
@ -1065,16 +1068,11 @@ MulticopterPositionControl::publish_avoidance_desired_waypoint()
_traj_wp_avoidance_desired_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired),
&_traj_wp_avoidance_desired);
}
//reset avoidance waypoint desired
_traj_wp_avoidance_desired = _flight_tasks.getEmptyAvoidanceWaypoint();
}
void
MulticopterPositionControl::publish_attitude()
{
_att_sp.timestamp = hrt_absolute_time();
if (_att_sp_pub != nullptr) {
@ -1088,7 +1086,6 @@ MulticopterPositionControl::publish_attitude()
void
MulticopterPositionControl::publish_local_pos_sp()
{
_local_pos_sp.timestamp = hrt_absolute_time();
// publish local position setpoint