diff --git a/crazyCART.sh b/crazyCART.sh
new file mode 100755
index 0000000000000000000000000000000000000000..e6547e49183cf914864829165fffcbffbc930d85
--- /dev/null
+++ b/crazyCART.sh
@@ -0,0 +1,31 @@
+#!/bin/bash
+
+function handler()
+{
+ kill -s SIGINT $CFPID
+ kill -s SIGINT $APID
+ kill -s SIGINT $BPID
+}
+
+cd ./crazyflie_groundstation
+#xterm -hold -e './crazyflieGroundStation' &
+./crazyflieGroundStation &
+CFPID=$!
+echo "Waiting for crazyflie groundstation to be created..."
+cf_gs=crazyflie_groundstation.socket
+while [ ! -S "$cf_gs" ]
+do
+ sleep 1
+done
+cd ../groundStation/adapters/crazyflie
+sleep 1
+./bin/cf_adapter &
+APID=$!
+sleep 1
+cd ../..
+./BackEnd &
+BPID=$!
+
+trap handler INT
+
+wait
\ No newline at end of file
diff --git a/crazyflie_groundstation/.gitignore b/crazyflie_groundstation/.gitignore
index 245a8f80ebc0134dfbe67cec806e74803aef375b..28e2026088a5c9e92889acf2da7bd03cb5c90312 100644
--- a/crazyflie_groundstation/.gitignore
+++ b/crazyflie_groundstation/.gitignore
@@ -1,2 +1,3 @@
crazyflieGroundStation
test_connection
+logs/*
diff --git a/crazyflie_groundstation/inc/CCrazyflie.h b/crazyflie_groundstation/inc/CCrazyflie.h
index 62281cefe605649fa2dda02e1b3936608f747665..b1a738f6b2891df219c50a728d299781d5a9a477 100644
--- a/crazyflie_groundstation/inc/CCrazyflie.h
+++ b/crazyflie_groundstation/inc/CCrazyflie.h
@@ -313,6 +313,8 @@ class CCrazyflie {
*/
void openLogFile(char *baseFileName);
+ void printUpdatedHeader();
+
bool loadLoggingBlocksFromFile(std::string blockFileName);
bool resetLoggingBlocks();
diff --git a/crazyflie_groundstation/inc/CTOC.h b/crazyflie_groundstation/inc/CTOC.h
index 22da31fdc714abab28da81ba2704772eadc341d9..fd96360f7b41207504cf9f149e7e4c25063dac5c 100644
--- a/crazyflie_groundstation/inc/CTOC.h
+++ b/crazyflie_groundstation/inc/CTOC.h
@@ -39,6 +39,7 @@
#include <cstdlib>
#include <iostream>
#include <fstream>
+#include <vector>
// Private
#include "CCrazyRadio.h"
@@ -67,6 +68,7 @@ struct LoggingBlock {
int nID;
double dFrequency;
std::list<int> lstElementIDs;
+ bool isActive;
};
// Backward declaration of the CCrazyflie and CCrazyradio class to make the compiler happy
@@ -82,6 +84,8 @@ class CTOC {
int m_nItemCount;
std::list<struct TOCElement> m_lstTOCElements;
std::list<struct LoggingBlock> m_lstLoggingBlocks;
+ std::vector<std::string> m_lstActiveLogging;
+ int updateAmount = 0;
std::ofstream param_toc_file;
std::ofstream log_toc_file;
std::string paramfilename;
@@ -116,6 +120,7 @@ class CTOC {
// For loggable variables only
bool registerLoggingBlock(std::string strName, double dFrequency);
bool unregisterLoggingBlock(std::string strName);
+ bool unregisterLoggingBlocks();
struct LoggingBlock loggingBlockForName(std::string strName, bool& bFound);
struct LoggingBlock loggingBlockForID(int nID, bool& bFound);
@@ -127,6 +132,8 @@ class CTOC {
bool enableLogging(std::string strBlockName);
bool disableLogging(std::string strBlockName);
+ void setBlockActive(int nID, bool activity);
+ std::string createActiveHeader();
// void processPackets(std::list<CCRTPPacket*> lstPackets);
void processPackets(CCRTPPacket *crtpPacket);
@@ -140,6 +147,9 @@ class CTOC {
void openParamTOCFile(char *baseFileName);
std::string getLogTOCFile();
std::string getParamTOCFile();
+
+ std::string activeLogName(int index);
+ int sizeOfActiveList();
};
diff --git a/crazyflie_groundstation/src/CTOC.cpp b/crazyflie_groundstation/src/CTOC.cpp
index d609404474455edb9c44127000cad972ba0a5ac6..27c1cf444cd3b99d808785d828cb0716fb62b803 100644
--- a/crazyflie_groundstation/src/CTOC.cpp
+++ b/crazyflie_groundstation/src/CTOC.cpp
@@ -30,10 +30,10 @@
#include <limits>
-static const char *param_header = "Param ID\tType\t\tGroup\t\t"
+static const char *param_header = "Param ID\tType\tGroup\t"
"Identifier Name";
-static const char *log_header = "Log ID\tType\t\tGroup\t\t"
+static const char *log_header = "Log ID\tType\tGroup\t"
"Identifier Name";
CTOC::CTOC(CCrazyRadio *crRadio, CCrazyflie *crazyflie, int radioChannel, int nPort) {
@@ -172,6 +172,7 @@ bool CTOC::downloadTOC() {
printf("Parameter Progress: %i / %i\n", nI+1, m_nItemCount);
this->requestItem(nI);
}
+ param_toc_file.close();
}
else {
this->requestMetaData();
@@ -179,6 +180,7 @@ bool CTOC::downloadTOC() {
printf("Logging Progress: %i / %i\n", nI+1, m_nItemCount);
this->requestItem(nI);
}
+ log_toc_file.close();
}
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__);
return true;
@@ -219,10 +221,10 @@ bool CTOC::processItem(CCRTPPacket* crtpItem) {
if(ALL_THE_DEBUG) printf("Group.Name: %s.%s ID: %i Type: %i ", strGroup.c_str(), strIdentifier.c_str(), nID, nType);
if(m_nPort == 0x02) {
- this->param_toc_file << teNew.nID << "\t\t" << teNew.nType << "\t\t" << teNew.strGroup << "\t\t" << teNew.strIdentifier << std::endl;
+ this->param_toc_file << teNew.nID << "\t" << teNew.nType << "\t" << teNew.strGroup << "\t" << teNew.strIdentifier << std::endl;
}
else if(m_nPort = 0x05) {
- this->log_toc_file << teNew.nID << "\t\t" << teNew.nType << "\t\t" << teNew.strGroup << "\t\t" << teNew.strIdentifier << std::endl;
+ this->log_toc_file << teNew.nID << "\t" << teNew.nType << "\t" << teNew.strGroup << "\t" << teNew.strIdentifier << std::endl;
}
m_lstTOCElements.push_back(teNew);
@@ -561,6 +563,45 @@ void CTOC::printLoggingBlocksInitialized() {
}
}
+bool compare_id(const struct LoggingBlock& first, const struct LoggingBlock& second) {
+ return (first.nID < second.nID);
+}
+
+std::string CTOC::createActiveHeader() {
+ std::string header = "#" + std::to_string(updateAmount) + ": " + "\t";
+ header = header + "time" + "\t";
+ std::string element = "";
+ m_lstActiveLogging.clear();
+ m_lstLoggingBlocks.sort(compare_id);
+ bool bFound = false;
+ for (std::list<struct LoggingBlock>::iterator itBlock = m_lstLoggingBlocks.begin();
+ itBlock != m_lstLoggingBlocks.end();
+ itBlock++) {
+ struct LoggingBlock lbCurrent = *itBlock;
+
+ if(lbCurrent.isActive) {
+ for(int i = 0; i < lbCurrent.lstElementIDs.size(); i++) {
+ int elementID = this->elementIDinBlock(lbCurrent.nID, i);
+ struct TOCElement teCurrent = this->elementForID(elementID,
+ bFound);
+ element = teCurrent.strGroup + "." + teCurrent.strIdentifier;
+ header = header + element + "\t";
+ m_lstActiveLogging.push_back(element);
+ }
+ }
+ }
+ updateAmount++;
+ return header;
+}
+
+std::string CTOC::activeLogName(int index) {
+ return m_lstActiveLogging.at(index);
+}
+
+int CTOC::sizeOfActiveList() {
+ return m_lstActiveLogging.size();
+}
+
struct LoggingBlock CTOC::loggingBlockForName(std::string strName,
bool& bFound) {
if( ALL_THE_DEBUG ) printf( "%s\n", __FUNCTION__);
@@ -667,6 +708,7 @@ bool CTOC::enableLogging(std::string strBlockName) {
struct LoggingBlock lbCurrent = this->loggingBlockForName(strBlockName,
bFound);
if (bFound) {
+ this->setBlockActive(lbCurrent.nID, true);
uint8_t logPeriod= (uint8_t) ( (1 / lbCurrent.dFrequency) * 1000 );
char cPayload[3] = { 0x03, (char)lbCurrent.nID, logPeriod }; // JRB: A little concerned about this cast from double to char
@@ -692,6 +734,7 @@ bool CTOC::disableLogging(std::string strBlockName) {
bFound);
if (bFound) {
+ this->setBlockActive(lbCurrent.nID, false);
char cPayload[2] = { 0x04, (char)lbCurrent.nID }; // JRB: A little concerned about this cast from double to char
CCRTPPacket* crtpEnable = new CCRTPPacket(cPayload, 2, 1);
@@ -721,6 +764,31 @@ bool CTOC::unregisterLoggingBlock(std::string strName) {
return false;
}
+bool CTOC::unregisterLoggingBlocks() {
+ if( ALL_THE_DEBUG ) printf( "%s\n", __FUNCTION__);
+ bool bFound;
+
+ char cPayload[1] = { 0x05 };
+
+ CCRTPPacket* resetBlock = new CCRTPPacket(cPayload, 1, 1);
+ resetBlock->setPort(0x05);
+ resetBlock->setChannel(1);
+
+ CCRTPPacket* crtpReceived = m_crRadio->sendAndReceive(resetBlock, m_nRadioChannel, m_crazyflie);
+
+ delete resetBlock;
+
+ if (crtpReceived) {
+ delete crtpReceived;
+ for(std::list<struct LoggingBlock>::iterator itBlock = m_lstLoggingBlocks.begin(); itBlock != m_lstLoggingBlocks.end(); itBlock++) {
+ itBlock = m_lstLoggingBlocks.erase(itBlock);
+ }
+ return true;
+ }
+
+ return false;
+}
+
bool CTOC::unregisterLoggingBlockID(int nID) {
if( ALL_THE_DEBUG ) printf( "%s\n", __FUNCTION__);
char cPayload[2] = { 0x02, (char)nID };
@@ -945,8 +1013,9 @@ void CTOC::openParamTOCFile(char *baseFileName) {
printf("Opening param toc file %s for quadcopter %d...", logFileName, (this->m_crazyflie->getQuadcopterNumber() + 1));
try {
- char dir[250];
- getcwd(dir, 250);
+ char dir[256];
+ dir[0] = '/';
+ getcwd(&dir[1], 255);
std::string fullFilePath(dir);
fullFilePath += "/";
fullFilePath += logFileName;
@@ -985,8 +1054,9 @@ void CTOC::openLogTOCFile(char *baseFileName) {
printf("Opening param toc file %s for quadcopter %d...", logFileName, (this->m_crazyflie->getQuadcopterNumber() + 1));
try {
- char dir[250];
- getcwd(dir, 250);
+ char dir[256];
+ dir[0] = '/';
+ getcwd(&dir[1], 255);
std::string fullFilePath(dir);
fullFilePath += "/";
fullFilePath += logFileName;
@@ -1021,4 +1091,15 @@ std::string CTOC::getParamTOCFile() {
else {
return this->paramfilename;
}
+}
+
+void CTOC::setBlockActive(int nID, bool activity) {
+ if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__);
+ for(std::list<struct LoggingBlock>::iterator itBlock = m_lstLoggingBlocks.begin(); itBlock != m_lstLoggingBlocks.end(); itBlock++) {
+ struct LoggingBlock &lbCurrent = *itBlock;
+ if (nID == lbCurrent.nID) {
+ lbCurrent.isActive = activity;
+ return;
+ }
+ }
}
\ No newline at end of file
diff --git a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_commands.cpp b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_commands.cpp
index 7916a1100f664fc88eac2306b9364e754e08ad3f..2a3e4cf021544c214007bf32ddb47470b752e08f 100644
--- a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_commands.cpp
+++ b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_commands.cpp
@@ -179,7 +179,7 @@ bool CCrazyflie::parseReceivedUserCommand() {
case ACMD_GETPARAM:
{
- int nSize = 1;
+ int nSize = 2;
char cBuffer[nSize];
CCRTPPacket *crtpPacket = new CCRTPPacket(cBuffer, nSize, CRTP_PORT_PARAM);
@@ -187,13 +187,15 @@ bool CCrazyflie::parseReceivedUserCommand() {
CCRTPPacket *crtpReceived;
//int id = (char) command.payload;
- char id = (char) command.payload[2];
+ //char id = (char) command.payload[2];
+ uint16_t id;
+ memcpy(&id, &command.payload[2], sizeof(uint16_t));
+ memcpy(cBuffer, &command.payload[2], sizeof(uint16_t));
//float fvalue = build_float(&command.payload[4]);
//uint16_t pvalue = (uint16_t) fvalue;
bool bFound = false;
struct TOCElement element = this->m_tocParameters->elementForID(id, bFound);
- cBuffer[0] = id;
//memcpy(&cBuffer[1 * sizeof(uint16_t)], &pvalue, sizeof(uint16_t));
crtpPacket->setData(cBuffer, nSize);
crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
@@ -208,7 +210,7 @@ bool CCrazyflie::parseReceivedUserCommand() {
case ACMD_SETPARAM:
{
- int nSize = sizeof(double) + 1;
+ int nSize = sizeof(double) + 2;
char cBuffer[nSize];
CCRTPPacket *crtpPacket = new CCRTPPacket(cBuffer, nSize, CRTP_PORT_PARAM);
@@ -216,7 +218,10 @@ bool CCrazyflie::parseReceivedUserCommand() {
CCRTPPacket *crtpReceived;
//int id = (char) command.payload;
- char pid = (char) command.payload[2];
+ //char pid = (char) command.payload[2];
+ uint16_t pid;
+ memcpy(&pid, &command.payload[2], 2);
+ memcpy(cBuffer, &command.payload[2], 2);
bool bFound = false;
struct TOCElement element = this->m_tocParameters->elementForID(pid, bFound);
uint8_t pType = element.nType;
@@ -226,9 +231,8 @@ bool CCrazyflie::parseReceivedUserCommand() {
{
uint8_t pvalue = (uint8_t) fvalue;
- cBuffer[0] = pid;
- memcpy(&cBuffer[1], &pvalue, sizeof(uint8_t));
- nSize = 1 + sizeof(uint8_t);
+ memcpy(&cBuffer[2], &pvalue, sizeof(uint8_t));
+ nSize = 2 + sizeof(uint8_t);
crtpPacket->setData(cBuffer, nSize);
crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
break;
@@ -237,9 +241,8 @@ bool CCrazyflie::parseReceivedUserCommand() {
{
uint16_t pvalue = (uint16_t) fvalue;
- cBuffer[0] = pid;
- memcpy(&cBuffer[1], &pvalue, sizeof(uint16_t));
- nSize = 1 + sizeof(uint16_t);
+ memcpy(&cBuffer[2], &pvalue, sizeof(uint16_t));
+ nSize = 2 + sizeof(uint16_t);
crtpPacket->setData(cBuffer, nSize);
crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
break;
@@ -248,9 +251,8 @@ bool CCrazyflie::parseReceivedUserCommand() {
{
uint32_t pvalue = (uint32_t) fvalue;
- cBuffer[0] = pid;
- memcpy(&cBuffer[1], &pvalue, sizeof(uint32_t));
- nSize = 1 + sizeof(uint32_t);
+ memcpy(&cBuffer[2], &pvalue, sizeof(uint32_t));
+ nSize = 2 + sizeof(uint32_t);
crtpPacket->setData(cBuffer, nSize);
crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
break;
@@ -259,9 +261,8 @@ bool CCrazyflie::parseReceivedUserCommand() {
{
uint64_t pvalue = (uint64_t) fvalue;
- cBuffer[0] = pid;
- memcpy(&cBuffer[1], &pvalue, sizeof(uint64_t));
- nSize = 1 + sizeof(uint64_t);
+ memcpy(&cBuffer[2], &pvalue, sizeof(uint64_t));
+ nSize = 2 + sizeof(uint64_t);
crtpPacket->setData(cBuffer, nSize);
crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
break;
@@ -270,9 +271,8 @@ bool CCrazyflie::parseReceivedUserCommand() {
{
int8_t pvalue = (int8_t) fvalue;
- cBuffer[0] = pid;
- memcpy(&cBuffer[1], &pvalue, sizeof(int8_t));
- nSize = 1 + sizeof(int8_t);
+ memcpy(&cBuffer[2], &pvalue, sizeof(int8_t));
+ nSize = 2 + sizeof(int8_t);
crtpPacket->setData(cBuffer, nSize);
crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
break;
@@ -281,9 +281,8 @@ bool CCrazyflie::parseReceivedUserCommand() {
{
int16_t pvalue = (int16_t) fvalue;
- cBuffer[0] = pid;
- memcpy(&cBuffer[1], &pvalue, sizeof(int16_t));
- nSize = 1 + sizeof(int16_t);
+ memcpy(&cBuffer[2], &pvalue, sizeof(int16_t));
+ nSize = 2 + sizeof(int16_t);
crtpPacket->setData(cBuffer, nSize);
crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
break;
@@ -292,9 +291,8 @@ bool CCrazyflie::parseReceivedUserCommand() {
{
int32_t pvalue = (uint32_t) fvalue;
- cBuffer[0] = pid;
- memcpy(&cBuffer[1], &pvalue, sizeof(int32_t));
- nSize = 1 + sizeof(int32_t);
+ memcpy(&cBuffer[2], &pvalue, sizeof(int32_t));
+ nSize = 2 + sizeof(int32_t);
crtpPacket->setData(cBuffer, nSize);
crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
break;
@@ -303,9 +301,8 @@ bool CCrazyflie::parseReceivedUserCommand() {
{
int64_t pvalue = (int64_t) fvalue;
- cBuffer[0] = pid;
- memcpy(&cBuffer[1], &pvalue, sizeof(int64_t));
- nSize = 1 + sizeof(int64_t);
+ memcpy(&cBuffer[2], &pvalue, sizeof(int64_t));
+ nSize = 2 + sizeof(int64_t);
crtpPacket->setData(cBuffer, nSize);
crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
break;
@@ -314,9 +311,8 @@ bool CCrazyflie::parseReceivedUserCommand() {
{
float pvalue = fvalue;
- cBuffer[0] = pid;
- memcpy(&cBuffer[1], &pvalue, sizeof(float));
- nSize = 1 + sizeof(float);
+ memcpy(&cBuffer[2], &pvalue, sizeof(float));
+ nSize = 2 + sizeof(float);
crtpPacket->setData(cBuffer, nSize);
crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
break;
@@ -325,16 +321,13 @@ bool CCrazyflie::parseReceivedUserCommand() {
{
double pvalue = (double) fvalue;
- cBuffer[0] = pid;
- memcpy(&cBuffer[1], &pvalue, sizeof(double));
- nSize = 1 + sizeof(double);
+ memcpy(&cBuffer[2], &pvalue, sizeof(double));
+ nSize = 2 + sizeof(double);
crtpPacket->setData(cBuffer, nSize);
crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
break;
}
}
-
-
break;
}
@@ -345,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:
@@ -425,34 +422,40 @@ bool CCrazyflie::parseReceivedUserCommand() {
case 0:
{
this->resetLoggingBlocks();
+ this->printUpdatedHeader();
break;
}
case 1:
{
this->resetLoggingBlocks();
this->loadLoggingBlocksFromFile("loggingBlocks.txt");
+ this->printUpdatedHeader();
break;
}
case 2:
{
this->loadLoggingBlocksFromFile("loggingBlocks.txt");
+ this->printUpdatedHeader();
break;
}
case 3:
{
this->m_tocLogs->unregisterLoggingBlockID(id);
+ this->printUpdatedHeader();
break;
}
case 4:
{
struct LoggingBlock block = this->m_tocLogs->loggingBlockForID(id, found);
this->enableLogging(block.strName);
+ this->printUpdatedHeader();
break;
}
case 5:
{
struct LoggingBlock block = this->m_tocLogs->loggingBlockForID(id, found);
this->disableLogging(block.strName);
+ this->printUpdatedHeader();
break;
}
}
diff --git a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp
index 293470194d98e1370fb7bb0780ab3e79a69f7f7b..a786c9f4eaa67e0aa673f1623ea16aff62e63398 100644
--- a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp
+++ b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp
@@ -52,8 +52,9 @@ void CCrazyflie::openLogFile(char *baseFileName) {
printf("Opening log file %s for quadcopter %d...", logFileName, (m_quadNum + 1));
try {
- char dir[250];
- getcwd(dir, 250);
+ char dir[256];
+ dir[0] = '/';
+ getcwd(&dir[1], 255);
std::string fullFilePath(dir);
fullFilePath += "/";
fullFilePath += logFileName;
@@ -68,9 +69,9 @@ void CCrazyflie::openLogFile(char *baseFileName) {
file_log << "#Crazyflie" << std::endl;
// Add the controller type to the logfile
- file_log << "#" << this->getControllerTypeString() << std::endl;
- file_log << variables << std::endl;
- file_log << units << std::endl;
+ file_log << "#" << this->getControllerTypeString();
+ //file_log << variables << std::endl;
+ //file_log << units << std::endl;
std::cout << " Complete" << std::endl;
}
@@ -79,7 +80,7 @@ void CCrazyflie::openLogFile(char *baseFileName) {
* Write data to the log file
*/
void CCrazyflie::writeLogData() {
- // Make a new line
+ /* Make a new line
file_log << std::endl;
// Print the time
@@ -110,6 +111,12 @@ void CCrazyflie::writeLogData() {
file_log << "\t\t" << this->sensorDoubleValue("ctrlStdnt.rollRate");
file_log << "\t\t" << this->sensorDoubleValue("testStand");
+ */
+ file_log << std::endl;
+ file_log << this->currentTime();
+ for(int i = 0; i < m_tocLogs->sizeOfActiveList(); i++) {
+ file_log << "\t" << this->sensorDoubleValue(m_tocLogs->activeLogName(i));
+ }
}
@@ -165,22 +172,8 @@ bool CCrazyflie::addLoggingBlock(const char *name, uint16_t frequency) {
bool CCrazyflie::resetLoggingBlocks() {
if( ALL_THE_DEBUG ) printf( "%s\n", __FUNCTION__);
- char cPayload[1] = { 0x05 };
-
- CCRTPPacket* resetBlock = new CCRTPPacket(cPayload, 2, 1);
- resetBlock->setPort(0x05);
- resetBlock->setChannel(1);
-
- CCRTPPacket* crtpReceived = m_crRadio->sendAndReceive(resetBlock, m_nRadioChannel, this);
-
- delete resetBlock;
-
- if (crtpReceived) {
- delete crtpReceived;
- return true;
- }
-
- return false;
+ bool result = m_tocLogs->unregisterLoggingBlocks();
+ return result;
}
void CCrazyflie::removeLoggingBlock(const char *name) {
@@ -295,7 +288,7 @@ bool CCrazyflie::loadLoggingBlocksFromFile(std::string blockFileName) {
else if(!line.compare("END BLOCK")) {
enableLogging(curName);
blockLinesRead = 0;
- int curId = -1;
+ curId = -1;
curFreq = 0;
curName = "";
curEntryName = "";
@@ -304,6 +297,16 @@ bool CCrazyflie::loadLoggingBlocksFromFile(std::string blockFileName) {
else if(blockLinesRead == 1) {
try {
curId = std::stoi(line);
+ bool found = false;
+ m_tocLogs->loggingBlockForID(curId, found);
+ if(found) {
+ blockLinesRead = 0;
+ curId = -1;
+ curFreq = 0;
+ curName = "";
+ curEntryName = "";
+ continue;
+ }
}
catch(std::invalid_argument& e) {
std::cout << "Error: invalid ID. Skipping Block" << std::endl;
@@ -343,4 +346,9 @@ bool CCrazyflie::loadLoggingBlocksFromFile(std::string blockFileName) {
}
blockFile.close();
return true;
+}
+
+void CCrazyflie::printUpdatedHeader() {
+ file_log << std::endl;
+ file_log << m_tocLogs->createActiveHeader();
}
\ No newline at end of file
diff --git a/crazyflie_groundstation/src/crazyflieGroundStation.cpp b/crazyflie_groundstation/src/crazyflieGroundStation.cpp
index acf54773031a0250c67da9d145339f0cea419a96..c3f2a930fb523a7a47683886dcb80e8fa7036336 100644
--- a/crazyflie_groundstation/src/crazyflieGroundStation.cpp
+++ b/crazyflie_groundstation/src/crazyflieGroundStation.cpp
@@ -368,6 +368,7 @@ int main(int argc, char **argv) {
*/
crazyflie_info[i].cflieCopter->loadLoggingBlocksFromFile("loggingBlocks.txt");
+ crazyflie_info[i].cflieCopter->printUpdatedHeader();
crazyflie_info[i].cflieCopter->displayLoggingBlocksInitialized();
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/crtp_commander_generic.c b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/crtp_commander_generic.c
index 9d038cc5cef93c1fedca5e75ffc24ea7ad3e9524..a4b1f55c47b4b46387a0a6fd2fb5348b3465c9e1 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/crtp_commander_generic.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/crtp_commander_generic.c
@@ -72,6 +72,7 @@ enum packet_type {
fullStateType = 6,
positionType = 7,
attitudeRateType = 8,
+ attitudeType = 9,
};
/* ---===== 2 - Decoding functions =====--- */
@@ -399,6 +400,37 @@ static void attitudeRateDecoder(setpoint_t *setpoint, uint8_t type, const void *
setpoint->thrust = values->thrust;
}
+/*
+ * Custom Attitude decoder, bypasses the normal attitude control path,
+ * packet contains the desired attitude angle in addition to the thrust value
+ */
+struct attitudePacket_s{
+ float rollAngle; // deg
+ float pitchAngle; // deg
+ float yawAngle; // deg
+ float thrust; // thrust percentage 0 - 60,000
+} __attribute__((packed));
+static void attitudeDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen){
+
+ const struct attitudePacket_s *values = data;
+
+ ASSERT(datalen == sizeof(struct attitudePacket_s));
+
+ setpoint->mode.x = modeDisable;
+ setpoint->mode.y = modeDisable;
+ setpoint->mode.z = modeDisable;
+
+ setpoint->mode.roll = modeAbs;
+ setpoint->mode.pitch = modeAbs;
+ setpoint->mode.yaw = modeAbs;
+
+ setpoint->attitude.roll = values->rollAngle;
+ setpoint->attitude.pitch = values->pitchAngle;
+ setpoint->attitude.yaw = values->yawAngle;
+
+ setpoint->thrust = values->thrust;
+}
+
/* ---===== 3 - packetDecoders array =====--- */
const static packetDecoder_t packetDecoders[] = {
[stopType] = stopDecoder,
@@ -410,6 +442,7 @@ const static packetDecoder_t packetDecoders[] = {
[fullStateType] = fullStateDecoder,
[positionType] = positionDecoder,
[attitudeRateType] = attitudeRateDecoder,
+ [attitudeType] = attitudeDecoder,
};
/* Decoder switch */
diff --git a/groundStation/.gitignore b/groundStation/.gitignore
index 8a84dfc99eef1e265992a59c95b64e44e3b8cbd4..d79f2ff263194e6e7781caf076590450e9638831 100644
--- a/groundStation/.gitignore
+++ b/groundStation/.gitignore
@@ -56,11 +56,4 @@ setparam
gettrackable
settrackable
-#Qt
-gui/MicroCART/Makefile
-gui/MicroCART/MicroCART
-gui/MicroCART/MicroCART.pro*
-gui/MicroCART/moc*
-gui/MicroCART/qrc_resources.cpp
-gui/MicroCART/ui_mainwindow.h
diff --git a/groundStation/adapters/crazyflie/src/cf_adapter.c b/groundStation/adapters/crazyflie/src/cf_adapter.c
index eaf8f428756186c289296f22fbc8192e2def7ddb..84871e5b427c5df325c72d548198dfb3439a1567 100644
--- a/groundStation/adapters/crazyflie/src/cf_adapter.c
+++ b/groundStation/adapters/crazyflie/src/cf_adapter.c
@@ -5,7 +5,7 @@
//socket that the crazyflie groundstation will read from, check the README for information
//on how to setup this path.
-#define DEFAULT_ADAPTER_SOCKET "/home/bitcraze/projects/crazyflie_groundstation.socket"
+#define DEFAULT_ADAPTER_SOCKET "../../../crazyflie_groundstation/crazyflie_groundstation.socket"
//#define DEFAULT_ADAPTER_SOCKET "./crazyflie_groundstation.socket"
#define SOCKET_ENV "ADAPTER_SOCKET"
diff --git a/groundStation/gui/MicroCART/.gitignore b/groundStation/gui/MicroCART/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..d5ce34de5297da99798e7d40d2d85f383424787e
--- /dev/null
+++ b/groundStation/gui/MicroCART/.gitignore
@@ -0,0 +1,54 @@
+# C++ objects and libs
+*.slo
+*.lo
+*.o
+*.a
+*.la
+*.lai
+*.so
+*.so.*
+*.dll
+*.dylib
+
+# Qt-es
+object_script.*.Release
+object_script.*.Debug
+*_plugin_import.cpp
+/.qmake.cache
+/.qmake.stash
+*.pro.user
+*.pro.user.*
+*.qbs.user
+*.qbs.user.*
+*.moc
+moc_*.cpp
+moc_*.h
+qrc_*.cpp
+ui_*.h
+*.qmlc
+*.jsc
+Makefile*
+*build-*
+*.qm
+*.prl
+
+# Qt unit tests
+target_wrapper.*
+
+# QtCreator
+*.autosave
+
+# QtCreator Qml
+*.qmlproject.user
+*.qmlproject.user.*
+
+# QtCreator CMake
+CMakeLists.txt.user*
+
+# QtCreator 4.8< compilation database
+compile_commands.json
+
+# QtCreator local machine specific files for imported projects
+*creator.user*
+
+*_qmlcache.qrc
\ No newline at end of file
diff --git a/groundStation/gui/MicroCART/MicroCART.cflags b/groundStation/gui/MicroCART/MicroCART.cflags
new file mode 100644
index 0000000000000000000000000000000000000000..68d51653007222e0f75ee44e0b2330af6e9b7b2d
--- /dev/null
+++ b/groundStation/gui/MicroCART/MicroCART.cflags
@@ -0,0 +1 @@
+-std=c17
\ No newline at end of file
diff --git a/groundStation/gui/MicroCART/MicroCART.config b/groundStation/gui/MicroCART/MicroCART.config
new file mode 100644
index 0000000000000000000000000000000000000000..e0284f42573a21b9d3f32775068f4dc233bf1c27
--- /dev/null
+++ b/groundStation/gui/MicroCART/MicroCART.config
@@ -0,0 +1,2 @@
+// Add predefined macros for your project here. For example:
+// #define THE_ANSWER 42
diff --git a/groundStation/gui/MicroCART/MicroCART.creator b/groundStation/gui/MicroCART/MicroCART.creator
new file mode 100644
index 0000000000000000000000000000000000000000..e94cbbd3027df32cb00f4fca093d5c72d9696a85
--- /dev/null
+++ b/groundStation/gui/MicroCART/MicroCART.creator
@@ -0,0 +1 @@
+[General]
diff --git a/groundStation/gui/MicroCART/MicroCART.cxxflags b/groundStation/gui/MicroCART/MicroCART.cxxflags
new file mode 100644
index 0000000000000000000000000000000000000000..6435dfce2fbb9c120633cbc667f028badd3e17c2
--- /dev/null
+++ b/groundStation/gui/MicroCART/MicroCART.cxxflags
@@ -0,0 +1 @@
+-std=c++17
\ No newline at end of file
diff --git a/groundStation/gui/MicroCART/MicroCART.files b/groundStation/gui/MicroCART/MicroCART.files
new file mode 100644
index 0000000000000000000000000000000000000000..0ce8141d43852df971238ce1de596c4f643e6bfb
--- /dev/null
+++ b/groundStation/gui/MicroCART/MicroCART.files
@@ -0,0 +1,40 @@
+MicroCART
+MicroCART.pro
+README.md
+controlworker.cpp
+controlworker.h
+crazyflieworker.cpp
+crazyflieworker.h
+gamepadmonitor.cpp
+gamepadmonitor.h
+gridlines.gif
+main.cpp
+mainwindow.cpp
+mainwindow.h
+mainwindow.ui
+moc_controlworker.cpp
+moc_crazyflieworker.cpp
+moc_mainwindow.cpp
+moc_predefs.h
+moc_qFlightInstruments.cpp
+moc_qattitudeindicator.cpp
+moc_qcustomplot.cpp
+moc_quaditem.cpp
+moc_slotprocess.cpp
+moc_trackerworker.cpp
+qFlightInstruments.cpp
+qFlightInstruments.h
+qcustomplot.cpp
+qcustomplot.h
+qrc_resources.cpp
+quad.png
+quaditem.cpp
+quaditem.h
+resources.qrc
+setpoint.cpp
+setpoint.h
+slotprocess.cpp
+slotprocess.h
+trackerworker.cpp
+trackerworker.h
+ui_mainwindow.h
diff --git a/groundStation/gui/MicroCART/MicroCART.includes b/groundStation/gui/MicroCART/MicroCART.includes
new file mode 100644
index 0000000000000000000000000000000000000000..9c558e357c41674e39880abb6c3209e539de42e2
--- /dev/null
+++ b/groundStation/gui/MicroCART/MicroCART.includes
@@ -0,0 +1 @@
+.
diff --git a/groundStation/gui/MicroCART/MicroCART.pro b/groundStation/gui/MicroCART/MicroCART.pro
index 3548b01aab3ffb98b73ad450bf45eb0744e4683c..d38edbeee72bb9f4e95a72c0dd93931435e438cb 100644
--- a/groundStation/gui/MicroCART/MicroCART.pro
+++ b/groundStation/gui/MicroCART/MicroCART.pro
@@ -4,7 +4,7 @@
#
#-------------------------------------------------
-QT += core gui
+QT += core gui gamepad
greaterThan(QT_MAJOR_VERSION, 4): QT += widgets printsupport
@@ -23,7 +23,8 @@ SOURCES += main.cpp\
qFlightInstruments.cpp\
qcustomplot.cpp\
crazyflieworker.cpp\
- setpoint.cpp
+ setpoint.cpp\
+ gamepadmonitor.cpp
HEADERS += mainwindow.h \
trackerworker.h \
@@ -34,7 +35,8 @@ HEADERS += mainwindow.h \
qFlightInstruments.h\
qcustomplot.h\
crazyflieworker.h\
- setpoint.h
+ setpoint.h\
+ gamepadmonitor.h
FORMS += mainwindow.ui
diff --git a/groundStation/gui/MicroCART/crazyflieworker.cpp b/groundStation/gui/MicroCART/crazyflieworker.cpp
index d51f2ce766ac4ce7c70c452076b42ed7adee9dae..8894c2c63b265fc749ec2f122a7d885abef96f52 100644
--- a/groundStation/gui/MicroCART/crazyflieworker.cpp
+++ b/groundStation/gui/MicroCART/crazyflieworker.cpp
@@ -1,7 +1,9 @@
#include "crazyflieworker.h"
#include <iostream>
+#include <unistd.h>
+#include <sys/stat.h>
-CrazyflieWorker::CrazyflieWorker(QObject *parent) : QObject(parent)
+CrazyflieWorker::CrazyflieWorker(QObject *parent) : QObject(parent), conn(NULL)
{
}
@@ -22,8 +24,16 @@ void CrazyflieWorker::connectBackend()
conn = ucart_backendConnect();
emit (connected());
} else {
- qInfo() << "Attempted to connect trackerworker when already connected!";
+ qInfo() << "Attempted to connect crazyflieworker when already connected!";
}
+ loadParamIds();
+ loadLogIds();
+ QStringList paramGroupList;
+ for(int i = 0; i < paramGroups.size(); i++) {
+ QString qgName(paramGroups.at(i).groupName.c_str());
+ paramGroupList.append(qgName);
+ }
+ emit(gotParamGroups(paramGroupList));
}
/**
@@ -49,12 +59,12 @@ void CrazyflieWorker::setCurrAttSetpoint(float roll, float pitch, float yaw, flo
currAttitudeSetpoint.throttle = throttle;
}
-void CrazyflieWorker::setCurrAttRateSetpoint(float rollRate, float pitchRate, float yawRate, float throttleRate)
+void CrazyflieWorker::setCurrAttRateSetpoint(float rollRate, float pitchRate, float yawRate, float throttle)
{
currAttitudeRateSetpoint.roll = rollRate;
currAttitudeRateSetpoint.pitch = pitchRate;
currAttitudeRateSetpoint.yaw = yawRate;
- currAttitudeRateSetpoint.throttle = throttleRate;
+ currAttitudeRateSetpoint.throttle = throttle;
}
@@ -62,7 +72,7 @@ void CrazyflieWorker::sendAttRateSetpoint()
{
struct frontend_override_data data;
data.enable = 2; //attitude rate
- data.time = 0.2; //hold setpoint for 0.2 seconds TODO
+ data.time = 0.2; //hold setpoint for 0.2 seconds TODO check if this amount of time is good?
data.throttle = currAttitudeRateSetpoint.throttle;
data.roll = currAttitudeRateSetpoint.roll;
data.pitch = currAttitudeRateSetpoint.pitch;
@@ -88,18 +98,175 @@ void CrazyflieWorker::sendAttSetpoint()
}
void CrazyflieWorker::getParamValue(QString paramName){
- struct frontend_param_data data = paramNameLookup(paramName);
+ struct toc_info info = paramNameLookup(paramName);
+
+ struct frontend_param_data data;
+ data.block = 0;
+ data.param = info.id;
+ data.value = info.value;
if(frontend_getparam(conn, &data) == 0){
emit(gotParamValue(paramName, data.value));
}
}
-struct frontend_param_data CrazyflieWorker::paramNameLookup(QString paramName){
- //TODO
- //if param is in cache, use that else lookup from file
- struct frontend_param_data paramData;
+void CrazyflieWorker::setParamValue(QString paramName, double value) {
+ struct toc_info info = paramNameLookup(paramName);
+
+ struct frontend_param_data data;
+ data.block = 0;
+ data.param = info.id;
+ data.value = (float) value;
+
+ if(frontend_setparam(conn, &data) != 0){
+ //emit(gotParamValue(paramName, data.value));
+ qInfo() << "setParamError";
+ }
+}
+
+struct toc_info CrazyflieWorker::paramNameLookup(QString paramName){
+ struct toc_info paramData = paramIds.value(paramName);
return paramData;
}
+std::string CrazyflieWorker::getLogFile(int type) {
+ struct frontend_getlogfile_data logfile;
+ char fname[256];
+ logfile.command = type;
+ logfile.name = fname;
+ if(frontend_getlogfile(conn, &logfile)) {
+ return "";
+ }
+ std::string result(logfile.name, strlen(logfile.name));
+ return result;
+}
+
+void CrazyflieWorker::loadParamIds() {
+ std::string filename = getLogFile(1);
+ std::ifstream paramTOCFile;
+ paramTOCFile.open(filename.c_str());
+ std::string line;
+ //header and top line
+ std::getline(paramTOCFile, line);
+ std::getline(paramTOCFile, line);
+ //actual values;
+ while(std::getline(paramTOCFile, line)) {
+ struct toc_info values;
+ std::stringstream ss(line);
+ std::string temp_str;
+ for(int i = 0; i < 4; i++) {
+ std::getline(ss, temp_str, '\t');
+ if(i == 0) {
+ values.id = (int16_t) stoi(temp_str);
+ }
+ else if(i == 1) {
+ values.dataType = (int8_t) stoi(temp_str);
+ }
+ else if(i == 2) {
+ values.strGroup = temp_str;
+ }
+ else if(i == 3) {
+ values.strName = temp_str;
+ }
+ }
+ values.value = 0;
+
+ int gFound = findParamGroup(values.strGroup);
+ if(gFound == -1) {
+ struct toc_group groupx;
+ groupx.groupName = values.strGroup;
+ groupx.entryNames.push_back(values.strName);
+ paramGroups.push_back(groupx);
+ }
+ else {
+ paramGroups.at(gFound).entryNames.push_back(values.strName);
+ }
+
+ std::string identifier = values.strGroup + "." + values.strName;
+ QString Qidentifier(identifier.c_str());
+ paramIds.insert(Qidentifier, values);
+ }
+ paramTOCFile.close();
+}
+void CrazyflieWorker::loadLogIds() {
+ std::string filename = getLogFile(2);
+ std::ifstream logTOCFile;
+ logTOCFile.open(filename.c_str());
+ std::string line;
+ //top line
+ std::getline(logTOCFile, line);
+ std::getline(logTOCFile, line);
+ //actual values;
+ while(std::getline(logTOCFile, line)) {
+ struct toc_info values;
+ std::stringstream ss(line);
+ std::string temp_str;
+ for(int i = 0; i < 4; i++) {
+ std::getline(ss, temp_str, '\t');
+ if(i == 0) {
+ values.id = (int16_t) stoi(temp_str);
+ }
+ else if(i == 1) {
+ values.dataType = (int8_t) stoi(temp_str);
+ }
+ else if(i == 2) {
+ values.strGroup = temp_str;
+ }
+ else if(i == 3) {
+ values.strName = temp_str;
+ }
+ }
+ values.value = 0;
+
+ int gFound = findLogGroup(values.strGroup);
+ if(gFound == -1) {
+ struct toc_group groupx;
+ groupx.groupName = values.strGroup;
+ groupx.entryNames.push_back(values.strName);
+ logGroups.push_back(groupx);
+ }
+ else {
+ logGroups.at(gFound).entryNames.push_back(values.strName);;
+ }
+
+ std::string identifier = values.strGroup + "." + values.strName;
+ QString Qidentifier(identifier.c_str());
+ logIds.insert(Qidentifier, values);
+ }
+ logTOCFile.close();
+}
+
+int CrazyflieWorker::findParamGroup(std::string gName) {
+ int result = -1;
+ for(int i = 0; i < paramGroups.size(); i++) {
+ if(!paramGroups.at(i).groupName.compare(gName)) {
+ result = i;
+ break;
+ }
+ }
+ return result;
+}
+
+int CrazyflieWorker::findLogGroup(std::string gName) {
+ int result = -1;
+ for(int i = 0; i < logGroups.size(); i++) {
+ if(!logGroups.at(i).groupName.compare(gName)) {
+ result = i;
+ break;
+ }
+ }
+ return result;
+}
+
+void CrazyflieWorker::getGroupEntries(int box, QString gName) {
+ std::string name = gName.toStdString();
+ int index = findParamGroup(name);
+ std::vector<std::string> entries = paramGroups.at(index).entryNames;
+ QStringList entryList;
+ for(int i = 0; i < entries.size(); i++) {
+ QString cur(entries.at(i).c_str());
+ entryList.append(cur);
+ }
+ emit(gotGroupEntries(box, entryList));
+}
diff --git a/groundStation/gui/MicroCART/crazyflieworker.h b/groundStation/gui/MicroCART/crazyflieworker.h
index f22f24135ce5ae1dc1f93939c83ba8adde3412a2..104d37b86b3659fecdf0bcd248a438ade64060a8 100644
--- a/groundStation/gui/MicroCART/crazyflieworker.h
+++ b/groundStation/gui/MicroCART/crazyflieworker.h
@@ -2,12 +2,29 @@
#define CRAZYFLIEWORKER_H
#include <QObject>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <QStringList>
#include "frontend_common.h"
#include "frontend_param.h"
#include "frontend_override.h"
+#include "frontend_logfile.h"
#include "setpoint.h"
#include <QDebug>
+struct toc_info {
+ int16_t id;
+ int8_t dataType;
+ std::string strGroup;
+ std::string strName;
+ float value;
+};
+
+struct toc_group {
+ std::string groupName;
+ std::vector<std::string> entryNames;
+};
/**
* @brief The crazyflieworker class
@@ -24,6 +41,8 @@ public:
signals:
void gotParamValue(QString paramName, float value);
+ void gotParamGroups(QStringList paramGroupList);
+ void gotGroupEntries(int box, QStringList groupEntries);
void connected();
void disconnected();
@@ -36,6 +55,8 @@ public slots:
void getParamValue(QString paramName);
void setCurrAttSetpoint(float roll, float pitch, float yaw, float throttle);
void setCurrAttRateSetpoint(float rollRate, float pitchRate, float yawRate, float throttleRate);
+ void setParamValue(QString paramName, double value);
+ void getGroupEntries(int box, QString gName);
private:
struct backend_conn * conn;
@@ -43,9 +64,16 @@ private:
Setpoint currAttitudeRateSetpoint;
void sendAttitudeSetpoint();
void sendAttitudeRateSetpoint();
- struct frontend_param_data paramNameLookup(QString paramName);
-
-
+ std::string getLogFile(int type);
+ void loadParamIds();
+ void loadLogIds();
+ int findParamGroup(std::string gName);
+ int findLogGroup(std::string gName);
+ struct toc_info paramNameLookup(QString paramName);
+ QMap<QString, struct toc_info> paramIds;
+ QMap<QString, struct toc_info> logIds;
+ std::vector<struct toc_group> paramGroups;
+ std::vector<struct toc_group> logGroups;
};
#endif // CRAZYFLIEWORKER_H
diff --git a/groundStation/gui/MicroCART/gamepadmonitor.cpp b/groundStation/gui/MicroCART/gamepadmonitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..09f936fc56062f8b9f92e4c24f58974d87256312
--- /dev/null
+++ b/groundStation/gui/MicroCART/gamepadmonitor.cpp
@@ -0,0 +1,81 @@
+#include "gamepadmonitor.h"
+#include <QDebug>
+
+GamepadMonitor::GamepadMonitor(QObject *parent) : QObject(parent)
+{
+ m_gamepadManager = QGamepadManager::instance();
+
+ rollScale = 20; //-20 to 20 deg/s
+ pitchScale = 20;
+ yawScale = 30;
+ thrustScale = 30000; //0 to 60,000 thrust
+
+ connect(m_gamepadManager, SIGNAL(gamepadConnected(int)), this, SLOT(gamepadConnectedHandler(int)));
+ connect(m_gamepadManager, SIGNAL(gamepadDisconnected(int)), this, SLOT(gamepadDisconnectedHandler(int)));
+
+ auto gamepads = m_gamepadManager->connectedGamepads();
+ if (gamepads.isEmpty()) {
+ // Did not find any connected gamepads creating default gamepad;
+ m_gamepad = new QGamepad();
+ return;
+ }
+
+ m_gamepad = new QGamepad(*gamepads.begin(), this);
+ qInfo() << "gamepad is connected, deviceId = " << m_gamepad->deviceId();
+}
+
+void GamepadMonitor::gamepadConnectedHandler(int deviceId){
+ emit(gamepadConnected());
+ qInfo() << "gamepad is connected, deviceId = " << deviceId;
+ m_gamepad = new QGamepad(deviceId, this);
+}
+
+void GamepadMonitor::gamepadDisconnectedHandler(int deviceId){
+ emit(gamepadDisconnected());
+ qInfo() << "gamepad disconnected";
+}
+
+bool GamepadMonitor::isGamepadConnected(){
+ return m_gamepad->isConnected();
+}
+
+void GamepadMonitor::resetConfig(){
+ qInfo() << "gamepad config reset";
+ m_gamepadManager->resetConfiguration(m_gamepad->deviceId());
+}
+
+void GamepadMonitor::configureAxis(QString axisName){
+ if(axisName == "roll"){
+ m_gamepadManager->configureAxis(m_gamepad->deviceId(), QGamepadManager::AxisRightX);
+ qInfo() << "roll configured";
+ }else if(axisName == "yaw"){
+ m_gamepadManager->configureAxis(m_gamepad->deviceId(), QGamepadManager::AxisLeftX);
+ qInfo() << "yaw configured";
+ }else if(axisName == "pitch"){
+ m_gamepadManager->configureAxis(m_gamepad->deviceId(), QGamepadManager::AxisRightY);
+ qInfo() << "pitch configured";
+ }else if(axisName == "thrust"){
+ m_gamepadManager->configureAxis(m_gamepad->deviceId(), QGamepadManager::AxisLeftY);
+ qInfo() << "thrust configured";
+ }
+}
+
+float GamepadMonitor::getRoll(){
+ return (float) m_gamepad->axisRightX() * rollScale;
+}
+
+float GamepadMonitor::getYaw(){
+ return (float) m_gamepad->axisLeftX() * yawScale;
+}
+
+float GamepadMonitor::getPitch(){
+ return (float) m_gamepad->axisRightY() * pitchScale;
+}
+
+float GamepadMonitor::getThrust(){
+ return (float) (m_gamepad->axisLeftY() + 1) * thrustScale;
+}
+
+GamepadMonitor::~GamepadMonitor(){
+
+}
diff --git a/groundStation/gui/MicroCART/gamepadmonitor.h b/groundStation/gui/MicroCART/gamepadmonitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..5b3ef05cd76daef5ce874c70d814876876aaa3fc
--- /dev/null
+++ b/groundStation/gui/MicroCART/gamepadmonitor.h
@@ -0,0 +1,42 @@
+#ifndef GAMEPADMONITOR_H
+#define GAMEPADMONITOR_H
+
+#include <QObject>
+#include <QtGamepad/QGamepad>
+
+
+class GamepadMonitor : public QObject
+{
+ Q_OBJECT
+public:
+ explicit GamepadMonitor(QObject *parent = nullptr);
+ ~GamepadMonitor();
+
+ bool isGamepadConnected();
+
+ float getRoll();
+ float getYaw();
+ float getPitch();
+ float getThrust();
+ void configureAxis(QString axisName);
+ void resetConfig();
+
+
+private:
+ QGamepad *m_gamepad;
+ QGamepadManager *m_gamepadManager;
+
+ float rollScale;
+ float pitchScale;
+ float yawScale;
+ float thrustScale;
+
+signals:
+ void gamepadConnected();
+ void gamepadDisconnected();
+public slots:
+ void gamepadConnectedHandler(int deviceId);
+ void gamepadDisconnectedHandler(int deviceId);
+};
+
+#endif // GAMEPADMONITOR_H
diff --git a/groundStation/gui/MicroCART/mainwindow.cpp b/groundStation/gui/MicroCART/mainwindow.cpp
index 26cf536a50696027659177431a0b8d478a214089..26cd4162ec2ee856c5a9d92e39b0c4cd4db7268c 100644
--- a/groundStation/gui/MicroCART/mainwindow.cpp
+++ b/groundStation/gui/MicroCART/mainwindow.cpp
@@ -10,6 +10,7 @@
#include <QProcessEnvironment>
#include <QPixmap>
#include <QProcess>
+#include <QDebug>
#include "trackerworker.h"
#include "controlworker.h"
@@ -34,8 +35,10 @@ MainWindow::MainWindow(QWidget *parent) :
crazyflieTimer(new QTimer(this)),
backendProcess(new QProcess(this)),
matlabProcess(new QProcess(this)),
+ pollGamepadTimer(new QTimer(this)),
connectedWorkers(0)
{
+
ui->setupUi(this);
QGraphicsScene *posScene = new QGraphicsScene(this);
@@ -78,21 +81,6 @@ MainWindow::MainWindow(QWidget *parent) :
ControlWorker * controlWorker = new ControlWorker();
controlWorker->moveToThread(cwThread);
- /*Create worker for the crazyflie and moves it to new thread*/
- QThread * crazyflieThread = new QThread(this);
- CrazyflieWorker * crazyflieWorker = new CrazyflieWorker();
- controlWorker->moveToThread(crazyflieThread);
-
- /*sgnals from crazyflie worker*/
-
- /*signals to crazyflie worker*/
- connect(this, SIGNAL (rateSetpointSignal(float, float, float, float)), crazyflieWorker, SLOT (setCurrAttSetpoint(float, float, float, float)));
- connect(this, SIGNAL (angleSetpointSignal(float, float, float, float)), crazyflieWorker, SLOT (setCurrAttRateSetpoint(float, float, float, float)));
- //dont knwo if i need next line
- crazyflieTimer->setSingleShot(true);
- connect(crazyflieTimer, SIGNAL(timeout()), this, SLOT (on_stopSetpointButton_clicked()));
-
-
/* Connect signals from control worker */
connect(controlWorker, SIGNAL (gotNodes(QStringList)), this, SLOT (newNodes(QStringList)));
connect(controlWorker, SIGNAL (gotParams(QStringList)), this, SLOT (newParams(QStringList)));
@@ -109,6 +97,29 @@ MainWindow::MainWindow(QWidget *parent) :
connect(this, SIGNAL (setParamValue(QString, QString, float)), controlWorker, SLOT (setParamValue(QString, QString, float)));
connect(this, SIGNAL (getNodeOutput(QString)), controlWorker, SLOT (getNodeOutput(QString)));
+ /* Create another work for the crazyflie communication */
+ QThread * cfThread = new QThread(this);
+ crazyflieWorker = new CrazyflieWorker();
+ crazyflieWorker->moveToThread(cfThread);
+
+ /* connect signals for crazyflie worker */
+ connect(this, SIGNAL (rateSetpointSignal(float, float, float, float)), crazyflieWorker, SLOT (setCurrAttSetpoint(float, float, float, float)));
+ connect(this, SIGNAL (angleSetpointSignal(float, float, float, float)), crazyflieWorker, SLOT (setCurrAttRateSetpoint(float, float, float, float)));
+ crazyflieTimer->setSingleShot(true);
+ connect(crazyflieTimer, SIGNAL(timeout()), this, SLOT (on_stopSetpointButton_clicked()));
+ connect(crazyflieWorker, SIGNAL (gotParamGroups(QStringList)), this, SLOT (setParamGroups(QStringList)));
+ connect(ui->paramGroupComboBox, SIGNAL (currentIndexChanged(int)), this, SLOT (getGroupBoxChange(int)));
+ connect(ui->paramGroupComboBox_2, SIGNAL (currentIndexChanged(int)), this, SLOT (getGroupBox2Change(int)));
+ connect(ui->paramEntriesComboBox, SIGNAL (currentIndexChanged(int)), this, SLOT (getEntryBoxChange(int)));
+ connect(ui->paramEntriesComboBox_2, SIGNAL (currentIndexChanged(int)), this, SLOT (getEntryBoxChange2(int)));
+ connect(ui->pb_getParam, SIGNAL (clicked()), this, SLOT (on_pb_getParam_click()));
+ connect(ui->pb_setParam, SIGNAL (clicked()), this, SLOT (on_pb_setParam_click()));
+ connect(crazyflieWorker, SIGNAL (gotGroupEntries(int, QStringList)), this, SLOT (newGroupEntries(int, QStringList)));
+ connect(this, SIGNAL (getGroupEntries(int, QString)), crazyflieWorker, SLOT (getGroupEntries(int, QString)));
+ connect(this, SIGNAL (getParamValue(QString)), crazyflieWorker, SLOT (getParamValue(QString)));
+ connect(this, SIGNAL (setParamValue(QString, double)), crazyflieWorker, SLOT (setParamValue(QString, double)));
+ connect(crazyflieWorker, SIGNAL (gotParamValue(QString, float)), this, SLOT (displayParamValue(QString, float)));
+
/* Connect and disconnect from backend when signals emitted */
connect(workerStartTimer, SIGNAL (timeout()), trackerWorker, SLOT (connectBackend()));
connect(workerStartTimer, SIGNAL (timeout()), controlWorker, SLOT (connectBackend()));
@@ -149,16 +160,38 @@ MainWindow::MainWindow(QWidget *parent) :
trackerTimer->start(100);
workerThread->start();
cwThread->start();
- crazyflieThread->start();
+ cfThread->start();
matlabProcess->start();
/* Connect the setpointlist to the model */
// ui->setpointList->setModel(setpointList);
+ ui->pb_getParam->setEnabled(false);
+ ui->pb_setParam->setEnabled(false);
+ ui->setValueSpin->setMaximum(10000.00);
+ ui->setValueSpin->setMinimum(-10000.00);
+ ui->setValueSpin->setSingleStep(0.01);
+ ui->setValueSpin->setValue(0);
+
/* Connect various things that can result in sending setpoints */
// connect(ui->pbSendSetpoint, SIGNAL (clicked()), this, SLOT (sendSetpoints()));
// connect(ui->setpointList, SIGNAL (doubleClicked(QModelIndex)), this, SLOT (sendSelectedSetpoint()));
+ /* start gamepad monitor */
+ gamepadMonitor = new GamepadMonitor();
+ if(gamepadMonitor->isGamepadConnected()){
+ onGamepadConnect();
+ }else{
+ onGamepadDisconnect();
+ }
+
+ connect(gamepadMonitor, SIGNAL(gamepadDisconnected()), this, SLOT(onGamepadDisconnect()));
+ connect(gamepadMonitor, SIGNAL(gamepadConnected()), this, SLOT(onGamepadConnect()));
+ connect(ui->pb_configRoll, SIGNAL(clicked()), this, SLOT(on_pb_configRoll_clicked()));
+ connect(pollGamepadTimer, SIGNAL(timeout()), this, SLOT(updateGamepad()));
+ pollGamepadTimer->start(50);
+
+
/* Populate scripts list */
QDir scriptsDir("scripts/");
QStringList scripts = scriptsDir.entryList();
@@ -168,7 +201,6 @@ MainWindow::MainWindow(QWidget *parent) :
scriptProcess->setProgram(scriptsDir.filePath(scripts[i]));
connect(action, SIGNAL (triggered()), scriptProcess, SLOT (startProcess()));
}
-
}
MainWindow::~MainWindow()
@@ -299,6 +331,98 @@ void MainWindow::newNodes(QStringList blocks)
// this->ui->noGraphWarningLine->setVisible(false);
}
+void MainWindow::setParamGroups(QStringList groupNames) {
+ ui->paramGroupComboBox->addItem("");
+ ui->paramGroupComboBox->addItems(groupNames);
+ ui->paramGroupComboBox_2->addItem("");
+ ui->paramGroupComboBox_2->addItems(groupNames);
+}
+
+void MainWindow::getGroupBoxChange(int index) {
+ // for(int i = 0; i < ui->paramGroupComboBox->count(); i++) {
+ // ui->paramEntriesComboBox->removeItem(i);
+ // }
+ ui->paramEntriesComboBox->clear();
+ QString qStrGroup = ui->paramGroupComboBox->itemText(index);
+ std::string check = qStrGroup.toStdString();
+ ui->valueVallabel->clear();
+ if(!check.compare("")) {
+ ui->pb_getParam->setEnabled(false);
+ }
+ else {
+ emit(getGroupEntries(1, qStrGroup));
+ }
+}
+
+void MainWindow::getGroupBox2Change(int index) {
+ // for(int i = 0; i < ui->paramGroupComboBox->count(); i++) {
+ // ui->paramEntriesComboBox->removeItem(i);
+ // }
+ ui->paramEntriesComboBox_2->clear();
+ QString qStrGroup = ui->paramGroupComboBox_2->itemText(index);
+ std::string check = qStrGroup.toStdString();
+ if(!check.compare("")) {
+ ui->pb_setParam->setEnabled(false);
+ }
+ else {
+ emit(getGroupEntries(2, qStrGroup));
+ }
+}
+
+void MainWindow::getEntryBoxChange(int index) {
+ QString qStrGroup = ui->paramEntriesComboBox->itemText(index);
+ std::string check = qStrGroup.toStdString();
+ ui->valueVallabel->clear();
+ if(!check.compare("")) {
+ ui->pb_getParam->setEnabled(false);
+ }
+ else {
+ ui->pb_getParam->setEnabled(true);
+ }
+}
+
+void MainWindow::getEntryBoxChange2(int index) {
+ QString qStrGroup = ui->paramEntriesComboBox_2->itemText(index);
+ std::string check = qStrGroup.toStdString();
+ if(!check.compare("")) {
+ ui->pb_setParam->setEnabled(false);
+ }
+ else {
+ ui->pb_setParam->setEnabled(true);
+ }
+}
+
+void MainWindow::newGroupEntries(int box, QStringList entries) {
+ if(box == 1) {
+ ui->paramEntriesComboBox->addItem("");
+ ui->paramEntriesComboBox->addItems(entries);
+ }
+ else {
+ ui->paramEntriesComboBox_2->addItem("");
+ ui->paramEntriesComboBox_2->addItems(entries);
+ }
+}
+
+void MainWindow::on_pb_getParam_click() {
+ ui->valueVallabel->clear();
+ QString group = ui->paramGroupComboBox->currentText();
+ QString entry = ui->paramEntriesComboBox->currentText();
+ QString key = group + "." + entry;
+ emit(getParamValue(key));
+}
+
+void MainWindow::on_pb_setParam_click() {
+ QString group = ui->paramGroupComboBox_2->currentText();
+ QString entry = ui->paramEntriesComboBox_2->currentText();
+ QString key = group + "." + entry;
+ double value = ui->setValueSpin->value();
+ emit(setParamValue(key, value));
+}
+
+void MainWindow::displayParamValue(QString key, float value) {
+ ui->valueVallabel->setNum(value);
+}
+
void MainWindow::newConstantBlocks(QStringList blocks)
{
// ui->xSetpointSelect->clear();
@@ -629,20 +753,15 @@ void MainWindow::on_applySetpointButton_clicked()
sp_roll = ui->rollSetpointBox->text().toFloat();
sp_pitch = ui->pitchSetpointBox->text().toFloat();
sp_yaw = ui->yawSetpointBox->text().toFloat();
- sp_thrust = ui->thrustSetpointBox->text().toFloat();
+ sp_thrust = ui->tActual -> value();
crazyflieTimer -> start(10000);
- //testing print statements
- std::cout << "roll: " << sp_roll << std::endl;
- std::cout << "pitch: " << sp_pitch << std::endl;
- std::cout << "yaw: " << sp_yaw << std::endl;
-
- if(ui->angleRadioButton->isCheckable()) {
+ if(ui->angleRadioButton->isChecked()) {
//send as angle setpoint
emit(angleSetpointSignal(sp_roll, sp_pitch, sp_yaw, sp_thrust));
}
- else if (ui->rateRadioButton->isCheckable()) {
+ else if (ui->rateRadioButton->isChecked()) {
//send as rate setpoint
emit(rateSetpointSignal(sp_roll, sp_pitch, sp_yaw, sp_thrust));
}
@@ -656,17 +775,126 @@ void MainWindow::on_stopSetpointButton_clicked()
sp_pitch = 0.0;
sp_yaw = 0.0;
sp_thrust = 0.0;
- //debug prints
- std::cout << "roll: " << sp_roll << std::endl;
- std::cout << "pitch: " << sp_pitch << std::endl;
- std::cout << "yaw: " << sp_yaw << std::endl;
- if(ui->angleRadioButton->isCheckable()) {
+ if(ui->angleRadioButton->isChecked()) {
//send as angle setpoint
emit(angleSetpointSignal(sp_roll, sp_pitch, sp_yaw, sp_thrust));
}
- else if (ui->rateRadioButton->isCheckable()) {
+ else if (ui->rateRadioButton->isChecked()) {
//send as rate setpoint
emit(rateSetpointSignal(sp_roll, sp_pitch, sp_yaw, sp_thrust));
}
}
+void MainWindow::on_tActual_sliderMoved(int position)
+{
+ QToolTip::showText(QCursor::pos(), QString::number(position), nullptr);
+
+}
+
+
+/******** Gamepad handlers ********/
+
+void MainWindow::onGamepadDisconnect(){
+ crazyflieWorker->setCurrAttRateSetpoint(0,0,0,0);
+ ui->pitchSetpointBox->setText("0");
+ ui->rollSetpointBox->setText("0");
+ ui->yawSetpointBox->setText("0");
+ ui->tActual->setValue(0);
+
+ ui->noGamepadWarning->show();
+ ui->rbGamepadControl->setEnabled(false);
+ ui->pb_configPitch->setEnabled(false);
+ ui->pb_configRoll->setEnabled(false);
+ ui->pb_configYaw->setEnabled(false);
+ ui->pb_configThrust->setEnabled(false);
+ ui->pb_resetConfig->setEnabled(false);
+ ui->rbManualSetpoint->setChecked(true);
+}
+
+void MainWindow::onGamepadConnect(){
+ ui->noGamepadWarning->hide();
+ ui->rbGamepadControl->setEnabled(true);
+ ui->pb_configPitch->setEnabled(true);
+ ui->pb_configRoll->setEnabled(true);
+ ui->pb_configYaw->setEnabled(true);
+ ui->pb_configThrust->setEnabled(true);
+ ui->pb_resetConfig->setEnabled(true);
+}
+
+void MainWindow::updateGamepad(){
+ if(ui->tabWidget->currentWidget() == ui->Gamepad){
+ //on gamepad tab
+ ui->rollVizBar->setValue((int) gamepadMonitor->getRoll());
+ ui->yawVizBar->setValue((int) gamepadMonitor->getYaw());
+ ui->pitchVizBar->setValue((int) gamepadMonitor->getPitch());
+ ui->thrustVizBar->setValue((int) gamepadMonitor->getThrust());
+ }else if(ui->tabWidget->currentWidget() == ui->navigation){
+ if(ui->rbGamepadControl->isChecked()){
+ float roll = gamepadMonitor->getRoll();
+ float pitch = gamepadMonitor->getPitch();
+ float yaw = gamepadMonitor->getYaw();
+ float thrust = gamepadMonitor->getThrust();
+
+ crazyflieWorker->setCurrAttRateSetpoint(roll, pitch, yaw, thrust);
+ ui->rollSetpointBox->setText(QString::number(roll));
+ ui->pitchSetpointBox->setText(QString::number(pitch));
+ ui->yawSetpointBox->setText(QString::number(yaw));
+ ui->tActual->setValue((int) thrust);
+ }
+ }
+}
+
+void MainWindow::on_pb_resetConfig_clicked()
+{
+ gamepadMonitor->resetConfig();
+}
+
+void MainWindow::on_pb_configRoll_clicked()
+{
+ gamepadMonitor->configureAxis("roll");
+}
+
+void MainWindow::on_pb_configYaw_clicked()
+{
+ gamepadMonitor->configureAxis("yaw");
+}
+
+void MainWindow::on_pb_configPitch_clicked()
+{
+ gamepadMonitor->configureAxis("pitch");
+}
+
+void MainWindow::on_pb_configThrust_clicked()
+{
+ gamepadMonitor->configureAxis("thrust");
+}
+
+
+
+void MainWindow::on_rbManualSetpoint_toggled(bool checked)
+{
+ if(checked) {
+ crazyflieWorker->setCurrAttRateSetpoint(0,0,0,0);
+ ui->pitchSetpointBox->setText("0");
+ ui->rollSetpointBox->setText("0");
+ ui->yawSetpointBox->setText("0");
+ ui->tActual->setValue(0);
+ ui->rollSetpointBox-> setEnabled(true);
+ ui->pitchSetpointBox-> setEnabled(true);
+ ui->yawSetpointBox-> setEnabled(true);
+ ui->tActual-> setEnabled(true);
+ ui->angleRadioButton-> setEnabled(true);
+ ui->rateRadioButton-> setEnabled(true);
+ }
+}
+
+void MainWindow::on_rbGamepadControl_toggled(bool checked)
+{
+ ui->rollSetpointBox-> setEnabled(false);
+ ui->pitchSetpointBox-> setEnabled(false);
+ ui->yawSetpointBox-> setEnabled(false);
+ ui->tActual-> setEnabled(false);
+ ui->angleRadioButton-> setEnabled(false);
+ ui->rateRadioButton-> setEnabled(false);
+ ui->rateRadioButton-> setChecked(true);
+}
diff --git a/groundStation/gui/MicroCART/mainwindow.h b/groundStation/gui/MicroCART/mainwindow.h
index 194eaa03295e0ef8eac41d6165a7072f494a3379..725a74d3cc53e878ee86cbf8ef8bd963dcbb620e 100644
--- a/groundStation/gui/MicroCART/mainwindow.h
+++ b/groundStation/gui/MicroCART/mainwindow.h
@@ -6,6 +6,9 @@
#include <QStandardItemModel>
#include <QGraphicsScene>
#include "quaditem.h"
+#include "gamepadmonitor.h"
+#include "crazyflieworker.h"
+
namespace Ui {
class MainWindow;
@@ -23,11 +26,14 @@ signals:
void connectWorkers();
void disconnectWorkers();
void getParamValue(QString node, QString param);
+ void getParamValue(QString key);
void getNodeOutput(QString node);
void setParamValue(QString node, QString param, float value);
+ void setParamValue(QString key, double value);
void getPosAttFromBackend();
void rateSetpointSignal(float roll, float pitch, float yaw, float throttle);
void angleSetpointSignal(float roll, float pitch, float yaw, float throttle);
+ void getGroupEntries(int box, QString gName);
private slots:
void on_pbStart_clicked();
@@ -47,6 +53,11 @@ private slots:
void newParamValue(QString node, QString param, float val);
void newNodeOutput(QString node, float output);
void newConstantBlocks(QStringList blocks);
+ void newGroupEntries(int box, QStringList entries);
+
+ void on_pb_getParam_click();
+
+ void on_pb_setParam_click();
void on_paramSelect_currentIndexChanged(const QString &arg1);
@@ -96,6 +107,31 @@ private slots:
void on_applySetpointButton_clicked();
void on_stopSetpointButton_clicked();
+ void setParamGroups(QStringList groupNames);
+ void getGroupBoxChange(int index);
+ void getGroupBox2Change(int index);
+ void displayParamValue(QString param, float value);
+ void getEntryBoxChange(int index);
+ void getEntryBoxChange2(int index);
+
+ void onGamepadDisconnect();
+ void onGamepadConnect();
+ void on_pb_configRoll_clicked();
+ void updateGamepad();
+
+ void on_pb_resetConfig_clicked();
+
+ void on_pb_configYaw_clicked();
+
+ void on_pb_configThrust_clicked();
+
+ void on_pb_configPitch_clicked();
+
+ void on_tActual_sliderMoved(int position);
+
+ void on_rbGamepadControl_toggled(bool checked);
+
+ void on_rbManualSetpoint_toggled(bool checked);
private:
Ui::MainWindow *ui;
@@ -108,9 +144,12 @@ private:
QTimer * trackerTimer;
QTimer * workerStartTimer;
QTimer * crazyflieTimer;
+ QTimer *pollGamepadTimer;
QProcess * backendProcess;
QProcess * matlabProcess;
int connectedWorkers;
+ GamepadMonitor *gamepadMonitor;
+ CrazyflieWorker *crazyflieWorker;
};
#endif // MAINWINDOW_H
diff --git a/groundStation/gui/MicroCART/mainwindow.ui b/groundStation/gui/MicroCART/mainwindow.ui
index b76c0b0ed3182238043c080937c72a3a8b44e6f8..34a29f907a8b4e15df6e2f97b7a952d8c9f41269 100644
--- a/groundStation/gui/MicroCART/mainwindow.ui
+++ b/groundStation/gui/MicroCART/mainwindow.ui
@@ -134,11 +134,422 @@
<attribute name="title">
<string>Param</string>
</attribute>
+ <widget class="QComboBox" name="paramGroupComboBox">
+ <property name="geometry">
+ <rect>
+ <x>420</x>
+ <y>100</y>
+ <width>201</width>
+ <height>25</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QComboBox" name="paramEntriesComboBox">
+ <property name="geometry">
+ <rect>
+ <x>420</x>
+ <y>150</y>
+ <width>201</width>
+ <height>25</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QLabel" name="groupLabel">
+ <property name="geometry">
+ <rect>
+ <x>350</x>
+ <y>100</y>
+ <width>54</width>
+ <height>31</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Group:</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="getParamLabel">
+ <property name="geometry">
+ <rect>
+ <x>480</x>
+ <y>40</y>
+ <width>71</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Get Param</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label">
+ <property name="geometry">
+ <rect>
+ <x>350</x>
+ <y>150</y>
+ <width>54</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Entry:</string>
+ </property>
+ </widget>
+ <widget class="Line" name="line">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>279</y>
+ <width>1111</width>
+ <height>31</height>
+ </rect>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ <widget class="QLabel" name="setParamLabel">
+ <property name="geometry">
+ <rect>
+ <x>480</x>
+ <y>310</y>
+ <width>71</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Set Param</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="valueLabel">
+ <property name="geometry">
+ <rect>
+ <x>350</x>
+ <y>240</y>
+ <width>54</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Value:</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="valueVallabel">
+ <property name="geometry">
+ <rect>
+ <x>420</x>
+ <y>240</y>
+ <width>201</width>
+ <height>20</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ <widget class="QLabel" name="groupLabel_set">
+ <property name="geometry">
+ <rect>
+ <x>350</x>
+ <y>360</y>
+ <width>54</width>
+ <height>31</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Group:</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_5">
+ <property name="geometry">
+ <rect>
+ <x>350</x>
+ <y>400</y>
+ <width>54</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Entry:</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="valueLabel_2">
+ <property name="geometry">
+ <rect>
+ <x>350</x>
+ <y>450</y>
+ <width>54</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Value:</string>
+ </property>
+ </widget>
+ <widget class="QComboBox" name="paramGroupComboBox_2">
+ <property name="geometry">
+ <rect>
+ <x>420</x>
+ <y>360</y>
+ <width>201</width>
+ <height>25</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QComboBox" name="paramEntriesComboBox_2">
+ <property name="geometry">
+ <rect>
+ <x>420</x>
+ <y>400</y>
+ <width>201</width>
+ <height>25</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pb_getParam">
+ <property name="geometry">
+ <rect>
+ <x>480</x>
+ <y>200</y>
+ <width>80</width>
+ <height>25</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Get Param</string>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pb_setParam">
+ <property name="geometry">
+ <rect>
+ <x>480</x>
+ <y>500</y>
+ <width>80</width>
+ <height>25</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Set Param</string>
+ </property>
+ </widget>
+ <widget class="QDoubleSpinBox" name="setValueSpin">
+ <property name="geometry">
+ <rect>
+ <x>470</x>
+ <y>450</y>
+ <width>111</width>
+ <height>26</height>
+ </rect>
+ </property>
+ </widget>
</widget>
- <widget class="QWidget" name="controller">
+ <widget class="QWidget" name="Gamepad">
<attribute name="title">
- <string>Controller</string>
+ <string>Gamepad</string>
</attribute>
+ <widget class="QPushButton" name="pb_configRoll">
+ <property name="geometry">
+ <rect>
+ <x>720</x>
+ <y>130</y>
+ <width>141</width>
+ <height>25</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Configure Roll</string>
+ </property>
+ </widget>
+ <widget class="QSlider" name="rollVizBar">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>710</x>
+ <y>160</y>
+ <width>160</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="minimum">
+ <number>-20</number>
+ </property>
+ <property name="maximum">
+ <number>20</number>
+ </property>
+ <property name="value">
+ <number>0</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pb_resetConfig">
+ <property name="geometry">
+ <rect>
+ <x>440</x>
+ <y>90</y>
+ <width>191</width>
+ <height>25</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Reset Configuration</string>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pb_configYaw">
+ <property name="geometry">
+ <rect>
+ <x>160</x>
+ <y>140</y>
+ <width>141</width>
+ <height>25</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Configure Yaw</string>
+ </property>
+ </widget>
+ <widget class="QSlider" name="yawVizBar">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>150</x>
+ <y>170</y>
+ <width>160</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="minimum">
+ <number>-30</number>
+ </property>
+ <property name="maximum">
+ <number>30</number>
+ </property>
+ <property name="value">
+ <number>0</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ <widget class="QSlider" name="thrustVizBar">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>220</x>
+ <y>190</y>
+ <width>16</width>
+ <height>160</height>
+ </rect>
+ </property>
+ <property name="maximum">
+ <number>60000</number>
+ </property>
+ <property name="value">
+ <number>50</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ </widget>
+ <widget class="QSlider" name="pitchVizBar">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>780</x>
+ <y>180</y>
+ <width>16</width>
+ <height>160</height>
+ </rect>
+ </property>
+ <property name="minimum">
+ <number>-20</number>
+ </property>
+ <property name="maximum">
+ <number>20</number>
+ </property>
+ <property name="value">
+ <number>0</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pb_configThrust">
+ <property name="geometry">
+ <rect>
+ <x>250</x>
+ <y>260</y>
+ <width>141</width>
+ <height>25</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Configure Thrust</string>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="pb_configPitch">
+ <property name="geometry">
+ <rect>
+ <x>810</x>
+ <y>250</y>
+ <width>141</width>
+ <height>25</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Configure Pitch</string>
+ </property>
+ </widget>
+ <widget class="QLabel" name="noGamepadWarning">
+ <property name="geometry">
+ <rect>
+ <x>360</x>
+ <y>10</y>
+ <width>381</width>
+ <height>61</height>
+ </rect>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>20</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>No Gamepad Detected</string>
+ </property>
+ <property name="scaledContents">
+ <bool>false</bool>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label_6">
+ <property name="geometry">
+ <rect>
+ <x>450</x>
+ <y>130</y>
+ <width>181</width>
+ <height>91</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Select the input to bind then move the joystick that will control that input</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ <property name="wordWrap">
+ <bool>true</bool>
+ </property>
+ </widget>
</widget>
<widget class="QWidget" name="plots">
<attribute name="title">
@@ -157,14 +568,52 @@
<item>
<widget class="QLabel" name="label_1">
<property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
<property name="text">
<string>Send Setpoint</string>
</property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QRadioButton" name="rbManualSetpoint">
+ <property name="text">
+ <string>Manual Setpoint</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <attribute name="buttonGroup">
+ <string notr="true">buttonGroup_2</string>
+ </attribute>
+ </widget>
+ </item>
+ <item>
+ <widget class="QRadioButton" name="rbGamepadControl">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Gamepad Control</string>
+ </property>
+ <attribute name="buttonGroup">
+ <string notr="true">buttonGroup_2</string>
+ </attribute>
</widget>
</item>
<item>
@@ -190,6 +639,9 @@
<verstretch>0</verstretch>
</sizepolicy>
</property>
+ <property name="text">
+ <string>0</string>
+ </property>
</widget>
</item>
<item row="1" column="0">
@@ -210,6 +662,9 @@
<verstretch>0</verstretch>
</sizepolicy>
</property>
+ <property name="text">
+ <string>0</string>
+ </property>
</widget>
</item>
<item row="2" column="0">
@@ -230,26 +685,50 @@
<verstretch>0</verstretch>
</sizepolicy>
</property>
+ <property name="text">
+ <string>0</string>
+ </property>
</widget>
</item>
<item row="3" column="0">
- <widget class="QLabel" name="thrustlabel">
+ <widget class="QLabel" name="tLabel">
<property name="text">
<string>Thrust</string>
</property>
</widget>
</item>
<item row="3" column="1">
- <widget class="QLineEdit" name="thrustSetpointBox">
+ <widget class="QSlider" name="tActual">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
+ <property name="minimumSize">
+ <size>
+ <width>140</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="toolTipDuration">
+ <number>-1</number>
+ </property>
+ <property name="maximum">
+ <number>60000</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::TicksBelow</enum>
+ </property>
+ <property name="tickInterval">
+ <number>30000</number>
+ </property>
</widget>
</item>
<item row="4" column="0">
@@ -257,6 +736,12 @@
<property name="text">
<string>Angle</string>
</property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <attribute name="buttonGroup">
+ <string notr="true">AngleRateButtonGroup</string>
+ </attribute>
</widget>
</item>
<item row="4" column="1">
@@ -264,6 +749,9 @@
<property name="text">
<string>Rate</string>
</property>
+ <attribute name="buttonGroup">
+ <string notr="true">AngleRateButtonGroup</string>
+ </attribute>
</widget>
</item>
</layout>
@@ -308,7 +796,7 @@
</widget>
</item>
<item row="0" column="1">
- <widget class="QLabel" name="tsActual">
+ <widget class="QLabel" name="currThrustLabel">
<property name="text">
<string>N/A</string>
</property>
@@ -460,4 +948,8 @@
</customwidgets>
<resources/>
<connections/>
+ <buttongroups>
+ <buttongroup name="AngleRateButtonGroup"/>
+ <buttongroup name="buttonGroup_2"/>
+ </buttongroups>
</ui>
diff --git a/groundStation/src/backend/config.c b/groundStation/src/backend/config.c
index a7287220f5130a2abff03cf3e530fb9480bcbc5e..444e15734ce86eadaf923996773b6ddda5ef2846 100644
--- a/groundStation/src/backend/config.c
+++ b/groundStation/src/backend/config.c
@@ -23,8 +23,8 @@ trackable_t trackables[] = {
0,
//Adapter Variables
1,
- "/home/bitcraze/projects/microCART/MicroCART/groundStation/adapters/crazyflie/",
- "/home/bitcraze/projects/microCART/MicroCART/groundStation/adapters/crazyflie/cf_adapter.socket",
+ "./adapters/crazyflie/",
+ "./adapters/crazyflie/cf_adapter.socket",
NULL,
//Local Communication Variables
0, //Set to 1 if Intending to use local Fifo
diff --git a/groundStation/src/backend/param.c b/groundStation/src/backend/param.c
index bb0dc1a9d3ea33249a9164b85f961c99ab4125cf..b31241cef73c440ae45d48ee2bbdc34176525fb7 100644
--- a/groundStation/src/backend/param.c
+++ b/groundStation/src/backend/param.c
@@ -129,8 +129,10 @@ ssize_t EncodeResponseParam(
data[RESP_BLOCK_ID_L] = 0;
data[RESP_BLOCK_ID_H] = 0;
- data[RESP_PARAM_ID_L] = msg[1];
- data[RESP_PARAM_ID_H] = 0;
+ //data[RESP_PARAM_ID_L] = msg[1];
+ //data[RESP_PARAM_ID_H] = msg[2];
+
+ memcpy(&data[RESP_PARAM_ID_L], &msg[1], sizeof(uint16_t));
float fl = -1;
@@ -138,70 +140,70 @@ ssize_t EncodeResponseParam(
case 8:
{
uint8_t pvalue;
- memcpy(&pvalue, &msg[2], sizeof(uint8_t));
+ memcpy(&pvalue, &msg[4], sizeof(uint8_t));
fl = pvalue;
break;
}
case 9:
{
uint16_t pvalue;
- memcpy(&pvalue, &msg[2], sizeof(uint16_t));
+ memcpy(&pvalue, &msg[4], sizeof(uint16_t));
fl = pvalue;
break;
}
case 10:
{
uint32_t pvalue;
- memcpy(&pvalue, &msg[2], sizeof(uint32_t));
+ memcpy(&pvalue, &msg[4], sizeof(uint32_t));
fl = pvalue;
break;
}
case 11:
{
uint64_t pvalue;
- memcpy(&pvalue, &msg[2], sizeof(uint64_t));
+ memcpy(&pvalue, &msg[4], sizeof(uint64_t));
fl = pvalue;
break;
}
case 0:
{
int8_t pvalue;
- memcpy(&pvalue, &msg[2], sizeof(int8_t));
+ memcpy(&pvalue, &msg[4], sizeof(int8_t));
fl = pvalue;
break;
}
case 1:
{
int16_t pvalue;
- memcpy(&pvalue, &msg[2], sizeof(int16_t));
+ memcpy(&pvalue, &msg[4], sizeof(int16_t));
fl = pvalue;
break;
}
case 2:
{
int32_t pvalue;
- memcpy(&pvalue, &msg[2], sizeof(int32_t));
+ memcpy(&pvalue, &msg[4], sizeof(int32_t));
fl = pvalue;
break;
}
case 3:
{
int64_t pvalue;
- memcpy(&pvalue, &msg[2], sizeof(int64_t));
+ memcpy(&pvalue, &msg[4], sizeof(int64_t));
fl = pvalue;
break;
}
case 6:
{
float pvalue;
- memcpy(&pvalue, &msg[2], sizeof(float));
+ memcpy(&pvalue, &msg[4], sizeof(float));
fl = pvalue;
break;
}
case 7:
{
double pvalue;
- memcpy(&pvalue, &msg[2], sizeof(double));
+ memcpy(&pvalue, &msg[4], sizeof(double));
fl = pvalue;
break;
}
@@ -231,9 +233,12 @@ int DecodeResponseParam(
return -1;
}
+ uint16_t id;
+ memcpy(&id, &data[RESP_PARAM_ID_L], sizeof(uint16_t));
+
return snprintf(msg, max_len, "getparam %" PRId16 " %" PRId16 " %f\n",
BytesTo16(data[RESP_BLOCK_ID_L], data[RESP_BLOCK_ID_H]),
- BytesTo16(data[RESP_PARAM_ID_L], data[RESP_PARAM_ID_H]),
+ id,
BytesToFloat(data[RESP_VAL_1], data[RESP_VAL_2],
data[RESP_VAL_3], data[RESP_VAL_4]));
}