Commit 7a36760c authored by Ian McInerney's avatar Ian McInerney

Made the PID buffer into a C++ queue

parent 6951df95
......@@ -51,8 +51,6 @@
#include "vrpn_Connection.h"
#include "vrpn_Tracker.h"
#define PID_BUFFER_SIZE 12
// Ports defined for the CRTP protocol
enum CRTPPort {
......@@ -179,8 +177,7 @@ class CCrazyflie {
int controllerType;
// Various things for setting the PID parameters of the quadcopter
uint8_t m_bufferEnd;
PID_ControllerParams bufferedParams[PID_BUFFER_SIZE];
std::queue<PID_ControllerParams> pidToSend;
// Control related parameters
/*! \brief Maximum absolute value for the roll that will be sent to
......@@ -375,7 +372,7 @@ class CCrazyflie {
copter communication. */
bool isInitialized();
uint8_t setPID_Constants( PID_ControllerParams params );
void setPID_Constants( PID_ControllerParams params );
bool sendPID_Constants( PID_ControllerParams params );
/*
......
......@@ -98,8 +98,6 @@ CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, int quadNum, dou
m_startTime = startTime;
m_bufferEnd = 0;
// Open the file
try {
char fileName[255];
......@@ -225,14 +223,10 @@ bool CCrazyflie::cycle() {
}
delete pk;
if (m_bufferEnd > 0) {
// Send PID constants
for (int i = 0; i < m_bufferEnd; i++) {
this->sendPID_Constants(bufferedParams[i]);
}
// The buffer is empty
m_bufferEnd = 0;
if ( this->pidToSend.size() > 0 ) {
PID_ControllerParams param = this->pidToSend.front();
this->pidToSend.pop();
this->sendPID_Constants(param);
}
// If the setpoints are new, send them
......
......@@ -92,15 +92,11 @@ bool CCrazyflie::disableThrustController() {
}
uint8_t CCrazyflie::setPID_Constants( PID_ControllerParams params ) {
void CCrazyflie::setPID_Constants( PID_ControllerParams params ) {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
// If there is space in the buffer, add the data to it
if (m_bufferEnd < PID_BUFFER_SIZE) {
bufferedParams[m_bufferEnd] = params;
m_bufferEnd++;
return(0);
}
// Add the PID parameters to the buffer to send
pidToSend.push(params);
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
return(1);
}
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment