fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 07:00:36 +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-07-25 10:31:17 -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);
}
}
}