add attitude and stop setpoint

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: