多軸飛行的研究筆記: quqdcopter pi 的研究筆記

先看主程式

int main(int argc, char *argv[]){

//initializing Network communication

 remote.create();

/* Waiting fo Start command */

while (true){

 remote.exec_remoteCMD();

usleep(2000);

}//end

}

再來看 net.h

class Socket{

	private:
	sockaddr_in m_address;
	unsigned short m_port;
	int m_socket;
	unsigned char m_lastdata[256];
	int get_cmd(float& ,float& ,float& ,float&);
	int get_cmd();


	public:
	Socket();
	~Socket();

	void set_port(int port);
	void create();
	void Close();
	void exec_remoteCMD();
	unsigned char data[256];

	};

	extern Socket remote;

發覺他宣告了一個 Socket 物件 . 名字叫做 remote

void Socket::create()
	{

	//Opening socket
	m_socket = socket( AF_INET, SOCK_DGRAM, IPPROTO_UDP );
	if ( m_socket <= 0 )
	{
	printf( "Failed to create socket\n" );
	exit(EXIT_FAILURE);
	}

	//Binding to desired port number
	if ( bind( m_socket, (const sockaddr*) &m_address, sizeof(sockaddr_in) ) < 0 )
	{
	perror( "failed to bind socket");
	exit(EXIT_FAILURE);
	}

	//setting Socket to non blocking mode
	int nonBlocking = 1;
	if ( fcntl( m_socket, F_SETFL, O_NONBLOCK, nonBlocking ) == -1 )
	{
	printf( "failed to set non-blocking socket\n" );
	exit(EXIT_FAILURE);
	}

	printf( "Succeed to create socket\nWaiting for Instructions...\n" );

	}

void Socket::exec_remoteCMD()
	{
	//PID variables
	float kp_,ki_,kd_;

	switch(get_cmd()){
	//returns 1 for Start(Initialize)
	//returns 2 for Initialize
	//returns 10 for yawstab PID constants
	//returns 11 for yawrate PID constants
	//returns 12 for PRstab PID constants
	//returns 13 for PRrate PID constants

	case NOTHING:
	//nothing to do here
	break;

	case SHUTDOWN:
	//On Shutdown :

	//stop Timer
	Timer.stop();
	if (Timer.started){
	printf("toto");
	}

	//close socket
	Close();

	//stop servos
	if (ESC.Is_open_blaster()){
	ESC.stopServo();
	ESC.close_blaster();
	}

	//shutdown
	system("sudo shutdown -h now");

	//exit quad_pilot
	exit(1);

	//printf("PID stopped \n");

	break;

	case UPDATE_REMOTE:
	//set rcinput values values
	parser.parse(data,Timer.thr,Timer.ypr_setpoint);
	break;

	case UPDATE_PID_YAW_STAB:
	//set pid constants YAW Stab
	parser.parse(data,kp_,ki_,kd_);
	yprSTAB[YAW].set_Kpid(kp_,ki_,kd_);
	break;

	case UPDATE_PID_YAW_RATE:
	//set pid constants YAW Rate
	parser.parse(data,kp_,ki_,kd_);
	yprRATE[YAW].set_Kpid(kp_,ki_,kd_);
	break;

	case UPDATE_PID_PR_STAB:
	//set pid constants
	parser.parse(data,kp_,ki_,kd_);
	yprSTAB[PITCH].set_Kpid(kp_,ki_,kd_);
	yprSTAB[ROLL].set_Kpid(kp_,ki_,kd_);
	//printf("PID: %7.2f %7.2f %7.2f \n",kp_,ki_,kd_);
	break;

	case UPDATE_PID_PR_RATE:
	//set pid constants
	parser.parse(data,kp_,ki_,kd_);
	yprRATE[PITCH].set_Kpid(kp_,ki_,kd_);
	yprRATE[ROLL].set_Kpid(kp_,ki_,kd_);
	//printf("PID: %7.5f %7.5f %7.5f \n",kp_,ki_,kd_);
	break;

	case INIT:
	//intialization of IMU
	if (!Timer.started && !imu.initialized){
	imu.set_com();
	imu.initialize();
	}

	//initilization of PID constants
	yprRATE[YAW].set_Kpid(3.5,0.1,0.1);
	yprRATE[PITCH].set_Kpid(2.9,0.1,0.125);
	yprRATE[ROLL].set_Kpid (2.9,0.1,0.125);
	yprSTAB[PITCH].set_Kpid(3.3,0.035,0.04);
	yprSTAB[ROLL].set_Kpid(3.3,0.035,0.04);

	break;

	case STOP_PID:
	//On exit :

	//stop Timer
	Timer.stop();

	//stop servos
	if (ESC.Is_open_blaster()){
	ESC.stopServo();
	ESC.close_blaster();
	}

	//reset PID
	for (int i=0;i<DIM;i++) yprSTAB[i].reset();
	for (int i=0;i<DIM;i++) yprRATE[i].reset();

	printf("PID Stopped \n");
	break;


	case START_PID:
	//Remote says "Start"

	if (Timer.started){
	//PID already running
	break;
	} else if (!imu.initialized){
	//IMU not initialized
	printf("DMP not Initalized\n Can't start...\n");
	break;
	}

	//Initializing ESCs
	printf("Initialization of ESC...\n");
	ESC.open_blaster();
	ESC.init();
	printf(" ... DONE.\n");

	//Things are getting started !
	//launch the Alarm signal
	Timer.start();
	while (Timer.started){
	sleep(1000);
	}

	}//end switch

	return;
	}

