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

try with increasing thrust

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-04-28 18:36:23 +02:00
parent f767ee2583
commit 4b063ce9b2
1 changed files with 9 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -50,6 +50,7 @@ private:
bool has_sent_status = false; bool has_sent_status = false;
bool flying = false; bool flying = false;
int setpoint_count = 0; int setpoint_count = 0;
float thrust = 0;
/** /**
* @brief Only the attitude is enabled, because that is how the drone will be controlled. * @brief Only the attitude is enabled, because that is how the drone will be controlled.
@@ -58,9 +59,12 @@ private:
void send_setpoint() void send_setpoint()
{ {
if (setpoint_count < 21)
setpoint_count++; setpoint_count++;
if (setpoint_count % 20 == 0 && thrust <= 1) {
thrust += 0.1;
}
if (setpoint_count == 20) if (setpoint_count == 20)
{ {
// switch to offboard mode and arm // switch to offboard mode and arm
@@ -79,12 +83,12 @@ private:
// result quaternion // result quaternion
std::array<float, 4> q = {0, 0, 0, 0}; std::array<float, 4> q = {0, 0, 0, 0};
if (this->get_clock()->now().seconds() - start_time_ < 20) if (this->get_clock()->now().seconds() - start_time_ < 30)
{ {
// 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
msg.thrust_body[2] = 1; // down, 100% thrust up msg.thrust_body[2] = thrust; // down, 100% thrust up
calculate_quaternion(q, 0, degrees_to_radians(20), 0); calculate_quaternion(q, 0, degrees_to_radians(20), 0);
} }