diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-14 10:38:34 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-14 10:38:34 +0200 |
| commit | 51a4ef5de1bc542ac4f7072d95250cd62ea73ed6 (patch) | |
| tree | b71db4faea6a0ac39e4fa28481421a2acc13a896 /src/modules/controllib/uorb/blocks.cpp | |
| parent | 5e0911046173e01a6c66b91d3e38212e093159d0 (diff) | |
| parent | ddc8f1fa5f5b88549af5e4f5f46c751a5f3af3ce (diff) | |
| download | px4-firmware-51a4ef5de1bc542ac4f7072d95250cd62ea73ed6.tar.gz px4-firmware-51a4ef5de1bc542ac4f7072d95250cd62ea73ed6.tar.bz2 px4-firmware-51a4ef5de1bc542ac4f7072d95250cd62ea73ed6.zip | |
merged upstream/master into sbus2_sensorssbus2_sensors
Diffstat (limited to 'src/modules/controllib/uorb/blocks.cpp')
| -rw-r--r-- | src/modules/controllib/uorb/blocks.cpp | 18 |
1 files changed, 9 insertions, 9 deletions
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 448a42a99..e8fecef0d 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -54,26 +54,26 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {}; void BlockWaypointGuidance::update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd) + position_setpoint_s &missionCmd, + position_setpoint_s &lastMissionCmd) { // heading to waypoint float psiTrack = get_bearing_to_next_waypoint( (double)pos.lat / (double)1e7d, (double)pos.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); + missionCmd.lat, + missionCmd.lon); // cross track struct crosstrack_error_s xtrackError; get_distance_to_line(&xtrackError, (double)pos.lat / (double)1e7d, (double)pos.lon / (double)1e7d, - (double)lastPosCmd.lat / (double)1e7d, - (double)lastPosCmd.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); + lastMissionCmd.lat, + lastMissionCmd.lon, + missionCmd.lat, + missionCmd.lon); _psiCmd = _wrap_2pi(psiTrack - _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); @@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), + _missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20), _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz |