sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

remove yaw pitch and roll body

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-03 14:35:44 +02:00
parent 6db6c89bd3
commit fbef263682
1 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/px4_connection/src/px4_controller.cpp
View File

@@ -83,16 +83,16 @@ private:
{ {
if (armed) if (armed)
{ {
if (request->yaw == 0 && request->pitch = 0 && request->roll = 0)
{
last_setpoint[0] = degrees_to_radians(request->yaw); last_setpoint[0] = degrees_to_radians(request->yaw);
last_setpoint[1] = degrees_to_radians(request->pitch); last_setpoint[1] = degrees_to_radians(request->pitch);
last_setpoint[2] = degrees_to_radians(request->roll); last_setpoint[2] = degrees_to_radians(request->roll);
} else { // if (request->yaw == 0 && request->pitch = 0 && request->roll = 0)
last_setpoint[0] += degrees_to_radians(request->yaw); // {
last_setpoint[1] += degrees_to_radians(request->pitch); // } else {
last_setpoint[2] += degrees_to_radians(request->roll); // last_setpoint[0] += degrees_to_radians(request->yaw);
} // last_setpoint[1] += degrees_to_radians(request->pitch);
// last_setpoint[2] += degrees_to_radians(request->roll);
// }
last_thrust = request->thrust; last_thrust = request->thrust;
@@ -169,9 +169,9 @@ private:
// if (this->get_clock()->now().seconds() - start_time_ < 30) // if (this->get_clock()->now().seconds() - start_time_ < 30)
// { // {
msg.yaw_body = last_setpoint[0]; // msg.yaw_body = last_setpoint[0];
msg.pitch_body = last_setpoint[1]; // msg.pitch_body = last_setpoint[1];
msg.roll_body = last_setpoint[2]; // msg.roll_body = last_setpoint[2];
// move up? // move up?
msg.thrust_body[0] = 0; // north msg.thrust_body[0] = 0; // north
msg.thrust_body[1] = 0; // east msg.thrust_body[1] = 0; // east