Paso 16: Poner juntos: el código
La parte más interesante del código que implementa los conceptos de los cuatro pasos anteriores, cabe en una sola página (o las dos imágenes en este paso). Aquí está, con breves descripciones de lo que hace cada parte:
readGyro();
readAcc();
// START OF + MODE ----------------------------------------------------------------------
a_pitch = (accel_x_zero - accel_x);
a_roll = (accel_y - accel_y_zero);
a_z = (int firmado) analogRead(A_Z) - a_z_z;
g_pitch = (gyro_y - gyro_y_zero);
g_roll = (gyro_x - gyro_x_zero);
g_yaw = (gyro_z - gyro_z_zero);
Esta parte toma las señales crudas de acelerómetro y giroscopio. También lo hace la reducción a cero, aunque tengo todos mis ceros establecer... cero... ya que el mínimo de Pololu-9 hace un buen trabajo de puesta a cero de sus resultados ya. Si es necesario, se podían cambiar los valores de cero para recortar la cuadrotor. Nota: se invierte a_pitch. Este es un tema físico. En tono positivo, el vector de la gravedad apunta hacia el eje X de la IMU. Esto se registra como aceleración negativa en X, puesto que el accelrometer no puede distinguir entre la que se inclina hacia adelante y acelerar hacia atrás. a_roll no tiene esta inversión.
rate_pitch = (float) g_pitch * G_GAIN;
rate_roll = (float) g_roll * G_GAIN;
rate_yaw = (float) g_yaw * G_GAIN;
Convertir las señales del girocompás crudo en unidades físicas de º / s.
angle_pitch = AA * (angle_pitch + rate_pitch * DT);
angle_pitch += (1,0 - AA) * (float) a_pitch * A_GAIN;
pitch_error = (float) (pitch_command - 127) / 127.0 * MAX_ANGLE - angle_pitch;
pitch_error_integral += pitch_error * DT;
Si (pitch_error_integral > = INT_SAT) {pitch_error_integral = INT_SAT;}
Si (pitch_error_integral < = - INT_SAT) {pitch_error_integral = - INT_SAT;}
angle_roll = AA * (angle_roll + rate_roll * DT);
angle_roll += (1,0 - AA) * (float) a_roll * A_GAIN;
roll_error = (float) (roll_command - 127) / 127.0 * MAX_ANGLE - angle_roll;
roll_error_integral += roll_error * DT;
Si (roll_error_integral > = INT_SAT) {roll_error_integral = INT_SAT;}
Si (roll_error_inttegral < = - INT_SAT) {roll_error_integral = - INT_SAT;}
Echada y rodillo Filtros complementarios, así como cálculo de error del ángulo. pitch_command y roll_command provienen de la radio y se transmiten desde la estación de tierra basada en entradas del usuario. MAX_ANGLE es una constante que define el ángulo de la echada y del rodillo de ordenada máximo en grados de definir usuario. Los términos control integral son todos comentados, pero no dudes en jugar con ellos.
Control
front_command_f = pitch_error * KP;
front_command_f = pitch_error_integral * KI;
front_command_f += rate_pitch * KD;
front_command_f-= ((float) (yaw_command - 127) / 127.0 * MAX_RATE - rate_yaw) * KY;
rear_command_f += pitch_error * KP;
rear_command_f += pitch_error_integral * KI;
rear_command_f = rate_pitch * KD;
rear_command_f-= ((float) (yaw_command - 127) / 127.0 * MAX_RATE - rate_yaw) * KY;
right_command_f = roll_error * KP;
right_command_f = roll_error_integral * KI;
right_command_f += 1.3 * rate_roll * KD;
right_command_f += ((float) (yaw_command - 127) / 127.0 * MAX_RATE - rate_yaw) * KY;
left_command_f += roll_error * KP;
left_command_f += roll_error_integral * KI;
left_command_f = rate_roll * KD;
left_command_f += ((float) (yaw_command - 127) / 127.0 * MAX_RATE - rate_yaw) * KY;
// END OF + MODE ------------------------------------------------------------------------
Esta es la matriz de comandos del paso 12. El mando de guiñada es en-alineado, que clase de tonto. yaw_command viene de la radio y se transmite por la estación de tierra. MAX_RATE es una constante definida por el usuario que establece la máxima al mando de guiñada tarifa de la rotación en grados por segundo.
