mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-22 01:12:31 +00:00
FlightTasks: refactoring for CamelCase naming convention, small comment and declaration order renicements
This commit is contained in:
@@ -69,7 +69,7 @@ public:
|
|||||||
*/
|
*/
|
||||||
bool update()
|
bool update()
|
||||||
{
|
{
|
||||||
if (is_any_task_active()) {
|
if (isAnyTaskActive()) {
|
||||||
_subscription_array.update();
|
_subscription_array.update();
|
||||||
return _current_task->updateInitialize() && _current_task->update();
|
return _current_task->updateInitialize() && _current_task->update();
|
||||||
}
|
}
|
||||||
@@ -80,9 +80,9 @@ public:
|
|||||||
/**
|
/**
|
||||||
* Get the output data from the current task
|
* Get the output data from the current task
|
||||||
*/
|
*/
|
||||||
const vehicle_local_position_setpoint_s &get_position_setpoint()
|
const vehicle_local_position_setpoint_s &getPositionSetpoint()
|
||||||
{
|
{
|
||||||
return _current_task->get_position_setpoint();
|
return _current_task->getPositionSetpoint();
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -90,24 +90,24 @@ public:
|
|||||||
*/
|
*/
|
||||||
inline const vehicle_local_position_setpoint_s &operator()()
|
inline const vehicle_local_position_setpoint_s &operator()()
|
||||||
{
|
{
|
||||||
return get_position_setpoint();
|
return getPositionSetpoint();
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Switch to the next task in the available list (for testing)
|
* Switch to the next task in the available list (for testing)
|
||||||
* @return true on success, false on error
|
* @return true on success, false on error
|
||||||
*/
|
*/
|
||||||
int switch_task()
|
int switchTask()
|
||||||
{
|
{
|
||||||
return switch_task(_current_task_index + 1);
|
return switchTask(_current_task_index + 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Switch to a specific task (for normal usage)
|
* Switch to a specific task (for normal usage)
|
||||||
* @param task number to switch to
|
* @param task number to switch to
|
||||||
* @return true on success, false on error
|
* @return 0 on success, <0 on error
|
||||||
*/
|
*/
|
||||||
int switch_task(int task_number)
|
int switchTask(int task_number)
|
||||||
{
|
{
|
||||||
/* switch to the running task, nothing to do */
|
/* switch to the running task, nothing to do */
|
||||||
if (task_number == _current_task_index) {
|
if (task_number == _current_task_index) {
|
||||||
@@ -161,17 +161,18 @@ public:
|
|||||||
* Get the number of the active task
|
* Get the number of the active task
|
||||||
* @return number of active task, -1 if there is none
|
* @return number of active task, -1 if there is none
|
||||||
*/
|
*/
|
||||||
int get_active_task() const { return _current_task_index; }
|
int getActiveTask() const { return _current_task_index; }
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Check if any task is active
|
* Check if any task is active
|
||||||
* @return true if a task is active, false if not
|
* @return true if a task is active, false if not
|
||||||
*/
|
*/
|
||||||
bool is_any_task_active() const { return _current_task; }
|
bool isAnyTaskActive() const { return _current_task; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
/** union with all existing tasks: we use it to make sure that only the memory of the largest existing
|
/**
|
||||||
|
* Union with all existing tasks: we use it to make sure that only the memory of the largest existing
|
||||||
* task is needed, and to avoid using dynamic memory allocations.
|
* task is needed, and to avoid using dynamic memory allocations.
|
||||||
*/
|
*/
|
||||||
union TaskUnion {
|
union TaskUnion {
|
||||||
|
|||||||
@@ -25,10 +25,10 @@ bool FlightTask::updateInitialize()
|
|||||||
_time = (_time_stamp_current - _time_stamp_activate) / 1e6f;
|
_time = (_time_stamp_current - _time_stamp_activate) / 1e6f;
|
||||||
_deltatime = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f;
|
_deltatime = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f;
|
||||||
_time_stamp_last = _time_stamp_current;
|
_time_stamp_last = _time_stamp_current;
|
||||||
return _evaluate_vehicle_position();
|
return _evaluateVehiclePosition();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool FlightTask::_evaluate_vehicle_position()
|
bool FlightTask::_evaluateVehiclePosition()
|
||||||
{
|
{
|
||||||
if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) {
|
if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) {
|
||||||
_position = matrix::Vector3f(&_sub_vehicle_local_position->get().x);
|
_position = matrix::Vector3f(&_sub_vehicle_local_position->get().x);
|
||||||
|
|||||||
@@ -60,7 +60,7 @@ public:
|
|||||||
virtual ~FlightTask() = default;
|
virtual ~FlightTask() = default;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* initialize the uORB subscriptions using an array
|
* Initialize the uORB subscriptions using an array
|
||||||
* @return true on success, false on error
|
* @return true on success, false on error
|
||||||
*/
|
*/
|
||||||
virtual bool initializeSubscriptions(SubscriptionArray &subscription_array);
|
virtual bool initializeSubscriptions(SubscriptionArray &subscription_array);
|
||||||
@@ -87,13 +87,13 @@ public:
|
|||||||
/**
|
/**
|
||||||
* Get the output data
|
* Get the output data
|
||||||
*/
|
*/
|
||||||
const vehicle_local_position_setpoint_s &get_position_setpoint()
|
const vehicle_local_position_setpoint_s &getPositionSetpoint()
|
||||||
{
|
{
|
||||||
return _vehicle_local_position_setpoint;
|
return _vehicle_local_position_setpoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
/* time abstraction */
|
/* Time abstraction */
|
||||||
static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
|
static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
|
||||||
float _time = 0; /**< passed time in seconds since the task was activated */
|
float _time = 0; /**< passed time in seconds since the task was activated */
|
||||||
float _deltatime = 0; /**< passed time in seconds since the task was last updated */
|
float _deltatime = 0; /**< passed time in seconds since the task was last updated */
|
||||||
@@ -101,25 +101,26 @@ protected:
|
|||||||
hrt_abstime _time_stamp_current = 0; /**< time stamp at the beginning of the current task update */
|
hrt_abstime _time_stamp_current = 0; /**< time stamp at the beginning of the current task update */
|
||||||
hrt_abstime _time_stamp_last = 0; /**< time stamp when task was last updated */
|
hrt_abstime _time_stamp_last = 0; /**< time stamp when task was last updated */
|
||||||
|
|
||||||
/* Current vehicle position for every task */
|
/* Current vehicle position */
|
||||||
matrix::Vector3f _position; /**< current vehicle position */
|
matrix::Vector3f _position; /**< current vehicle position */
|
||||||
matrix::Vector3f _velocity; /**< current vehicle velocity */
|
matrix::Vector3f _velocity; /**< current vehicle velocity */
|
||||||
float _yaw = 0.f;
|
float _yaw = 0.f;
|
||||||
|
bool _evaluateVehiclePosition();
|
||||||
|
|
||||||
/* Put the position vector produced by the task into the setpoint message */
|
/* Put the position vector produced by the task into the setpoint message */
|
||||||
void _set_position_setpoint(const matrix::Vector3f &position_setpoint) { position_setpoint.copyToRaw(&_vehicle_local_position_setpoint.x); }
|
void _setPositionSetpoint(const matrix::Vector3f &position_setpoint) { position_setpoint.copyToRaw(&_vehicle_local_position_setpoint.x); }
|
||||||
|
|
||||||
/* Put the velocity vector produced by the task into the setpoint message */
|
/* Put the velocity vector produced by the task into the setpoint message */
|
||||||
void _set_velocity_setpoint(const matrix::Vector3f &velocity_setpoint) { velocity_setpoint.copyToRaw(&_vehicle_local_position_setpoint.vx); }
|
void _setVelocitySetpoint(const matrix::Vector3f &velocity_setpoint) { velocity_setpoint.copyToRaw(&_vehicle_local_position_setpoint.vx); }
|
||||||
|
|
||||||
/* Put the acceleration vector produced by the task into the setpoint message */
|
/* Put the acceleration vector produced by the task into the setpoint message */
|
||||||
void _set_acceleration_setpoint(const matrix::Vector3f &acceleration_setpoint) { acceleration_setpoint.copyToRaw(&_vehicle_local_position_setpoint.acc_x); }
|
void _setAccelerationSetpoint(const matrix::Vector3f &acceleration_setpoint) { acceleration_setpoint.copyToRaw(&_vehicle_local_position_setpoint.acc_x); }
|
||||||
|
|
||||||
/* Put the yaw angle produced by the task into the setpoint message */
|
/* Put the yaw angle produced by the task into the setpoint message */
|
||||||
void _set_yaw_setpoint(const float yaw) { _vehicle_local_position_setpoint.yaw = yaw; }
|
void _setYawSetpoint(const float yaw) { _vehicle_local_position_setpoint.yaw = yaw; }
|
||||||
|
|
||||||
/* Put the yaw anglular rate produced by the task into the setpoint message */
|
/* Put the yaw anglular rate produced by the task into the setpoint message */
|
||||||
void _set_yawspeed_setpoint(const float &yawspeed) { _vehicle_local_position_setpoint.yawspeed = yawspeed; }
|
void _setYawspeedSetpoint(const float &yawspeed) { _vehicle_local_position_setpoint.yawspeed = yawspeed; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
|
uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
|
||||||
|
|||||||
@@ -90,7 +90,7 @@ bool FlightTaskManual::initializeSubscriptions(SubscriptionArray &subscription_a
|
|||||||
|
|
||||||
bool FlightTaskManual::updateInitialize()
|
bool FlightTaskManual::updateInitialize()
|
||||||
{
|
{
|
||||||
return FlightTask::updateInitialize() && _evaluate_sticks();
|
return FlightTask::updateInitialize() && _evaluateSticks();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool FlightTaskManual::update()
|
bool FlightTaskManual::update()
|
||||||
@@ -117,7 +117,7 @@ bool FlightTaskManual::update()
|
|||||||
|
|
||||||
/* smooth out velocity setpoint by slewrate and return it */
|
/* smooth out velocity setpoint by slewrate and return it */
|
||||||
vel_sp_slewrate(stick_xy, _sticks(3), velocity_setpoint);
|
vel_sp_slewrate(stick_xy, _sticks(3), velocity_setpoint);
|
||||||
_set_velocity_setpoint(velocity_setpoint);
|
_setVelocitySetpoint(velocity_setpoint);
|
||||||
|
|
||||||
/* handle position and altitude hold */
|
/* handle position and altitude hold */
|
||||||
const bool stick_xy_zero = stick_xy_norm <= FLT_EPSILON;
|
const bool stick_xy_zero = stick_xy_norm <= FLT_EPSILON;
|
||||||
@@ -143,11 +143,11 @@ bool FlightTaskManual::update()
|
|||||||
_hold_position(2) = NAN;
|
_hold_position(2) = NAN;
|
||||||
}
|
}
|
||||||
|
|
||||||
_set_position_setpoint(_hold_position);
|
_setPositionSetpoint(_hold_position);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool FlightTaskManual::_evaluate_sticks()
|
bool FlightTaskManual::_evaluateSticks()
|
||||||
{
|
{
|
||||||
if ((_time_stamp_current - _sub_manual_control_setpoint->get().timestamp) < _timeout) {
|
if ((_time_stamp_current - _sub_manual_control_setpoint->get().timestamp) < _timeout) {
|
||||||
/* get data and scale correctly */
|
/* get data and scale correctly */
|
||||||
|
|||||||
@@ -62,6 +62,7 @@ public:
|
|||||||
|
|
||||||
protected:
|
protected:
|
||||||
matrix::Vector<float, 4> _sticks;
|
matrix::Vector<float, 4> _sticks;
|
||||||
|
bool _evaluateSticks();
|
||||||
|
|
||||||
float get_input_frame_yaw() { return _yaw; }
|
float get_input_frame_yaw() { return _yaw; }
|
||||||
|
|
||||||
|
|||||||
@@ -80,8 +80,8 @@ bool FlightTaskOrbit::update()
|
|||||||
|
|
||||||
float yaw = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
|
float yaw = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
|
||||||
|
|
||||||
_set_position_setpoint(Vector3f(NAN, NAN, _z));
|
_setPositionSetpoint(Vector3f(NAN, NAN, _z));
|
||||||
_set_velocity_setpoint(Vector3f(velocity_xy(0), velocity_xy(1), 0.f));
|
_setVelocitySetpoint(Vector3f(velocity_xy(0), velocity_xy(1), 0.f));
|
||||||
_set_yaw_setpoint(yaw);
|
_setYawSetpoint(yaw);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -55,7 +55,6 @@ public:
|
|||||||
bool update() override;
|
bool update() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
float _r = 0.f; /**< radius with which to orbit the target */
|
float _r = 0.f; /**< radius with which to orbit the target */
|
||||||
float _v = 0.f; /**< linear velocity for orbiting in m/s */
|
float _v = 0.f; /**< linear velocity for orbiting in m/s */
|
||||||
float _z = 0.f; /**< local z coordinate in meters */
|
float _z = 0.f; /**< local z coordinate in meters */
|
||||||
|
|||||||
Reference in New Issue
Block a user