int Socket::get_cmd(){

	int type=0;

	//returns 1 for Start
	//returns 2 for Initialize
	//returns 666 for Exit
	//retunrs 10 for yaw PID constants
	//retunrs 11 for yawrate PID constants
	//retunrs 12 for PR PID constants
	//retunrs 13 for PRrate PID constants

	//returns 0 for rcinputs (default)

	int size = sizeof(data);
	assert( size > 0 );

	if ( m_socket == 0 )
	printf("Socket is closed...");

	int received_bytes = -1;
	sockaddr_in from;
	socklen_t fromLength = sizeof( from );

	received_bytes = recvfrom( m_socket, data, size, 0,
	(sockaddr*)&from,
	&fromLength);
	if (received_bytes == -1){
	//printf("received bytes = -1 \n");
	return NOTHING;
	}

	std::string packet( reinterpret_cast< char const* > (data));
	std::istringstream ss(packet);

	std::string sub;
	ss >> sub;

	do{
	if (sub == "START"){
	type = START_PID;
	break;
	}else if (sub == "STOP"){
	type = STOP_PID;
	break;
	}else if (sub == "EXIT"){
	type = SHUTDOWN;
	break;
	}else if (sub == "INIT"){
	type = INIT;
	break;
	} else if(sub == "pid"){
	ss >> sub;
	if (sub == "yaw_rate"){
	type = UPDATE_PID_YAW_RATE;
	break;
	}else if (sub == "yaw_stab"){
	type = UPDATE_PID_YAW_STAB;
	break;
	}else if (sub == "pr_stab"){
	type = UPDATE_PID_PR_STAB;
	break;
	}else if (sub == "pr_rate"){
	type = UPDATE_PID_PR_RATE;
	break;
	}
	} else { break; }
	}while(ss);

	// printf("%d\n",type);
	return(type);

	}

重點就是.....

Timer.start();

while (Timer.started){

sleep(1000);

}

