bebop2 airframe file and python param estimation

2026-05-09 22:49:53 +08:00 · 2018-04-26 15:04:10 +02:00
parent 424cd788f5
commit 59ffb4caa0
4 changed files with 130 additions and 55 deletions
@@ -15,11 +15,12 @@
 	  <!-- Subsystem section -->
    <module name="telemetry" type="transparent_udp"/>
    <module name="radio_control" type="datalink"/>
-    <module name="motor_mixing"/>
    <module name="actuators" type="bebop"/>
    <module name="imu" type="bebop"/>
    <module name="gps" type="ublox"/>
-    <module name="stabilization" type="indi_simple"/>
+    <module name="stabilization" type="indi">
+      <define name="INDI_RPM_FEEDBACK" value="TRUE"/>
+    </module>
    <module name="ahrs" type="int_cmpl_quat">
      <configure name="USE_MAGNETOMETER" value="TRUE"/>
      <define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
@@ -45,26 +46,17 @@
  </commands>

  <servos driver="Default">
-    <servo name="TOP_LEFT" no="0" min="2500" neutral="2500" max="12000"/>
-    <servo name="TOP_RIGHT" no="1" min="2500" neutral="2500" max="12000"/>
-    <servo name="BOTTOM_RIGHT" no="2" min="2500" neutral="2500" max="12000"/>
-    <servo name="BOTTOM_LEFT" no="3" min="2500" neutral="2500" max="12000"/>
+    <servo name="TOP_LEFT" no="0" min="3000" neutral="3000" max="11000"/>
+    <servo name="TOP_RIGHT" no="1" min="3000" neutral="3000" max="11000"/>
+    <servo name="BOTTOM_RIGHT" no="2" min="3000" neutral="3000" max="11000"/>
+    <servo name="BOTTOM_LEFT" no="3" min="3000" neutral="3000" max="11000"/>
  </servos>

-  <section name="MIXING" prefix="MOTOR_MIXING_">
-    <define name="TRIM_ROLL" value="0"/>
-    <define name="TRIM_PITCH" value="0"/>
-    <define name="TRIM_YAW" value="0"/>
-    <define name="REVERSE" value="TRUE"/>
-    <define name="TYPE" value="QUAD_X"/>
-  </section>
-
  <command_laws>
-    <call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
-    <set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
-    <set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
-    <set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
-    <set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
+    <set servo="TOP_LEFT"     value="autopilot_get_motors_on() ? actuators_pprz[0] : -MAX_PPRZ"/>
+    <set servo="TOP_RIGHT"    value="autopilot_get_motors_on() ? actuators_pprz[1] : -MAX_PPRZ"/>
+    <set servo="BOTTOM_RIGHT" value="autopilot_get_motors_on() ? actuators_pprz[2] : -MAX_PPRZ"/>
+    <set servo="BOTTOM_LEFT"  value="autopilot_get_motors_on() ? actuators_pprz[3] : -MAX_PPRZ"/>
  </command_laws>

  <section name="AIR_DATA" prefix="AIR_DATA_">
@@ -105,56 +97,41 @@
    <define name="DEADBAND_R" value="50"/>
  </section>

-  <section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
-    <!-- attitude reference generation model -->
-    <define name="REF_OMEGA_P" value="450" unit="deg/s"/>
-    <define name="REF_ZETA_P" value="0.9"/>
-    <define name="REF_MAX_P" value="600." unit="deg/s"/>
-    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
-
-    <define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
-    <define name="REF_ZETA_Q" value="0.9"/>
-    <define name="REF_MAX_Q" value="600." unit="deg/s"/>
-    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
-
-    <define name="REF_OMEGA_R" value="450" unit="deg/s"/>
-    <define name="REF_ZETA_R" value="0.9"/>
-    <define name="REF_MAX_R" value="600." unit="deg/s"/>
-    <define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
-  </section>
-
  <section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
    <!-- control effectiveness -->
