diff --git a/crazyflie_groundstation/loggingBlocks.txt b/crazyflie_groundstation/loggingBlocks.txt
index 06bcfd5f6ea8bfbb01af4712403f0e50d785e4e9..1affed75fac3b0324afefff9e93a0ba2ff6d937e 100644
--- a/crazyflie_groundstation/loggingBlocks.txt
+++ b/crazyflie_groundstation/loggingBlocks.txt
@@ -1,7 +1,7 @@
START BLOCK
0
gyro
-100
+30
gyro.x
gyro.y
gyro.z
@@ -10,7 +10,7 @@ END BLOCK
START BLOCK
1
acc
-100
+30
acc.x
acc.y
acc.z
@@ -19,7 +19,7 @@ END BLOCK
START BLOCK
2
stabilizer
-100
+30
stabilizer.roll
stabilizer.pitch
stabilizer.yaw
@@ -28,7 +28,7 @@ END BLOCK
START BLOCK
3
pwm
-100
+30
pwm.m1_pwm
pwm.m2_pwm
pwm.m3_pwm
diff --git a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie.cpp b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie.cpp
index c03a37a5a300c1ffb35e5b9d3cbfd8ac12e26c55..5486bc4dbdb7225c9ff5b1a42f25f64d671ffc1a 100644
--- a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie.cpp
+++ b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie.cpp
@@ -32,6 +32,8 @@
#include <pthread.h>
#include "CCrazyflie.h"
#include "CCrazyRadio.h"
+
+int everyX = 0;
CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, XferRate dataRate, int quadNum, double startTime) {
m_quadNum = quadNum;
@@ -349,7 +351,13 @@ bool CCrazyflie::cycle() {
this->m_sendPosition = false;
} else {
// Send a dummy packet for keepalive otherwise
- m_crRadio->sendDummyPacket(m_nRadioChannel, this);
+ //TODO this is a hacky fix to limit amount of data sent, should use a timer instead
+ if(everyX >= 100){
+ m_crRadio->sendDummyPacket(m_nRadioChannel, this);
+ everyX = 0;
+ }else{
+ everyX++;
+ }
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c
index 6a34ce0adeeeafb33417605a0dd2befc40596acb..c510f70f1f4abb7501a5a90966d54f5bb26162ef 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c
@@ -102,6 +102,13 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
control->yaw = 0;
return;
}
+
+ // Rate-controled YAW is moving YAW angle setpoint
+ if (setpoint->mode.yaw == modeVelocity) {
+ attitudeDesired.yaw += setpoint->attitudeRate.yaw * STUDENT_UPDATE_DT;
+ } else {
+ attitudeDesired.yaw = setpoint->attitude.yaw;
+ }
//set desired roll and pitch and yaw angles
attitudeDesired.roll = setpoint->attitude.roll;
@@ -130,10 +137,7 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
rateDesired.pitch = setpoint->attitudeRate.pitch;
studentAttitudeControllerResetPitchAttitudePID();
}
- if(setpoint->mode.yaw == modeVelocity) {
- rateDesired.yaw = setpoint->attitudeRate.yaw;
- studentAttitudeControllerResetYawAttitudePID();
- }
+
//update the attitude rate PID, given the current angular rate
diff --git a/groundStation/gui/MicroCART/crazyflieworker.cpp b/groundStation/gui/MicroCART/crazyflieworker.cpp
index edd1c2a11c0302119606ea2468b85899d1777fb6..33cecb732a25b8a462801d15fb9ed7eb923426da 100644
--- a/groundStation/gui/MicroCART/crazyflieworker.cpp
+++ b/groundStation/gui/MicroCART/crazyflieworker.cpp
@@ -98,12 +98,22 @@ void CrazyflieWorker::setCurrAttRateSetpoint(float rollRate, float pitchRate, fl
currAttitudeRateSetpoint.throttle = throttle;
}
+void CrazyflieWorker::setCurrMixedSetpoint(float roll, float pitch, float yawRate, float throttle){
+ qInfo() << "set rate";
+ currMixedSetpoint.roll = roll;
+ currMixedSetpoint.pitch = pitch;
+ currMixedSetpoint.yaw = yawRate;
+ currMixedSetpoint.throttle = throttle;
+}
+
+
void CrazyflieWorker::sendAttRateSetpoint()
{
- //qInfo() << " send att rate" << currAttitudeRateSetpoint.throttle << " " << currAttitudeRateSetpoint.roll;
if(isBackendConnected){
+ qInfo() << " send att rate" << currAttitudeRateSetpoint.throttle << " " << currAttitudeRateSetpoint.roll;
+
struct frontend_override_data data;
data.enable = 2; //attitude rate
data.time = 0;
@@ -113,16 +123,19 @@ void CrazyflieWorker::sendAttRateSetpoint()
data.yaw = currAttitudeRateSetpoint.yaw;
//send setpoint
- frontend_setoutputoverride(conn, &data);
+ if(frontend_setoutputoverride(conn, &data) != 0){
+ qInfo() << "error sending att rate setpoint";
+ }
}
}
void CrazyflieWorker::sendAttSetpoint()
{
- //qInfo() << " send att" << currAttitudeSetpoint.throttle;
if(isBackendConnected){
+ qInfo() << " send att" << currAttitudeSetpoint.throttle;
+
struct frontend_override_data data;
data.enable = 1; //attitude
data.time = 0;
@@ -132,25 +145,32 @@ void CrazyflieWorker::sendAttSetpoint()
data.yaw = currAttitudeSetpoint.yaw;
//send setpoint
- frontend_setoutputoverride(conn, &data);
+ if(frontend_setoutputoverride(conn, &data) != 0){
+ qInfo() << "error sending att setpoint";
+ }
}
}
void CrazyflieWorker::sendMixedAttSetpoint(){
- //qInfo() << "send mixed setpoint";
if(isBackendConnected){
+
struct frontend_override_data data;
data.enable = 3; //roll and pitch are attitude, yaw is attitude angle
data.time = 0;
- data.throttle = currAttitudeSetpoint.throttle;
- data.roll = currAttitudeSetpoint.roll;
- data.pitch = currAttitudeSetpoint.pitch;
- data.yaw = currAttitudeRateSetpoint.yaw;
+ data.throttle = currMixedSetpoint.throttle;
+ data.roll = currMixedSetpoint.roll;
+ data.pitch = currMixedSetpoint.pitch;
+ data.yaw = currMixedSetpoint.yaw;
+
+ qInfo() << "send mixed setpoint" << "roll:" << data.roll << " pitch:" << data.pitch;
+
//send setpoint
- frontend_setoutputoverride(conn, &data);
+ if(frontend_setoutputoverride(conn, &data) != 0){
+ qInfo() << "error sending mixed att setpoint";
+ }
}
}
diff --git a/groundStation/gui/MicroCART/crazyflieworker.h b/groundStation/gui/MicroCART/crazyflieworker.h
index 50cbde7723714958f3831d3dc7c05dc3f83027f7..3e57d4fee11407934ae72f4f35a7845d49f80766 100644
--- a/groundStation/gui/MicroCART/crazyflieworker.h
+++ b/groundStation/gui/MicroCART/crazyflieworker.h
@@ -68,6 +68,7 @@ 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 setCurrMixedSetpoint(float roll, float pitch, float yawRate, float throttle);
void setParamValue(QString paramName, double value);
void getGroupEntries(int box, QString gName);
void frontendLogBlockCommand(int8_t command, int8_t id);
@@ -78,6 +79,7 @@ private:
struct backend_conn * conn;
Setpoint currAttitudeSetpoint;
Setpoint currAttitudeRateSetpoint;
+ Setpoint currMixedSetpoint;
std::string getLogFile(int type);
void loadParamIds();
void loadLogIds();
diff --git a/groundStation/gui/MicroCART/mainwindow.cpp b/groundStation/gui/MicroCART/mainwindow.cpp
index f73beb9a09462b1b86fed61aabace9a29bcf7ee4..ce9bce105711f496c5f930726aee845c0aaef6e9 100644
--- a/groundStation/gui/MicroCART/mainwindow.cpp
+++ b/groundStation/gui/MicroCART/mainwindow.cpp
@@ -131,6 +131,7 @@ MainWindow::MainWindow(QWidget *parent) :
connect(this, SIGNAL (rateSetpointSignal(float, float, float, float)), crazyflieWorker, SLOT (setCurrAttRateSetpoint(float, float, float, float)));
connect(this, SIGNAL (angleSetpointSignal(float, float, float, float)), crazyflieWorker, SLOT (setCurrAttSetpoint(float, float, float, float)));
+ connect(this, SIGNAL (mixedSetpointSignal(float, float, float, float)), crazyflieWorker, SLOT (setCurrMixedSetpoint(float, float, float, float)));
connect(crazyflieTimer, SIGNAL(timeout()), this, SLOT(trigger_send_setpoint()));
connect(this, SIGNAL(triggerAttSetpointSend()), crazyflieWorker, SLOT(sendAttSetpoint()));
connect(this, SIGNAL(triggerAttRateSetpointSend()), crazyflieWorker, SLOT(sendAttRateSetpoint()));
@@ -862,7 +863,9 @@ void MainWindow::on_tActual_sliderMoved(int position)
void MainWindow::onGamepadDisconnect(){
- crazyflieWorker->setCurrAttRateSetpoint(0,0,0,0);
+ emit(mixedSetpointSignal(0, 0, 0, 0));
+ emit(rateSetpointSignal(0, 0, 0, 0));
+ emit(angleSetpointSignal(0, 0, 0, 0));
//navigation tab
ui->pitchSetpointBox->setText("0");
ui->rollSetpointBox->setText("0");
@@ -931,8 +934,7 @@ void MainWindow::updateGamepad(){
float thrust = gamepadMonitor->getThrust();
//create mixed setpoint
- crazyflieWorker->setCurrAttSetpoint(roll, pitch, 0, thrust);
- crazyflieWorker->setCurrAttRateSetpoint(0, 0, yaw, 0);
+ emit(mixedSetpointSignal(roll, pitch, yaw, thrust));
ui->rollSetpointBox->setText(QString::number(roll));
ui->pitchSetpointBox->setText(QString::number(pitch));
@@ -976,8 +978,10 @@ void MainWindow::on_pb_configThrust_clicked()
void MainWindow::on_rbManualSetpoint_toggled(bool checked)
{
if(checked) {
- crazyflieWorker->setCurrAttRateSetpoint(0,0,0,0);
- crazyflieWorker->setCurrAttSetpoint(0,0,0,0);
+ emit(mixedSetpointSignal(0, 0, 0, 0));
+ emit(rateSetpointSignal(0, 0, 0, 0));
+ emit(angleSetpointSignal(0, 0, 0, 0));
+
ui->pitchSetpointBox->setText("0");
ui->rollSetpointBox->setText("0");
ui->yawSetpointBox->setText("0");
diff --git a/groundStation/gui/MicroCART/mainwindow.h b/groundStation/gui/MicroCART/mainwindow.h
index 895fa44e1486d5daa23ac8f99e84acb867674cc9..843c614e48c09bd46a93b20cf92a739ec091e57e 100644
--- a/groundStation/gui/MicroCART/mainwindow.h
+++ b/groundStation/gui/MicroCART/mainwindow.h
@@ -34,6 +34,7 @@ signals:
void getPosAttFromBackend();
void rateSetpointSignal(float roll, float pitch, float yaw, float throttle);
void angleSetpointSignal(float roll, float pitch, float yaw, float throttle);
+ void mixedSetpointSignal(float roll, float pitch, float yaw, float throttle);
void getGroupEntries(int box, QString gName);
void triggerAttSetpointSend();
void triggerAttRateSetpointSend();