Added set and get functions for base thrust

groundstation-LateralOnQuad/inc/CCrazyflie.h

@@ -278,6 +278,7 @@ class CCrazyflie {
   float getSetpointY();
   float getSetpointZ();
   float getSetpointYaw();
+  uint16_t getBaseThrust();
 
   /**
    * These functions will set the setpoints of the quadcopter
@@ -286,6 +287,7 @@ class CCrazyflie {
   void setSetpointY(float setpoint);
   void setSetpointZ(float setpoint);
   void setSetpointYaw(float setpoint);
+  void setBaseThrust(uint16_t thrust);
 
   /**
    * Enable or disable the thrust controller.
@@ -317,18 +319,6 @@ class CCrazyflie {
    */
   void takeoffCycle();
 
-  /*! \brief Set the thrust control set point
-
-    The thrust value that will be sent to the internal copter
-    controller as a set point.
-
-    \param nThrust The thrust value to send (> 10000) */
-  void setThrust(int nThrust);
-  /*! \brief Returns the current thrust
-
-    \return The current thrust value as reported by the copter */
-  int thrust();
-
   /*! \brief Manages internal calculation operations
 
     Should be called during every 'cycle' of the main program using

groundstation-LateralOnQuad/src/ccrazyflie/CCrazyflie.cpp

@@ -109,33 +109,6 @@ bool CCrazyflie::sendSetpoint(float fRoll, float fPitch, float fYaw,
 	}
 }
 
-void CCrazyflie::setThrust(int nThrust) {
-	if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__  );
-
-	m_nThrust = nThrust;
-
-	if (m_nThrust < m_nMinThrust) {
-		m_nThrust = m_nMinThrust;
-	} else if (m_nThrust > m_nMaxThrust) {
-		m_nThrust = m_nMaxThrust;
-	}
-
-	if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__  );
-}
-
-int CCrazyflie::thrust() {
-	return this->sensorDoubleValue("stabilizer.thrust");
-}
-/*
-// Trying out an idea
-void CCrazyflie::cheat_process_packets() {
-	if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__  );
-	m_tocLogs->processPackets( m_lstLoggingPackets );
-
-	if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__  );	
-}
-*/
-
 bool CCrazyflie::cycle() {
 //	double dTimeNow = this->currentTime();
 	if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__  );

groundstation-LateralOnQuad/src/ccrazyflie/CCrazyflie_controllerFuncs.cpp

@@ -44,9 +44,8 @@ void CCrazyflie::takeoffCycle() {
 		this->setFlightMode(HOVER_MODE);
 	}
 
-	// Set the base thrust and send the setpoints
-	this->m_baseThrust = this->takeoffProfile[this->currentStep].thrust;
-	this->m_sendSetpoints = true;
+	// Set the base thrust
+	this->setBaseThrust( this->takeoffProfile[this->currentStep].thrust );
 }
 
 /**

groundstation-LateralOnQuad/src/ccrazyflie/CCrazyflie_getFuncs.cpp

@@ -80,4 +80,8 @@ float CCrazyflie::getSetpointZ() {
 
 float CCrazyflie::getSetpointYaw() {
 	return this->m_desiredYaw;
+}
+
+uint16_t CCrazyflie::getBaseThrust() {
+	return this->m_baseThrust;
 }
\ No newline at end of file

groundstation-LateralOnQuad/src/ccrazyflie/CCrazyflie_setFuncs.cpp

@@ -34,12 +34,10 @@ void CCrazyflie::setFlightMode( enum FlightMode mode) {
 			this->currentStep = 0;
 			break;
 		case GROUNDED_MODE:
-			this->m_baseThrust = 0;
-			this->m_sendSetpoints = true;
+			this->setBaseThrust( 0 );
 			break;
 		case LANDING_MODE:
-			this->m_desiredPosition.z = 0;
-			this->m_sendSetpoints = true;
+			this->setSetpointZ( 0 );
 			break;
 		default:
 			// Do nothing
@@ -69,6 +67,11 @@ void CCrazyflie::setSetpointYaw(float setpoint) {
 	this->m_desiredYaw = setpoint;
 	this->m_sendSetpoints = true;
 }
+
+void CCrazyflie::setBaseThrust(uint16_t thrust) {
+	this->m_baseThrust = thrust;
+	this->m_sendSetpoints = true;
+}
 /*
 void CCrazyflie::setXYZYawSetpoint(float xSetpoint, float ySetpoint, float zSetpoint,
 									float yawSetpoint, uint16_t baseThrust,