rate controller update

apps/multirotor_att_control/multirotor_att_control_main.c

@@ -104,14 +104,14 @@ static void *rate_control_thread_main(void *arg)
 	while (!thread_should_exit) {
 		/* rate control at maximum rate */
 		/* wait for a sensor update, check for exit condition every 1000 ms */
-		int ret = poll(&fds, 1, 1000);
+//		int ret = poll(&fds, 1, 1000);
-
+//
-		if (ret < 0) {
+//		if (ret < 0) {
-			/* XXX this is seriously bad - should be an emergency */
+//			/* XXX this is seriously bad - should be an emergency */
-		} else if (ret == 0) {
+//		} else if (ret == 0) {
-			/* XXX this means no sensor data - should be critical or emergency */
+//			/* XXX this means no sensor data - should be critical or emergency */
-			printf("[mc att control] WARNING: Not getting gyro data, no rate control\n");
+//			printf("[mc att control] WARNING: Not getting gyro data, no rate control\n");
-		} else {
+//		} else {
 			/* get data */
 			orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report);
 			bool rates_sp_valid = false;
@@ -133,7 +133,8 @@ static void *rate_control_thread_main(void *arg)
 				multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
 				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 			}
-		}
+//		}
+		usleep(2000);
 	}
 
 	return NULL;

apps/multirotor_att_control/multirotor_rate_control.c

@@ -56,18 +56,21 @@
 // PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f);
 
 PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */
-PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
+//PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
+//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
 PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 8.0f);	/**< roughly < 500 deg/s limit */
 
 struct mc_rate_control_params {
 
 	float yawrate_p;
+	float yawrate_d;
 	float yawrate_i;
 	float yawrate_awu;
 	float yawrate_lim;
 
 	float attrate_p;
+	float attrate_d;
 	float attrate_i;
 	float attrate_awu;
 	float attrate_lim;
@@ -79,11 +82,13 @@ struct mc_rate_control_param_handles {
 
 	param_t yawrate_p;
 	param_t yawrate_i;
+	param_t yawrate_d;
 	param_t yawrate_awu;
 	param_t yawrate_lim;
 
 	param_t attrate_p;
 	param_t attrate_i;
+	param_t attrate_d;
 	param_t attrate_awu;
 	param_t attrate_lim;
 };
@@ -106,11 +111,13 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
 	/* PID parameters */
 	h->yawrate_p 	=	param_find("MC_YAWRATE_P");
 	h->yawrate_i 	=	param_find("MC_YAWRATE_I");
+	h->yawrate_d 	=	param_find("MC_YAWRATE_D");
 	h->yawrate_awu 	=	param_find("MC_YAWRATE_AWU");
 	h->yawrate_lim 	=	param_find("MC_YAWRATE_LIM");
 
 	h->attrate_p 	= 	param_find("MC_ATTRATE_P");
 	h->attrate_i 	= 	param_find("MC_ATTRATE_I");
+	h->attrate_d 	= 	param_find("MC_ATTRATE_D");
 	h->attrate_awu 	= 	param_find("MC_ATTRATE_AWU");
 	h->attrate_lim 	= 	param_find("MC_ATTRATE_LIM");
 
@@ -121,11 +128,13 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 {
 	param_get(h->yawrate_p, &(p->yawrate_p));
 	param_get(h->yawrate_i, &(p->yawrate_i));
+	param_get(h->yawrate_d, &(p->yawrate_d));
 	param_get(h->yawrate_awu, &(p->yawrate_awu));
 	param_get(h->yawrate_lim, &(p->yawrate_lim));
 
 	param_get(h->attrate_p, &(p->attrate_p));
 	param_get(h->attrate_i, &(p->attrate_i));
+	param_get(h->attrate_d, &(p->attrate_d));
 	param_get(h->attrate_awu, &(p->attrate_awu));
 	param_get(h->attrate_lim, &(p->attrate_lim));
 
@@ -151,106 +160,43 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	static bool initialized = false;
 
-	/* initialize the pid controllers when the function is called for the first time */
-	if (initialized == false) {
-		parameters_init(&h);
-		parameters_update(&h, &p);
-
-		pid_init(&yaw_speed_controller, p.yawrate_p, 0, p.yawrate_i, p.yawrate_awu,
-			PID_MODE_DERIVATIV_SET);
-		pid_init(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu,
-			PID_MODE_DERIVATIV_SET);
-		pid_init(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu,
-			PID_MODE_DERIVATIV_SET);
-
-		initialized = true;
-	}
 
 	/* load new parameters with lower rate */
-	if (motor_skip_counter % 250 == 0) {
+
+	if (motor_skip_counter % 500 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
 		/* apply parameters */
+
+
+
 		pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu);
 		pid_set_parameters(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu);
 		pid_set_parameters(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu);
 	}
 
 	/* calculate current control outputs */
+	float setpointXrate;
+	float setpointYrate;
+	float setpointZrate;
+	float setRollRate=rate_sp->roll;
+	float setPitchRate=rate_sp->pitch;
+	float setYawRate=rate_sp->yaw;
+
 	
-	/* control pitch (forward) output */
+			//x-axis
-	float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch,
+			setpointXrate=p.attrate_p*(setRollRate-gyro_filtered[0]);
-					rates[1], 0.0f, deltaT);
+			//Y-axis
-	/* control roll (left/right) output */
+			setpointYrate=p.attrate_p*(setPitchRate-gyro_filtered[1]);
-	float roll_control = pid_calculate(&roll_controller, rate_sp->roll,
+			//Z-axis
-					rates[0], 0.0f, deltaT);
+			setpointZrate=p.yawrate_p*(setYawRate-gyro_filtered[2]);
-	/* control yaw rate */
-	float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
-
-	/*
-	 * compensate the vertical loss of thrust
-	 * when thrust plane has an angle.
-	 * start with a factor of 1.0 (no change)
-	 */
-	float zcompensation = 1.0f;
-
-	// if (fabsf(att->roll) > 0.3f) {
-	// 	zcompensation *= 1.04675160154f;
-
-	// } else {
-	// 	zcompensation *= 1.0f / cosf(att->roll);
-	// }
-
-	// if (fabsf(att->pitch) > 0.3f) {
-	// 	zcompensation *= 1.04675160154f;
-
-	// } else {
-	// 	zcompensation *= 1.0f / cosf(att->pitch);
-	// }
-
-	float motor_thrust = 0.0f;
-
-	motor_thrust = rate_sp->thrust;
-
-	/* compensate thrust vector for roll / pitch contributions */
-	motor_thrust *= zcompensation;
-
-	/* limit yaw rate output */
-	if (yaw_rate_control > p.yawrate_lim) {
-		yaw_rate_control = p.yawrate_lim;
-		yaw_speed_controller.saturated = 1;
-	}
-
-	if (yaw_rate_control < -p.yawrate_lim) {
-		yaw_rate_control = -p.yawrate_lim;
-		yaw_speed_controller.saturated = 1;
-	}
-
-	if (pitch_control > p.attrate_lim) {
-		pitch_control = p.attrate_lim;
-		pitch_controller.saturated = 1;
-	}
-
-	if (pitch_control < -p.attrate_lim) {
-		pitch_control = -p.attrate_lim;
-		pitch_controller.saturated = 1;
-	}
 
 
-	if (roll_control > p.attrate_lim) {
-		roll_control = p.attrate_lim;
-		roll_controller.saturated = 1;
-	}
 
-	if (roll_control < -p.attrate_lim) {
+	actuators->control[0] = setpointXrate; //roll
-		roll_control = -p.attrate_lim;
+	actuators->control[1] = setpointYrate; //pitch
-		roll_controller.saturated = 1;
+	actuators->control[2] = setpointZrate; //yaw
-	}
+	actuators->control[3] = rate_sp->thrust;
-
-	actuators->control[0] = roll_control;
-	actuators->control[1] = pitch_control;
-	actuators->control[2] = yaw_rate_control;
-	actuators->control[3] = motor_thrust;
 
 	motor_skip_counter++;
 }