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

FlightTasks: added access to prepared velocity state for every task

Browse Source
This commit is contained in:
Matthias Grob
2017-05-23 11:49:34 +02:00
committed by Beat Küng
parent 8da1d3b16e
commit 37cb8c1a59
2 changed files with 19 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
src/lib/FlightTasks/tasks/FlightTask.hpp
View File

@@ -80,6 +80,7 @@ public:
_last_time_stamp = hrt_absolute_time();
_evaluate_sticks();
_evaluate_position();
_evaluate_velocity();
return 0;
};
@@ -107,8 +108,10 @@ protected:
float _deltatime = 0; /*< passed time in seconds since the task was last updated */
void _reset_time() { _starting_time_stamp = hrt_absolute_time(); };
/* Prepared general inputs for every task */
matrix::Vector<float, 4> _sticks;
matrix::Vector3f _position;
matrix::Vector3f _velocity;
void _set_position_setpoint(const matrix::Vector3f position_setpoint)
{
@@ -147,14 +150,17 @@ private:
void _evaluate_position()
{
if (_vehicle_local_position != NULL && hrt_elapsed_time(&_vehicle_local_position->timestamp) < _timeout) {
_position(0) = _vehicle_local_position->x;
_position(1) = _vehicle_local_position->y;
_position(2) = _vehicle_local_position->z;
_position = matrix::Vector3f(&_vehicle_local_position->x);
}
}
void _evaluate_velocity()
{
if (_vehicle_local_position != NULL && hrt_elapsed_time(&_vehicle_local_position->timestamp) < _timeout) {
_velocity = matrix::Vector3f(&_vehicle_local_position->vx);
} else {
for (int i = 0; i < 3; i++) {
_position(i) = 0.f;
}
_velocity = matrix::Vector3f(); /* default is all zero */
}
}
@@ -167,9 +173,7 @@ private:
_sticks(3) = (_manual_control_setpoint->z - 0.5f) * 2.f; /* "thrust" resacaled from [0,1] to [-1,1] */
} else {
for (int i = 0; i < 4; i++) {
_sticks(i) = 0.f;
}
_sticks = matrix::Vector<float, 4>(); /* default is all zero */
}
}

9
src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
View File

@@ -73,10 +73,12 @@ public:
{
FlightTask::update();
r += _sticks(1) * _deltatime;
vu += _sticks(0) * _deltatime;
printf("%f %f %f\n", (double)_deltatime, (double)r, (double)vu);
vu += _sticks(0) * 0.1f * _deltatime;
altitude += _sticks(3) * _deltatime;
//printf("%f %f %f\n", (double)altitude, (double)r, (double)vu);
//printf("%f %f %f\n", (double)_position(0), (double)_position(1), (double)_position(2));
printf("%f %f %f\n", (double)_velocity(0), (double)_velocity(1), (double)_velocity(2));
float v = 2 * M_PI_F * vu; /* velocity for orbiting in radians per second */
float altitude = 2; /* altitude in meters */
_set_position_setpoint(matrix::Vector3f(r * cosf(v * _time), r * sinf(v * _time), -altitude));
return 0;
};
@@ -85,5 +87,6 @@ private:
float r = 2.f; /* radius with which to orbit the target */
float vu = 0.1f; /* velocity for orbiting in revolutions per second */
float altitude = 2; /* altitude in meters */
};