void TimerClass::sig_handler_(int signum)
	{
	pthread_mutex_lock(&TimerMutex_);

	//1-Get and Execute Command from remote
	remote.exec_remoteCMD();

	//2- get attitude of the drone
	imu.getAttitude();

	// printf("ATTITUDE: %7.2f %7.2f %7.2f\n",imu.ypr[YAW],
	// imu.ypr[PITCH],
	// imu.ypr[ROLL]);

	//3- Timer dt
	Timer.calcdt_();

	//4-1 Calculate PID on attitude

	// if (abs(Timer.ypr_setpoint[YAW])<5) {
	// Timer.ypr_setpoint[YAW] = imu.ypr[YAW];
	// }

	#ifdef PID_STAB
	//Stabilization is only done on Pitch and Roll
	//Yaw is Rate PID only
	for (int i=1;i<DIM;i++){
	Timer.PIDout[i] =
	yprSTAB[i].update_pid_std(Timer.ypr_setpoint[i],
	imu.ypr[i],
	Timer.dt);
	}
	Timer.PIDout[0] = Timer.ypr_setpoint[0];

	// printf("PITCH: %7.2f %7.2f %7.2f\n",Timer.ypr_setpoint[PITCH],
	// imu.ypr[PITCH],
	// Timer.PIDout[PITCH]);

	// printf("ROLL: %7.2f %7.2f %7.2f\n",Timer.ypr_setpoint[ROLL],
	// imu.ypr[ROLL],
	// Timer.PIDout[ROLL]);


	for (int i=0;i<DIM;i++){
	Timer.PIDout[i] =
	yprRATE[i].update_pid_std(Timer.PIDout[i],
	imu.gyro[i],
	Timer.dt);
	}

	// printf("YAW: %7.2f %7.2f %7.2f\n",Timer.ypr_setpoint[YAW],
	// imu.gyro[YAW],
	// Timer.PIDout[YAW]);


	// printf("PITCH: %7.2f %7.2f %7.2f\n",Timer.ypr_setpoint[PITCH],
	// imu.gyro[PITCH],
	// Timer.PIDout[PITCH]);

	// printf("ROLL: %7.2f %7.2f %7.2f\n",Timer.ypr_setpoint[ROLL],
	// imu.gyro[ROLL],
	// Timer.PIDout[ROLL]);


	#endif

	//4-2 Calculate PID on rotational rate
	#ifdef PID_RATE
	for (int i=0;i<DIM;i++){
	Timer.PIDout[i] =
	yprRATE[i].update_pid_std(Timer.ypr_setpoint[i],
	imu.gyro[i],
	Timer.dt);
	}
	//printf("%7.2f %7.2f\n",imu.gyro[PITCH],Timer.PIDout[PITCH]);
	#endif

	//5- ESC update and compensate Timer
	// if timer has not been stopped
	if (Timer.started){
	ESC.update(Timer.thr,Timer.PIDout);
	//printf("%7.2f %7.2f\n",Timer.thr,Timer.PIDout[ROLL]);

	Timer.compensate_();
	}

	pthread_mutex_unlock(&TimerMutex_);
	}

接下來看

//2- get attitude of the drone
	imu.getAttitude();

void DMP::getAttitude()
	{
	if (!dmpReady) return;

	// wait for FIFO count > 42 bits
	do {
	fifoCount = mpu.getFIFOCount();
	}while (fifoCount<42);

	if (fifoCount == 1024) {
	// reset so we can continue cleanly
	mpu.resetFIFO();
	printf("FIFO overflow!\n");

	// otherwise, check for DMP data ready interrupt
	//(this should happen frequently)
	} else {
	//read packet from fifo
	mpu.getFIFOBytes(fifoBuffer, packetSize);

	mpu.dmpGetQuaternion(&q, fifoBuffer);
	mpu.dmpGetGravity(&gravity, &q);
	mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

	ypr[0]-=m_ypr_off[0];

	//scaling for degrees output
	for (int i=0;i<DIM;i++){
	//no offset removal
	//should be accounted while triming
	//ypr[i]-=m_ypr_off[i];
	ypr[i]*=180/M_PI;
	}

	//printf(" %7.2f %7.2f %7.2f\n",ypr[0],ypr[1],ypr[2]);


	//unwrap yaw when it reaches 180
	ypr[0] = wrap_180(ypr[0]);

	//change sign of Pitch, MPU is attached upside down
	ypr[1]*=-1.0;

	mpu.dmpGetGyro(g, fifoBuffer);

	//0=gyroX, 1=gyroY, 2=gyroZ
	//swapped to match Yaw,Pitch,Roll
	//Scaled from deg/s to get tr/s
	for (int i=0;i<DIM;i++){
	gyro[i] = (float)(g[DIM-i-1])/131.0/360.0;
	}

	// printf("gyro %7.2f %7.2f %7.2f \n", (float)g[0]/131.0,
	// (float)g[1]/131.0,
	// (float)g[2]/131.0);

	}
	}

