From e2ac12ef2c537b540a8803d50202f77cd4b89f38 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Mon, 19 Mar 2018 11:43:11 +0100 Subject: [PATCH] FlightTask: comment fixes --- src/lib/FlightTasks/tasks/FlightTask.cpp | 2 - src/lib/FlightTasks/tasks/FlightTask.hpp | 6 --- src/lib/FlightTasks/tasks/FlightTaskAuto.hpp | 39 +++++++++---------- .../FlightTasks/tasks/FlightTaskAutoLine.cpp | 6 +-- .../FlightTasks/tasks/FlightTaskAutoLine.hpp | 2 +- .../mc_pos_control/PositionControl.cpp | 4 -- 6 files changed, 23 insertions(+), 36 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask.cpp index 7a11a7d79d..fcd16c2a6e 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask.cpp @@ -2,7 +2,6 @@ #include constexpr uint64_t FlightTask::_timeout; - /* First index of empty_setpoint corresponds to time-stamp and requires a finite number. */ const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}}; @@ -60,7 +59,6 @@ bool FlightTask::_evaluateVehicleLocalPosition() _position = matrix::Vector3f(&_sub_vehicle_local_position->get().x); _velocity = matrix::Vector3f(&_sub_vehicle_local_position->get().vx); _yaw = _sub_vehicle_local_position->get().yaw; - return true; } else { diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index 2142ec0907..7074df8e8d 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask.hpp @@ -132,12 +132,6 @@ protected: */ void _resetSetpoints(); - /** - * Vehicle local position subscription - * TODO: Implement a message that is smaller than the - * current vehicle local position message - */ uORB::Subscription *_sub_vehicle_local_position{nullptr}; - bool _evaluateVehicleLocalPosition(); }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp index c4e89ad1aa..dd155571cc 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,7 +45,9 @@ #include #include -/* This enum has to agree with position_setpoint_s type definition */ +/* This enum has to agree with position_setpoint_s type definition + * The only reason for not using the struct position_setpoint is because + * of the size */ enum class WaypointType : int { position = 0, velocity, @@ -69,27 +71,24 @@ public: bool updateInitialize() override; protected: - - matrix::Vector3f _prev_prev_wp{}; /**< Triplet previous-previous triplet. This will be used for smoothing trajectories -> not used yet. */ - matrix::Vector3f _prev_wp{}; /**< Triplet previous setpoint in local frame. If not previous triplet is available, the prev_wp is set to current position. */ - matrix::Vector3f _target{}; /**< Triplet target setpoint in local frame. */ - matrix::Vector3f _next_wp{}; /**< Triplet setpoint in local frame. If no next setpoint is available, next is set to target. */ - float _yaw_wp{0.0f}; /**< Triplet yaw waypoint. Unfortunately navigator sends yaw setpoint continuously. It would be better if a yaw setpoint is attached - to triplet waypoint. This way it would be easy for multicopter to implement features where yaw does not matter. */ - float _mc_cruise_speed{0.0f}; /**< Cruise speed with which multicopter flies and gets set by triplet. If no valid, default cruise speed is used. */ + matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */ + matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */ + matrix::Vector3f _target{}; /**< Target waypoint (local frame).*/ + matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */ + float _yaw_wp{0.0f}; /**< Triplet yaw waypoint. Currently it is not a yaw-waypoint, but rather a yaw setpoint at each time stamp. */ + float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */ WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */ private: - control::BlockParamFloat _mc_cruise_default; /**< Default mc cruise speed*/ - map_projection_reference_s _reference; /**< Reference frame from global to local */ + control::BlockParamFloat _mc_cruise_default; /**< Default mc cruise speed.*/ + map_projection_reference_s _reference; /**< Reference frame from global to local. */ uORB::Subscription *_sub_triplet_setpoint{nullptr}; - map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame */ - float _reference_altitude = 0.0f; /**< Altitude relative to ground */ - hrt_abstime _time_stamp_reference = 0; /**< time stamp when last reference update */ + map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */ + float _reference_altitude = 0.0f; /**< Altitude relative to ground. */ + hrt_abstime _time_stamp_reference = 0; /**< time stamp when last reference update occured. */ - bool _evaluateTriplets(); /**< Checks and sets triplets */ - bool _isFinite(const position_setpoint_s sp); - void _updateReference(); - - void _evaluateVehicleGlobalPosition(); /**< Required for reference update */ + bool _evaluateTriplets(); /**< Checks and sets triplets. */ + bool _isFinite(const position_setpoint_s sp); /**< Checks if all waypoint triplets are finite. */ + void _updateReference(); /**< Updates the local reference. */ + void _evaluateVehicleGlobalPosition(); /**< Checks if global position is valid. */ }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp index 3639db6541..1d6988ca12 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp @@ -69,7 +69,7 @@ bool FlightTaskAutoLine::update() bool follow_line = _type == WaypointType::loiter || _type == WaypointType::position; bool follow_line_prev = _type_previous == WaypointType::loiter || _type_previous == WaypointType::position; - /* 1st time that vehicle starts to follow line. Reset all setpoints to current vehicle state */ + /* 1st time that vehicle starts to follow line. Reset all setpoints to current vehicle state. */ if (follow_line && !follow_line_prev) { _reset(); } @@ -100,8 +100,8 @@ bool FlightTaskAutoLine::update() /* For now yaw setpoint comes directly form triplets. * TODO: In the future, however, yaw should be set in this - * task based on flag: yaw along path, yaw along gimbal, yaw - * same as during home... */ + * task based on flag: yaw along path, yaw based on gimbal, yaw + * same as home yaw ... */ _yaw_setpoint = _yaw_wp; /* Update previous type */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp index f903f377fb..a41e875f1d 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index a7d978827f..e7be49eb76 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -227,10 +227,6 @@ void PositionControl::_velocityController(const float &dt) Vector3f vel_err = _vel_sp - _vel; - /* - * TODO: add offboard acceleration mode - * */ - /* Consider thrust in D-direction */ float thrust_desired_D = Pv(2) * vel_err(2) + Dv(2) * _vel_dot(2) + _thr_int(2) - _ThrHover;