-    <define name="G1_P" value="0.05"/>
-    <define name="G1_Q" value="0.025"/>
-    <define name="G1_R" value="0.0022"/>
-    <define name="G2_R" value="0.20"/>
+    <define name="G1_ROLL" value="{18.f, -18.f, -18.f , 18.f}"/>
+    <define name="G1_PITCH" value="{15.f , 15.f, -15.f, -15.f }"/>
+    <define name="G1_YAW" value="{0.65f, -0.65f, 0.65f, -0.65f}"/>
+    <define name="G1_THRUST" value="{-.7f, -.7f, -.7f, -.7f}"/>
+    <!--Counter torque effect of spinning up a rotor-->
+    <define name="G2" value="{ 110.f,   -110.f,   110.f,   -110.f}"/>

    <!-- For the bebop2 we need to filter the roll rate due to the dampers -->
-    <define name="FILTER_ROLL_RATE" value="TRUE"/>
+    <define name="FILTER_ROLL_RATE" value="FALSE"/>
    <define name="FILTER_PITCH_RATE" value="FALSE"/>
    <define name="FILTER_YAW_RATE" value="FALSE"/>

    <!-- reference acceleration for attitude control -->
-    <define name="REF_ERR_P" value="170.0"/>
-    <define name="REF_ERR_Q" value="600.0"/>
-    <define name="REF_ERR_R" value="600.0"/>
-    <define name="REF_RATE_P" value="14.3"/>
-    <define name="REF_RATE_Q" value="28.0"/>
-    <define name="REF_RATE_R" value="28.0"/>
+    <define name="REF_ERR_P" value="380.f"/>
+    <define name="REF_ERR_Q" value="380.f"/>
+    <define name="REF_ERR_R" value="380.f"/>
+    <define name="REF_RATE_P" value="23.f"/>
+    <define name="REF_RATE_Q" value="23.f"/>
+    <define name="REF_RATE_R" value="23.f"/>

    <!-- second order filter parameters -->
-    <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF" value="3.2f"/>
+    <define name="ESTIMATION_FILT_CUTOFF" value="3.2f"/>

    <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_DYN" value="{0.08f, 0.08f, 0.08f, 0.08f}"/>

    <!-- Adaptive Learning Rate -->
    <define name="USE_ADAPTIVE" value="FALSE"/>
-    <define name="ADAPTIVE_MU" value="0.0001"/>
+    <define name="ADAPTIVE_MU" value="0.0001f"/>
+
+    <!--Priority for each axis (roll, pitch, yaw and thrust)-->
+    <define name="WLS_PRIORITIES" value="{1000.f, 1000.f, 1.f, 100.f}"/>
  </section>

  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
@@ -471,7 +471,7 @@
   telemetry="telemetry/default_rotorcraft.xml"
   flight_plan="flight_plans/tudelft/delft_basic.xml"
   settings="settings/rotorcraft_basic.xml settings/estimation/ahrs_secondary.xml"
-   settings_modules="modules/gps_ubx_ucenter.xml modules/air_data.xml modules/geo_mag.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
+   settings_modules="modules/gps_ubx_ucenter.xml modules/air_data.xml modules/geo_mag.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
   gui_color="#baa3d698b729"
  />
  <aircraft