#define YAW 0
#define PITCH 1
#define ROLL 2
#define DIM 3

//3- Timer dt

inline void TimerClass::calcdt_()
	{
	oldtime_ = time_;
	clock_gettime(CLOCK_MONOTONIC, &time_);
	Timer.dt = ((static_cast <int64_t>(time_.tv_sec) * 1000000000 +
	static_cast <int64_t>(time_.tv_nsec)) -
	(static_cast <int64_t>(oldtime_.tv_sec) * 1000000000 +
	static_cast <int64_t>(oldtime_.tv_nsec))) / 1000000000.0;
	}

//4-1 Calculate PID on attitude

for (int i=1;i<DIM;i++){ 

  Timer.PIDout[i] = yprSTAB[i].update_pid_std(Timer.ypr_setpoint[i], imu.ypr[i], Timer.dt);

 } 

 Timer.PIDout[0] = Timer.ypr_setpoint[0];

for (int i=0;i<DIM;i++){ 

Timer.PIDout[i] = yprRATE[i].update_pid_std(Timer.PIDout[i], imu.gyro[i], Timer.dt); 

}

float PID::update_pid_std(float setpoint, float input, float dt)

{

//Computes error

m_err = setpoint-input;

//Integrating errors

m_sum_err += m_err * m_Ki * dt;

//calculating error derivative

//Input derivative is used to avoid derivative kick

m_ddt_err = -m_Kd / dt * (input - m_lastInput);

//Calculation of the output

//winds up boundaries

m_output = m_Kp*m_err + m_sum_err + m_ddt_err;

if (m_output > m_outmax) {

//winds up boundaries

m_sum_err = 0.0;

m_output = m_outmax;

}else if (m_output < m_outmin) {

//winds up boundaries

m_sum_err = 0.0;

m_output = m_outmin;

}

m_lastInput= input;

//printf("kp %f ki %f kd %f\n", m_Kp, m_Ki, m_Kd);

//printf("setpt %7.2f input %7.2f output %f\n", setpoint, input, m_output);

//printf("err %7.2f ddt_err %7.2f sum_err %7.2f\n", m_err, m_ddt_err, m_sum_err);

return m_output;

}

extern PID yprSTAB[3];
extern PID yprRATE[3];

//5- ESC update and compensate Timer

// if timer has not been stopped

if (Timer.started){

ESC.update(Timer.thr,Timer.PIDout);

//printf("%7.2f %7.2f\n",Timer.thr,Timer.PIDout[ROLL]);

Timer.compensate_();

}

void Servo::update(float throttle, float PIDoutput[DIM])

{

servoval[3] =(int)(throttle + PIDoutput[ROLL] - PIDoutput[YAW]);  //  FL

servoval[0] =(int)(throttle + PIDoutput[PITCH] + PIDoutput[YAW]); // FR

servoval[2] =(int)(throttle - PIDoutput[PITCH] + PIDoutput[YAW]); // BL

servoval[1] =(int)(throttle - PIDoutput[ROLL] - PIDoutput[YAW]);    // BR

    hal.rcout->write(MOTOR_FL, rcthr + roll_output + pitch_output - yaw_output);
    hal.rcout->write(MOTOR_BL, rcthr + roll_output - pitch_output + yaw_output);
    hal.rcout->write(MOTOR_FR, rcthr - roll_output + pitch_output + yaw_output);
    hal.rcout->write(MOTOR_BR, rcthr - roll_output - pitch_output - yaw_output);

setServo();

}

void Servo::setServo()

{

   if (Is_open_blaster()){

       for (int i=0;i<4;i++){

              fprintf(fid_servo, "%d=%dus\n",m_servoId[i], servoval[i]);

               fflush(fid_servo);

     }

} 

else {

         printf("Servoblaster not opened \n");

       }

}