Commit 4403d4d2 authored by Ian McInerney's avatar Ian McInerney

Groundstation update of static gain state feedback controller

parent 597247c2
......@@ -474,6 +474,8 @@ class CCrazyflie {
void setPID_Constants( PID_ControllerParams params );
bool sendPID_Constants( PID_ControllerParams params );
bool sendSGSF_Gains( CONTROLLER_SGSF_GAINS gains);
/*
void setXYZYawPosition(float xPosition, float yPosition, float zPosition,
float yawPosition);
......
......@@ -11,10 +11,10 @@
#include <fstream>
// The number of radios to use
#define NUM_RADIOS 2
#define NUM_RADIOS 1
// The number of quadcopters to initialize
#define NUM_QUADS 4
#define NUM_QUADS 1
#define BASE_THRUST 42000
#define Kp 5000
......@@ -24,6 +24,8 @@
#define Ki 0
#define Kd 750
extern CONTROLLER_SGSF_GAINS ssGains[STATE_END+1];
typedef struct CONTROLLER_DATA {
double loopTimeStartPrevious; // The start time of the last loop to run
double loopTime; // The time taken by running the last loop
......
......@@ -35,6 +35,58 @@ typedef struct computation_localization_config_packet {
float k3;
} __attribute__((packed)) computation_localization_config_packet;
// The ordering of the states for the K matrix
enum stateOrdering {
state_u = 0,
state_v,
state_w,
state_p,
state_q,
state_r,
state_x,
state_y,
state_z,
state_phi,
state_theta,
state_psi,
// Lateral velocity integrator states
state_u_integ,
state_v_integ,
state_w_integ,
// Angular rate integrator states
state_p_integ,
state_q_integ,
state_r_integ,
// Position integrator states
state_x_integ,
state_y_integ,
state_z_integ,
// Euler angle integrator states
state_phi_integ,
state_theta_integ,
state_psi_integ,
// This is an item that will help to know how many states there are
STATE_END = state_psi_integ,
};
enum CONTROLLER_SGSF_OPCODES {
CONT_SGSF_UPDATE_GAINS = 0,
CONT_SGSF_RETRIEVE_GAINS,
};
typedef struct CONTROLLER_SGSF_GAINS {
uint8_t state;
float ut;
float ua;
float ue;
float ur;
} __attribute__((packed)) CONTROLLER_SGSF_GAINS;
typedef enum coordinateSystem {
coor_global = 0,
coor_local,
......
......@@ -41,9 +41,51 @@ int CCrazyflie::queryControllerType() {
return( retVal );
}
bool CCrazyflie::sendSGSF_Gains( CONTROLLER_SGSF_GAINS gains) {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
if (this->getControllerType() != 4) {
std::cout << "SGSF not enabled, ignoring constants" << std::endl;
return false;
}
int nSize = 1 + sizeof(CONTROLLER_SGSF_GAINS);
char cBuffer[nSize];
cBuffer[0] = CONT_SGSF_UPDATE_GAINS;
memcpy(&(cBuffer[1]), &gains, sizeof(CONTROLLER_SGSF_GAINS) );
printf( "sending SGSF Gains (%d, %5.3f, %5.3f, %5.3f, %4.1f)\n",
gains.state, gains.ut, gains.ua, gains.ue, gains.ur);
// The PID update is on port 8
CCRTPPacket *crtpPacket = new CCRTPPacket(cBuffer, nSize, CRTP_PORT_CONTROLLER_UPDATE);
crtpPacket->setChannel(CONTROLLER_CALL_UPDATE); // The PID update is on channel 1
CCRTPPacket *crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
bool retVal = false;
if (crtpReceived != NULL) {
retVal = true;
} else {
retVal = false;
}
// Clear variables and return
delete crtpPacket;
delete crtpReceived;
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
return( retVal );
}
bool CCrazyflie::sendPID_Constants( PID_ControllerParams params ) {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
if (this->getControllerType() != 1) {
std::cout << "PID not enabled, ignoring constants" << std::endl;
return false;
}
int nSize = 4 * sizeof(float) + 1*sizeof(uint8_t);
char cBuffer[nSize];
memcpy(&cBuffer[0 * sizeof(float)], &params.Kp, sizeof(float));
......
......@@ -2,7 +2,7 @@
#include "CCrazyRadio.h"
int CCrazyflie::getControllerType() {
return controllerType;
return this->controllerType;
}
int CCrazyflie::getComputationType() {
......@@ -35,6 +35,8 @@ std::string CCrazyflie::getControllerTypeString() {
case 3:
return( "Controller:LQI" );
break;
case 4:
return( "Controller:SGSF" );
}
return( "Controller:Unknown" );
}
......
......@@ -28,7 +28,8 @@ static const char *variables = "%Time\t\tPositionDelta\t\tBattery\t\t"
"Pitch_Out\t\tRoll_Out\t\tYaw_Out\t\t"
"PitchRate_Out\t\tRollRate_Out\t\tYawRate_Out\t\t"
"baseThrust\t\tcontrollerReset\t\t"
"Ut\t\tUa\t\tUe\t\tUr";
"Ut\t\tUa\t\tUe\t\tUr\t\t"
"controllerDuration\t\testimatorDuration";
static const char *units = "&sec\t\tsec\t\tVolt\t\t"
"meters\t\tmeters\t\t"
......@@ -45,7 +46,8 @@ static const char *units = "&sec\t\tsec\t\tVolt\t\t"
"raw\t\traw\t\traw\t\t"
"raw\t\traw\t\traw\t\t"
"raw\t\tboolean\t\t"
"raw\t\traw\t\traw\t\traw";
"raw\t\traw\t\traw\t\traw\t\t"
"millisec\t\tmillisec";
/*
* Open a log file for the quadcopter
......@@ -158,7 +160,9 @@ void CCrazyflie::writeLogData() {
file_log << "\t\t" << this->sensorDoubleValue("mixer.ctr_roll");
file_log << "\t\t" << this->sensorDoubleValue("mixer.ctr_pitch");
file_log << "\t\t" << this->sensorDoubleValue("mixer.ctr_yaw");
file_log << "\t\t" << this->sensorDoubleValue("controlStats.contDur");
file_log << "\t\t" << this->sensorDoubleValue("controlStats.estimDur");
}
......
......@@ -186,26 +186,39 @@ int main(int argc, char **argv) {
crazyflie_info[i].cflieCopter->configureComputation( (void*) &config, sizeof(computation_localization_config_packet));
// Change the PID controller values
cout << "Sending PID Constants" << endl;
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_X] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_Y] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_Z] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_ROLL] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_PITCH] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_YAW] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_ROLL_RATE] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_PITCH_RATE] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_YAW_RATE] );
switch (crazyflie_info[i].cflieCopter->getControllerType()) {
case 1:
// Change the PID controller values
cout << "Sending PID Constants" << endl;
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_X] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_Y] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_Z] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_ROLL] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_PITCH] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_YAW] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_ROLL_RATE] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_PITCH_RATE] );
usleep(1000);
crazyflie_info[i].cflieCopter->sendPID_Constants( crazyflie_info[i].pidParams[PID_YAW_RATE] );
break;
case 4:
// Change the state space controller values
cout << "Sending state space gains" << endl;
for (int j=0; j < STATE_END; j++) {
usleep(1000);
crazyflie_info[i].cflieCopter->sendSGSF_Gains( ssGains[j] );
}
break;
}
usleep(1000);
// Only do this for the RADA motors
......@@ -228,6 +241,12 @@ int main(int argc, char **argv) {
// The current information about the quad
// Log the controller loop time
if ( crazyflie_info[i].cflieCopter->addLoggingBlock("contStats", 200) ) {
crazyflie_info[i].cflieCopter->addLoggingEntry("contStats", "controlStats.estimDur");
crazyflie_info[i].cflieCopter->addLoggingEntry("contStats", "controlStats.contDur");
}
/* usleep(1000);
if ( crazyflie_info[i].cflieCopter->addLoggingBlock("motor", 100) ) {
......@@ -237,14 +256,14 @@ int main(int argc, char **argv) {
crazyflie_info[i].cflieCopter->addLoggingEntry("motor", "motor.m4");
}
*/
usleep(1000);
/* usleep(1000);
if ( crazyflie_info[i].cflieCopter->addLoggingBlock("mixer", 100) ) {
crazyflie_info[i].cflieCopter->addLoggingEntry("mixer", "mixer.ctr_thrust");
crazyflie_info[i].cflieCopter->addLoggingEntry("mixer", "mixer.ctr_roll");
crazyflie_info[i].cflieCopter->addLoggingEntry("mixer", "mixer.ctr_pitch");
crazyflie_info[i].cflieCopter->addLoggingEntry("mixer", "mixer.ctr_yaw");
}
*/
usleep(1000);
if ( crazyflie_info[i].cflieCopter->addLoggingBlock("stabilizer", 100) ) {
crazyflie_info[i].cflieCopter->addLoggingEntry("stabilizer", "stabilizer.roll");
......@@ -356,7 +375,7 @@ int main(int argc, char **argv) {
pthread_create(&threads[i++], NULL, UIThread, (void*)1);
pthread_create(&threads[i++], NULL, vrpn_go, (void*)2);
// pthread_create(&threads[i++], NULL, displayData, (void*)3);
pthread_create(&threads[i++], NULL, displayData, (void*)3);
for (int j=0; j<NUM_RADIOS; j++) {
pthread_create(&threads[i+j], NULL, CCrazyRadio::startThread, (void*) radios[j].radio );
}
......
......@@ -5,6 +5,32 @@
#include "callbacks.h"
#include "computations/NetworkForwarding.h"
// State Space controller gains
CONTROLLER_SGSF_GAINS ssGains[STATE_END+1] = {
{state_u, 0.000, 0.000, -1500.000, 0.000},
{state_v, 0.000, 1500.037, 0.000, 0.000},
{state_w, -8345.105, 0.000, 0.000, 0.000},
{state_p, 0.000, 2400.728, 0.000, 0.000},
{state_q, 0.000, 0.000, 2340.517, 0.000},
{state_r, 0.000, 0.000, 0.000, 5952.081},
{state_x, 0.000, 0.000, -5000.000, 0.000},
{state_y, 0.000, 5119.382, 0.000, 0.000},
{state_z, -18000.000, 0.000, 0.000, 0.000},
{state_phi, 0.000, 8173.638, 0.000, 0.000},
{state_theta, 0.000, 0.000, 6825.156, 0.000},
{state_psi, 0.000, 0.000, 0.000, 40000.000},
{state_u_integ, 0.000, 0.000, 0.000, 0.000},
{state_v_integ, 0.000, 0.000, 0.000, 0.000},
{state_w_integ, 0.000, 0.000, 0.000, 0.000},
{state_p_integ, 0.000, 0.000, 0.000, 0.000},
{state_q_integ, 0.000, 0.000, 0.000, 0.000},
{state_x_integ, 0.000, 0.000, 1000.000, 0.000},
{state_y_integ, 0.000, -1000.000, 0.000, 0.000},
{state_z_integ, 5000.000, 0.000, 0.000, 0.000},
{state_phi_integ, 0.000, -1000.000, 0.000, 0.000},
{state_theta_integ, 0.000, 0.000, -1000.000, 0.000},
{state_psi_integ, 0.000, 0.000, 0.000, -500.000},
};
// Takeoff routine steps
// First column is the desired base thrust to use
......@@ -13,9 +39,11 @@
int quad1_numTakeoffSteps = 3;
takeoffProfileStep quad1_takeoffSteps[] = {
{19000, step_time, 1},
{50000, step_height, -0.3},
// {54000, step_height, -0.3},
{52000, step_height, -0.3},
// {50000, step_time, 5},
{48739, step_final, 0}
// {50000, step_final, 0}
};
int networkNumNodes = 4;
......@@ -128,16 +156,10 @@ QUADCOPTERS_t crazyflie_info[] = {
},
*/
// Quadcopter 1
{"Crazyflie21",
//"Bridge1",
//"UAV",
// "BigQuad7",
{"Crazyflie23",
NULL,
NULL,
// 75,
25,//swapped 1,2
// 80,
// 45,
45,
0,
XFER_2M,
0,
......@@ -254,7 +276,7 @@ QUADCOPTERS_t crazyflie_info[] = {
{"Crazyflie23",
NULL,
NULL,
65, //swapped radio on 1 and 3
85, //swapped radio on 1 and 3
1,
XFER_2M, // The datarate of the Crazyflie
0,
......@@ -282,12 +304,12 @@ QUADCOPTERS_t crazyflie_info[] = {
},
"logs/cflie3", // The base name of the logfile
{ // The PID values {Kp ,Ki, Kd, iLimit, PID_ID}
{20.0, 10.0, 440.0, 40.0, PID_X},
{20.0, 10.0, 440.0, 40.0, PID_Y},
{-40.0/2, -2.0/2, -44.0/2, 40.0, PID_X},
{40.0/2, 2.0/2, 44.0/2, 40.0, PID_Y},
{-10000.0, -2000.0, -15000.0, 10000.0, PID_Z},
{6.0, 1.0, 0.0, 20.0, PID_ROLL},
{6.0, 1.0, 0.0, 20.0, PID_PITCH},
{6.0, 1.0, 0.35, 360.0, PID_YAW},
{6.0, 2.0, 0.35, 360.0, PID_YAW},
{250.0, 500.0, 2.5, 33.3, PID_ROLL_RATE},
{250.0, 500.0, 2.5, 33.3, PID_PITCH_RATE},
{70.0, 16.7, 0.0, 166.7, PID_YAW_RATE}
......@@ -298,7 +320,7 @@ QUADCOPTERS_t crazyflie_info[] = {
{"Crazyflie24",
NULL,
NULL,
85,
65,
1,
XFER_2M, // The datarate of the Crazyflie
0,
......@@ -327,12 +349,12 @@ QUADCOPTERS_t crazyflie_info[] = {
},
"logs/cflie4", // The base name of the logfile
{ // The PID values {Kp ,Ki, Kd, iLimit, PID_ID}
{20.0, 10.0, 440.0, 40.0, PID_X},
{20.0, 10.0, 440.0, 40.0, PID_Y},
{-40.0/2, -2.0/2, -44.0/2, 40.0, PID_X},
{40.0/2, 2.0/2, 44.0/2, 40.0, PID_Y},
{-10000.0, -2000.0, -15000.0, 10000.0, PID_Z},
{5.4, 1.0, 0.0, 20.0, PID_ROLL},
{5.4, 1.0, 0.0, 20.0, PID_PITCH},
{6.0, 1.0, 0.35, 360.0, PID_YAW},
{6.0, 1.0, 0.0, 20.0, PID_ROLL},
{6.0, 1.0, 0.0, 20.0, PID_PITCH},
{6.0, 2.0, 0.35, 360.0, PID_YAW},
{250.0, 500.0, 2.5, 33.3, PID_ROLL_RATE},
{250.0, 500.0, 2.5, 33.3, PID_PITCH_RATE},
{70.0, 16.7, 0.0, 166.7, PID_YAW_RATE}
......
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