@@ -0,0 +1,98 @@
+#!/usr/bin/env python3
+
+# Copyright (C) 2018 Ewoud Smeur
+
+import scipy as sp
+from scipy import signal
+import csv
+import numpy as np
+from numpy import genfromtxt
+import matplotlib.pyplot as plt
+from matplotlib.pyplot import figure, show
+
+# Do a linear least squares fit of the data
+def fit_axis(x,y, axis, start, end):
+    c = np.linalg.lstsq(x[start:end], y[start:end])[0]
+    print('Fit of axis ' + axis + ': ' + str(c*1000))
+    plt.figure()
+    plt.plot(t,y)
+    plt.plot(t,np.dot(x,c))
+    plt.ylabel(axis + ' dotdot[rad/s^3]')
+    plt.xlabel('t [s]')
+    return c
+
+# Effort to make a function that accurately models the actuator, but this is still not sufficient for the bebop2
+def actuator_dyn_2nd(x, zeta, omega, dt, maxrate, maxaccel):
+    y = np.zeros(x.shape)
+    y[0] = x[0]
+    yd = 0
+    ydd = 0
+    for i in range(x.shape[0]-1):
+        ydd = -2*zeta*omega*yd + (x[i]-y[i])*omega*omega
+        if(abs(ydd) > maxaccel):
+            ydd = maxaccel*np.sign(ydd)
+        yd = yd + ydd*dt
+        if(abs(yd) > maxrate):
+            yd = maxrate*np.sign(yd)
+        y[i+1] = y[i] + yd*dt
+    return y
+
+
+# Read data from log file
+data = genfromtxt('00008.csv', delimiter=',', skip_header=1)
+
+# Sample frequency
+sf = 512;
+#First order actuator dynamics constant (discrete, depending on sf)
+fo_c = 0.08
+
+N = data.shape[0]
+
+# Data structure
+t = np.arange(N)/sf
+gyro = data[:,1:4]
+accel = data[:,4:7]/pow(2,10)
+cmd = data[:,7:11]
+# If actuator feedback is available:
+obs = data[:,11:15]
+# If testing the actuator response:
+servotest = data[:,15]
+
+# Actuator dynamics
+cmd_a = sp.signal.lfilter([fo_c], [1, -(1-fo_c)], cmd, axis=0)
+servotest_a = sp.signal.lfilter([fo_c], [1, -(1-fo_c)], servotest, axis=0)
+
+# b, a = signal.butter(2, 7/(sf/2), 'low', analog=False)
+# servotest_a = sp.signal.lfilter(b, a, servotest, axis=0)
+# servotest_a = actuator_dyn_2nd(servotest, 0.8, 70, 1/sf, 60000, 10000000)
+
+# Filtering
+b, a = signal.butter(2, 3.2/(sf/2), 'low', analog=False)
+gyro_f = sp.signal.lfilter(b, a, gyro, axis=0)
+cmd_af = sp.signal.lfilter(b, a, cmd_a, axis=0)
+accel_f = sp.signal.lfilter(b, a, accel, axis=0)
+
+# derivatives
+dgyro_f = np.vstack((np.zeros((1,3)), np.diff(gyro_f,1,axis=0)))*sf
+ddgyro_f = np.vstack((np.zeros((1,3)), np.diff(dgyro_f,1,axis=0)))*sf
+daccel_f = np.vstack((np.zeros((1,3)), np.diff(accel_f,1,axis=0)))*sf
+dcmd_af = np.vstack((np.zeros((1,4)), np.diff(cmd_af,1,axis=0)))*sf
+ddcmd_af = np.vstack((np.zeros((1,4)), np.diff(dcmd_af,1,axis=0)))*sf
+
+# Selective data (for instance remove the takeoff and landing)
+start = 20*sf
+end = 40*sf
+# Estimation of the control effectiveness
+c = fit_axis(dcmd_af[:,0:4], ddgyro_f[:,[0]], 'p', 0, N)
+c = fit_axis(dcmd_af[:,0:4], ddgyro_f[:,[1]], 'q', 0, N)
+c = fit_axis(dcmd_af[:,0:4], daccel_f[:,[2]], 'accel', start, end)
+# Use all commands, to see if there is crosstalk
+c = fit_axis(np.hstack((dcmd_af[:,0:4], ddcmd_af[:,0:4]/sf)), ddgyro_f[:,[2]], 'r', 0, end)
+
+plt.figure()
+plt.plot(t,cmd)
+plt.xlabel('t [s]')
+plt.ylabel('command [PPRZ]')
+
+# Show all plots
+plt.show()