yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

FlightTasks: FlightTask definition to cpp, fix include chain, fix cmake

Browse Source
This commit is contained in:
Matthias Grob
2017-11-08 10:57:56 +01:00
committed by Beat Küng
parent c211c807ad
commit 92acbfde3e
7 changed files with 68 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
src/lib/FlightTasks/tasks/FlightTask.cpp Normal file
View File

@@ -0,0 +1,45 @@
#include "FlightTask.hpp"
#include <mathlib/mathlib.h>
int FlightTask::update()
{
_time = hrt_elapsed_time(&_starting_time_stamp) / 1e6f;
_deltatime = math::min((int)hrt_elapsed_time(&_last_time_stamp), _timeout) / 1e6f;
_last_time_stamp = hrt_absolute_time();
updateSubscriptions();
_evaluate_vehicle_position();
return 0;
}
void FlightTask::_evaluate_vehicle_position()
{
if (hrt_elapsed_time(&_sub_vehicle_local_position.get().timestamp) < _timeout) {
_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;
} else {
_velocity.zero(); /* default velocity is all zero */
}
}
void FlightTask::_set_position_setpoint(const matrix::Vector3f position_setpoint)
{
_vehicle_local_position_setpoint.x = position_setpoint(0);
_vehicle_local_position_setpoint.y = position_setpoint(1);
_vehicle_local_position_setpoint.z = position_setpoint(2);
}
void FlightTask::_set_velocity_setpoint(const matrix::Vector3f velocity_setpoint)
{
_vehicle_local_position_setpoint.vx = velocity_setpoint(0);
_vehicle_local_position_setpoint.vy = velocity_setpoint(1);
_vehicle_local_position_setpoint.vz = velocity_setpoint(2);
}
void FlightTask::_set_acceleration_setpoint(const matrix::Vector3f acceleration_setpoint)
{
_vehicle_local_position_setpoint.acc_x = acceleration_setpoint(0);
_vehicle_local_position_setpoint.acc_y = acceleration_setpoint(1);
_vehicle_local_position_setpoint.acc_z = acceleration_setpoint(2);
}