From 0c0dc2098ae99d93d9e41ff897b6d846ece1ee6b Mon Sep 17 00:00:00 2001
From: zeisele <zeisele@iastate.edu>
Date: Sun, 20 Mar 2022 19:05:31 +0100
Subject: [PATCH] add attitude and stop setpoint
---
.../src/ccrazyflie/CCrazyflie_commands.cpp | 60 ++++++++++---------
1 file changed, 32 insertions(+), 28 deletions(-)
diff --git a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_commands.cpp b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_commands.cpp
index bc0a76965..95ba1c91d 100644
--- a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_commands.cpp
+++ b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_commands.cpp
@@ -338,41 +338,45 @@ bool CCrazyflie::parseReceivedUserCommand() {
float yaw = build_float(&command.payload[17]);
float fthrust = build_float(&command.payload[5]);
float ftime = build_float(&command.payload[1]);
+ int nSize = 4 * sizeof(float) + sizeof(char);
+ char cBuffer[nSize];
if(command.payload[0] == 1) {
- short thrust = (short) fthrust;
- this->sendSetpoint(roll, pitch, yaw, thrust);
+ cBuffer[0] = 9;
}
else if(command.payload[0] == 2) {
- int nSize = 4 * sizeof(float) + sizeof(char);
- char cBuffer[nSize];
cBuffer[0] = 8;
- memcpy(&cBuffer[0 * sizeof(float) + 1], &roll, sizeof(float));
- memcpy(&cBuffer[1 * sizeof(float) + 1], &pitch, sizeof(float));
- memcpy(&cBuffer[2 * sizeof(float) + 1], &yaw, sizeof(float));
- memcpy(&cBuffer[3 * sizeof(float) + 1], &fthrust, sizeof(float));
-
- CCRTPPacket *crtpPacket = new CCRTPPacket(cBuffer, nSize, 7);
- crtpPacket->setChannel(0x00);
- int milliseconds = ftime*1000;
- CCRTPPacket *crtpReceived = NULL;
- if(ftime == 0) {
+ }
+ else {
+ break;
+ }
+ memcpy(&cBuffer[0 * sizeof(float) + 1], &roll, sizeof(float));
+ memcpy(&cBuffer[1 * sizeof(float) + 1], &pitch, sizeof(float));
+ memcpy(&cBuffer[2 * sizeof(float) + 1], &yaw, sizeof(float));
+ memcpy(&cBuffer[3 * sizeof(float) + 1], &fthrust, sizeof(float));
+
+ CCRTPPacket *crtpPacket = new CCRTPPacket(cBuffer, nSize, 7);
+ crtpPacket->setChannel(0x00);
+ int milliseconds = ftime*1000;
+ CCRTPPacket *crtpReceived = NULL;
+ if(ftime == 0) {
+ crtpReceived = m_crRadio->sendPacket(m_nRadioChannel, crtpPacket, this);
+ }
+ else {
+ auto start = std::chrono::high_resolution_clock::now();
+ auto timeNow = std::chrono::high_resolution_clock::now();
+ auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(timeNow-start);
+ while(duration.count() < milliseconds) {
+ usleep(10);
crtpReceived = m_crRadio->sendPacket(m_nRadioChannel, crtpPacket, this);
+ timeNow = std::chrono::high_resolution_clock::now();
+ duration = std::chrono::duration_cast<std::chrono::milliseconds>(timeNow-start);
}
- else {
- auto start = std::chrono::high_resolution_clock::now();
- auto timeNow = std::chrono::high_resolution_clock::now();
- auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(timeNow-start);
- while(duration.count() < milliseconds) {
- usleep(10);
- crtpReceived = m_crRadio->sendPacket(m_nRadioChannel, crtpPacket, this);
- timeNow = std::chrono::high_resolution_clock::now();
- duration = std::chrono::duration_cast<std::chrono::milliseconds>(timeNow-start);
- }
- }
-
-
- delete crtpPacket;
+ cBuffer[0] = 0;
+ nSize = sizeof(char);
+ crtpPacket->setData(cBuffer, nSize);
+ crtpReceived = m_crRadio->sendPacket(m_nRadioChannel, crtpPacket, this);
}
+ delete crtpPacket;
break;
}
case ACMD_GETLOGFILE:
--
GitLab