diff --git a/.vscode/launch.json b/.vscode/launch.json index 7059bd10b..88aa86663 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -17,7 +17,7 @@ "name": "MOCKUP docker", "type": "gdb", "request": "attach", - "executable": "${workspaceFolder}/obj/main/indiflight_MOCKUP.so", + //"executable": "${workspaceFolder}/obj/main/indiflight_MOCKUP.so", "target": "localhost:3333", "remote": true, "cwd": "${workspaceRoot}", diff --git a/configs/profiles/CineRat.txt b/configs/profiles/CineRat.txt index 27154a524..39e2fda2d 100644 --- a/configs/profiles/CineRat.txt +++ b/configs/profiles/CineRat.txt @@ -49,7 +49,6 @@ #define USE_LEARNER #define USE_NN_CONTROL - ################################################ # CRAFT SETTINGS (formerly known as "profile") # ################################################ @@ -204,6 +203,13 @@ indiprofile 0 set indi_attitude_gains = 3000, 3000, 1000 set indi_rate_gains = 230, 230, 150 +# --- feedforward control config +# H(z) = (b0 + b1*z^-1 + b2*z^-2) / (1 + a1*z^-1 + a2*z^-2) +# Coefficient: unitless * 1000 +# Coefficient order: b0, b1, b2, a1, a2 +# set indi_ff_coefs = 0, 0, 0, 0, 0 +set indi_ff_coefs = 292, -292, 0, -1949, 950 + # attitude controller limits: (deg/s) set indi_attitude_max_tilt_rate = 800 set indi_attitude_max_yaw_rate = 500 @@ -235,6 +241,11 @@ set indi_act_max = 100, 100, 100, 100 # % set indi_wls_act_preferred_state = 0,0,0,0 set indi_wls_act_penalties = 1,1,1,1 +# lead lag filtering for motor Outputs +set indi_use_motor_lead_lag = 0 +set indi_act_time_constant_true_ms = 25,25,25,25 +set indi_act_time_constant_des_ms = 50,50,50,50 + # fx, fy, fz is N/kg / 1 * 100. Roll, pitch is Nm / (kgm^2) / 1 * 10. Yaw is Nm / (kgm^2) / 1 * 10 set indi_act_g1_fx = 0,0,0,0 set indi_act_g1_fy = 0,0,0,0 diff --git a/configs/profiles/CineRatDemo.txt b/configs/profiles/CineRatDemo.txt new file mode 100644 index 000000000..a6b9f5327 --- /dev/null +++ b/configs/profiles/CineRatDemo.txt @@ -0,0 +1,457 @@ +# This file contains build flags and configuration for the CineRat 3inch drone + +############### +# BUILD FLAGS # +############### + +## ----- RC components +#define USE_SERIALRX +#define USE_SERIALRX_SBUS +#define USE_SERIALRX_CRSF + +#define USE_TELEMETRY +#define USE_TELEMETRY_SBUS +#define USE_TELEMETRY_CRSF +#define USE_TELEMETRY_PI + +## ----- Other features +#define USE_GPS +#define USE_GEOFENCE + +## ----- FPV features: LED and onscreen display +## #define USE_LED_STRIP +#define USE_OSD +#define USE_OSD_SD +#define USE_OSD_HD + +## ----- Outputs and logging +#define USE_DSHOT +## #define USE_SERVOS +## #define USE_ACTUATORS_T4 +#define USE_BLACKBOX + +## ----- Developer features +## #define USE_BENCHMARK +## #define USE_STACK_CHECK +## #define USE_CLI_DEBUG_PRINT +#define PI_STATS ## needs USE_TELEMETRY_PI +#define PI_USE_PRINT_MSGS + +## ----- INDIFLIGHT features +#define USE_INDI +#define USE_EKF +#define USE_LOCAL_POSITION +#define USE_TRAJECTORY_TRACKER + +## ----- DANGERZONE +#define USE_THROW_TO_ARM +#define USE_CATAPULT +#define USE_LEARNER +#define USE_NN_CONTROL + +################################################ +# CRAFT SETTINGS (formerly known as "profile") # +################################################ + +## ----- PERIPHERALS + +# feature +feature GPS +feature TELEMETRY +#feature LED_STRIP +feature OSD + +# gps +set gps_provider = UBLOX +set gps_auto_baud = ON +set gps_ublox_use_galileo = ON +set gps_update_rate_hz = 15 + +# geofence +geofence clear + +# serial ports +serial 5 64 115200 57600 0 115200 +serial 1 262144 115200 57600 0 115200 +#serial 3 2 115200 115200 0 115200 # GPS +serial 6 524288 115200 57600 0 115200 + +# rx +set serialrx_provider = SBUS + +# map +#map TAER1234 # todo: which one is it here? + +# switch configs +aux 0 0 1 1700 2100 0 0 +aux 1 1 3 900 2100 0 0 +aux 2 55 0 1700 2100 0 0 +aux 3 57 6 1300 1700 0 0 +aux 4 58 3 1300 1700 0 0 +aux 5 60 3 1700 2100 0 0 +#aux 5 13 7 1700 2100 0 0 ## beeper instead of NN +aux 6 36 2 1700 2100 0 0 +aux 7 56 6 1700 2100 0 0 +aux 8 56 6 1700 2100 0 0 ## reset home + +# blackbox config +set blackbox_disable_pids = ON +set blackbox_disable_setpoint = ON +set blackbox_disable_alt = OFF +set blackbox_disable_bat = OFF +set blackbox_disable_rssi = OFF +set blackbox_disable_debug = OFF +set blackbox_disable_indi = OFF +set blackbox_disable_pos = OFF +set blackbox_disable_ekf = OFF +set blackbox_disable_learner = OFF +set blackbox_high_resolution = ON +set debug_mode = BARO + +# sensor calibration / alignment +set align_board_roll = 0 +set align_board_pitch = 0 +set align_board_yaw = 0 +set acc_calibration = 0,0,0,1 +set acc_offset = -10, -12, 8 + +# motor setup +set dshot_bidir = ON +set dshot_idle_value = 550 +set motor_pwm_protocol = DSHOT600 +set motor_poles = 12 +set motor_output_reordering = 0,1,2,3,4,5,6,7 + +# failsafes +set failsafe_delay = 5 +set small_angle = 20 # angle at which to deny arming + + +## ----- FLIGHT PERFORMANCE + +# scheduling +set pid_process_denom = 4 # relative to imu +set ahrs_process_denom = 4 # relative to imu +set nn_rate_denom = 2 # relative to pid +set blackbox_sample_rate = 1/4 # relative to pid + +# ekf (these are the defaults). source 0: pi, 1: uros, 2: gps +set ekf_use_quat = 1 +set ekf_use_for_manual_flight = 0 +set ekf_meas_source = 0 +set ekf_proc_noise_acc = 500000, 500000, 500000 +set ekf_proc_noise_gyro = 100000, 100000, 100000 +set ekf_proc_noise_acc_bias = 100, 100, 100 +set ekf_proc_noise_gyro_bias = 10, 10, 10 +set ekf_meas_noise_position = 1000, 1000, 1000 +#set ekf_meas_noise_position = 5000, 5000, 40000 +set ekf_meas_noise_quat = 50000, 50000, 50000, 50000 +set ekf_meas_delay = 0 + +# filter setups +set gyro_lpf1_static_hz = 100 +set gyro_lpf2_static_hz = 0 +set dyn_notch_count = 0 +set dyn_notch_q = 500 +set gyro_lpf1_dyn_min_hz = 0 +set gyro_lpf1_dyn_max_hz = 250 +set acc_lpf_hz = 50 +set simplified_gyro_filter_multiplier = 40 +set rpm_filter_min_hz = 60 + +profile 0 + +set dterm_lpf1_dyn_min_hz = 0 +set dterm_lpf1_dyn_max_hz = 75 +set dterm_lpf1_static_hz = 0 +set dterm_lpf2_static_hz = 0 +set yaw_lowpass_hz = 0 +set thrust_linear = 55 +set simplified_dterm_filter = OFF + + +## ----- RATE PROFILES + +rateprofile 0 + +set rateprofile_name = velocidrone +set thr_mid = 50 +set thr_expo = 0 +set rates_type = ACTUAL +set roll_rc_rate = 100 +set pitch_rc_rate = 100 +set yaw_rc_rate = 100 +set roll_expo = 1 +set pitch_expo = 1 +set yaw_expo = 1 +set roll_srate = 70 +set pitch_srate = 70 +set yaw_srate = 70 +set throttle_limit_type = OFF +set throttle_limit_percent = 100 +set roll_rate_limit = 1998 +set pitch_rate_limit = 1998 +set yaw_rate_limit = 1998 + + +## ----- INDI ATTITUDE CONTROLLER + +indiprofile 0 + +# --- attitude control config and limits --- +# Attitude: (deg/s/s) / (deg) * 10 Rate: (deg/s/s) / (deg/s)*10 +set indi_attitude_gains = 3000, 3000, 1000 +set indi_rate_gains = 230, 230, 150 + +# --- feedforward control config +# H(z) = (b0 + b1*z^-1 + b2*z^-2) / (1 + a1*z^-1 + a2*z^-2) +# Coefficient: unitless * 1000 +# Coefficient order: b0, b1, b2, a1, a2 +# set indi_ff_coefs = 0, 0, 0, 0, 0 +set indi_ff_coefs = 292, -292, 0, -1949, 950 + +# attitude controller limits: (deg/s) +set indi_attitude_max_tilt_rate = 800 +set indi_attitude_max_yaw_rate = 500 + +# --- manual flight config and limits --- +set indi_manual_max_upwards_accel = 30 # N/kg (or 255 to set to max possible) +set indi_manual_max_tilt = 45 # deg +set indi_manual_use_coordinated_yaw = 1 + +# --- indi config --- +set indi_use_increment = 1 +set indi_use_accel_for_thrust = 1 +set indi_use_rpm_dot_feedback = 1 +set indi_act_num = 4 +set indi_sync_lowpass_hz = 15 + +# fx, fy, fz, roll, pitch, yaw +set indi_wls_axes_weights = 1,1,50,50,50,5 + +# --- actuators: fields can have up to 8 elements --- +set indi_act_is_motor = 1,1,1,1,0,0,0,0 +set indi_act_is_servo = 0,0,0,0,0,0,0,0 +set indi_act_time_constant_ms = 25, 25, 25, 25 # ms +set indi_act_max_rpm = 40000, 40000, 40000, 40000 # rpm +set indi_act_hover_rpm = 20000, 20000, 20000, 20000 # rpm +set indi_act_nonlinearity = 50, 50, 50, 50 # % +set indi_act_min = 0, 0, 0, 0 # % will be forced 0 for motors anyway +set indi_act_max = 100, 100, 100, 100 # % +set indi_wls_act_preferred_state = 0,0,0,0 +set indi_wls_act_penalties = 1,1,1,1 + +# lead lag filtering for motor Outputs +set indi_use_motor_lead_lag = 0 +set indi_act_time_constant_true_ms = 25,25,25,25 +set indi_act_time_constant_des_ms = 50,50,50,50 + +# fx, fy, fz is N/kg / 1 * 100. Roll, pitch is Nm / (kgm^2) / 1 * 10. Yaw is Nm / (kgm^2) / 1 * 10 +set indi_act_g1_fx = 0,0,0,0 +set indi_act_g1_fy = 0,0,0,0 +set indi_act_g1_fz = -1050, -1050, -1050, -1050 +set indi_act_g1_roll = -4000, -4000, 4000, 4000 +set indi_act_g1_pitch = -2600, 2600, -2600, 2600 +set indi_act_g1_yaw = -510, 510, 510, -510 + +# Nm / (kgm^2) / (rad/s/s) * 1e5 +set indi_act_g2_roll = 0,0,0,0 +set indi_act_g2_pitch = 0,0,0,0 +set indi_act_g2_yaw = -100, 100, 100, -100 + + +indiprofile 2 + +# indiprofile 2 --> these are the settings for the learned profile +# commented will be overwritten by the learner, so they dont need to be set +# as the initial condition is currently always zero. This may be changed in the future. +# the default values (values not set here) are the same as for all other profiles + +# these values are slightly more conservative than indiprofile 0 + +set indi_act_num = 4 +# set indi_attitude_gains = 1552,1552,1552 +# set indi_rate_gains = 174,174,174 +set indi_attitude_max_tilt_rate = 500 +set indi_attitude_max_yaw_rate = 300 +set indi_manual_use_coordinated_yaw = 1 +set indi_manual_max_upwards_accel = 20 +set indi_act_is_motor = 1,1,1,1,0,0,0,0 +set indi_act_is_servo = 0,0,0,0,0,0,0,0 +set indi_act_time_constant_ms = 25, 25, 25, 25 # ms +set indi_act_max_rpm = 40000, 40000, 40000, 40000 # rpm +set indi_act_hover_rpm = 20000, 20000, 20000, 20000 # rpm +set indi_act_nonlinearity = 50, 50, 50, 50 # % +set indi_act_min = 0, 0, 0, 0 # % will be forced 0 for motors anyway +set indi_act_max = 100, 100, 100, 100 # % +set indi_wls_act_preferred_state = 0,0,0,0 +set indi_wls_act_penalties = 1,1,1,1 + +# -- roll -56, pitch 81, yaw 170 +#set indi_act_g1_fx = -1037, -1037, -1037, -1037 +#set indi_act_g1_fy = -136, -136, -136, -136 +#set indi_act_g1_fz = -91, -91, -91, -91 +#set indi_act_g1_roll = 183, 1049, -41, -1190 +#set indi_act_g1_pitch = -2617, -4609, 4742, 2485 +#set indi_act_g1_yaw = -4016, 816, -727, 3927 +#set indi_act_g2_roll = -98, 98, 98, -98 +#set indi_act_g2_pitch = -12, 12, 12, -12 +#set indi_act_g2_yaw = -8, 8, 8, -8 + +set indi_sync_lowpass_hz = 15 +set indi_wls_axes_weights = 1, 1, 50, 50, 50, 5 +set indi_wls_act_penalties = 1,1,1,1 + + +## ----- POSITION CONTROL + +positionprofile 0 + +# gains * 10 +set position_horizontal_p = 30 +set position_horizontal_i = 2 +set position_horizontal_d = 30 +set position_vertical_p = 50 +set position_vertical_i = 5 +set position_vertical_d = 40 + +# max velocities in cm/s and cm/s^2 +set position_max_horizontal_speed = 2000 +set position_max_horizontal_accel = 10000 +set position_max_upwards_speed = 500 +set position_max_downwards_speed = 500 +set position_max_upwards_accel = 2000 +set position_max_downwards_accel = 800 + +# max tilt, yaw gain * 10, thrust config +set position_max_tilt = 180 +set position_yaw_p = 50 +set position_use_thrust_attenuation = 1 + + +positionprofile 2 + +# positionprofile 2 --> these are the settings for the learned profile +# commented will be overwritten by the learner, so they dont need to be set +# as the initial condition is currently always zero. This may be changed in the future. +# the default values (values not set here) are the same as for all other profiles + +# these values are more conservative than positionprofile 0 + +# set position_horizontal_p = 63 +# set position_horizontal_i = 4 +# set position_horizontal_d = 45 +set position_max_horizontal_speed = 500 +set position_max_horizontal_accel = 1000 +set position_max_tilt = 40 +# set position_vertical_p = 63 +# set position_vertical_i = 4 +# set position_vertical_d = 45 +set position_max_upwards_speed = 100 +set position_max_downwards_speed = 100 +set position_max_upwards_accel = 2500 +set position_max_downwards_accel = 500 +# set position_yaw_p = 44 +set position_weathervane_p = 0 +set position_use_thrust_attenuation = 1 + + +## ----- SET DEFAULT PROFILES + +profile 0 +rateprofile 0 +indiprofile 0 +positionprofile 0 + + +## ----- DANGERZONE + +# catapult +set catapult_target_altitude = 350 +set catapult_target_x_ned = 0 +set catapult_target_y_ned = 0 +set catapult_rotation_roll = 300 +set catapult_rotation_pitch = 300 +set catapult_rotation_yaw = 100 +set catapult_rotation_randomize = 0 +set catapult_rotation_time = 150 +set catapult_upwards_accel = 20 + +# throw to arm +set throw_to_arm_acc_high = 20 +set throw_to_arm_acc_clip = 45 +set throw_to_arm_acc_low = 20 +set throw_to_arm_gyro_high = 600 +set throw_to_arm_momentum_thresh = 400 +set throw_to_arm_release_delay_ms = 150 +set throw_to_arm_idle_before_throw = 0 +set throw_to_arm_allow_manual_modes = 0 + +# learner 0: none, 1: learn during flight, 2: query and learn after catapult, 4: idem. after throw +set learner_mode = 6 +set learner_num_actuators = 4 +set learner_delay_time_ms = 150 +set learner_step_time_ms = 50 +set learner_ramp_time_ms = 100 +set learner_overlap_time_ms = 50 +set learner_step_amplitude = 35 +set learner_ramp_amplitude = 70 +set learner_gyro_max = 1150 +set learner_imu_lowpass_hz = 10 +set learner_fx_lowpass_hz = 20 +set learner_motor_lowpass_hz = 40 +set learner_zeta_rate = 90 +set learner_zeta_attitude = 80 +set learner_zeta_velocity = 70 +set learner_zeta_position = 90 + +set learner_roll_misalignment = 0 +set learner_pitch_misalignment = 0 +set learner_yaw_misalignment = 0 +set learner_randomize_misalignment = 0 + +set learner_apply_indi_profile = 1 +set learner_apply_position_profile = 1 +set learner_apply_hover_rotation = 0 + + +## ----- ONSCREEN DISPLAY + +set osd_rssi_pos = 2080 +set osd_ah_sbar_pos = 2286 +set osd_ah_pos = 2158 +set osd_mah_drawn_pos = 2528 +set osd_avg_cell_voltage_pos = 2552 +set osd_rtc_date_time_pos = 2091 +set osd_log_status_pos = 2540 +set osd_canvas_width = 30 +set osd_canvas_height = 13 + + +## ----- LED STRIP, BEEPER, BEACON +# led +# ledstrip + +# beeper +beeper -GYRO_CALIBRATED +beeper -RX_LOST_LANDING +beeper -DISARMING +beeper -ARMING +beeper -ARMING_GPS_FIX +beeper -GPS_STATUS +#beeper -RX_SET +beeper -ACC_CALIBRATION +beeper -ACC_CALIBRATION_FAIL +beeper -DISARM_REPEAT +beeper -ARMED +beeper -SYSTEM_INIT +beeper -ON_USB +beeper -BLACKBOX_ERASE +beeper -CRASH_FLIP +beeper -CAM_CONNECTION_OPEN +beeper -CAM_CONNECTION_CLOSE +beeper -RC_SMOOTHING_INIT_FAIL + +# beacon +beacon -RX_LOST +beacon -RX_SET diff --git a/configs/profiles/CineRatRobustGainsTest.txt b/configs/profiles/CineRatRobustGainsTest.txt new file mode 100644 index 000000000..bb410182c --- /dev/null +++ b/configs/profiles/CineRatRobustGainsTest.txt @@ -0,0 +1,520 @@ +# This file contains build flags and configuration for the CineRat 3inch drone + +############### +# BUILD FLAGS # +############### + +## ----- RC components +#define USE_SERIALRX +#define USE_SERIALRX_SBUS +#define USE_SERIALRX_CRSF + +#define USE_TELEMETRY +#define USE_TELEMETRY_SBUS +#define USE_TELEMETRY_CRSF +#define USE_TELEMETRY_PI + +## ----- Other features +#define USE_GPS +#define USE_GEOFENCE + +## ----- FPV features: LED and onscreen display +## #define USE_LED_STRIP +#define USE_OSD +#define USE_OSD_SD +#define USE_OSD_HD + +## ----- Outputs and logging +#define USE_DSHOT +## #define USE_SERVOS +## #define USE_ACTUATORS_T4 +#define USE_BLACKBOX + +## ----- Developer features +## #define USE_BENCHMARK +## #define USE_STACK_CHECK +## #define USE_CLI_DEBUG_PRINT +#define PI_STATS ## needs USE_TELEMETRY_PI +#define PI_USE_PRINT_MSGS + +## ----- INDIFLIGHT features +#define USE_INDI +#define USE_EKF +#define USE_LOCAL_POSITION +#define USE_TRAJECTORY_TRACKER + +## ----- DANGERZONE +#define USE_THROW_TO_ARM +#define USE_CATAPULT +#define USE_LEARNER +## #define USE_NN_CONTROL +#define INJECT_ATTITUDE_SETPOINTS +#define INJECT_INPUT_DISTURBANCE +#define LEARNER_PARAM_OVERRIDE + +################################################ +# CRAFT SETTINGS (formerly known as "profile") # +################################################ + +## ----- PERIPHERALS + +# feature +feature GPS +feature TELEMETRY +#feature LED_STRIP +feature OSD + +# gps +set gps_provider = UBLOX +set gps_auto_baud = ON +set gps_ublox_use_galileo = ON +set gps_update_rate_hz = 15 + +# geofence +geofence clear + +# serial ports +serial 5 64 115200 57600 0 115200 +serial 1 262144 115200 57600 0 115200 +#serial 3 2 115200 115200 0 115200 # GPS +serial 6 524288 115200 57600 0 115200 + +# rx +set serialrx_provider = SBUS + +# map +#map TAER1234 # todo: which one is it here? + +# switch configs +aux 0 0 1 1700 2100 0 0 +aux 1 1 3 1300 2100 0 0 +aux 2 2 3 900 1275 0 0 +aux 3 55 0 1700 2100 0 0 +aux 4 57 6 1300 1700 0 0 +aux 5 58 3 1700 2100 0 0 +aux 6 61 7 1700 2100 0 0 +aux 7 36 2 1700 2100 0 0 +aux 8 56 6 1700 2100 0 0 + + +# blackbox config +set blackbox_disable_pids = ON +set blackbox_disable_setpoint = ON +set blackbox_disable_alt = OFF +set blackbox_disable_bat = OFF +set blackbox_disable_rssi = OFF +set blackbox_disable_debug = OFF +set blackbox_disable_indi = OFF +set blackbox_disable_pos = OFF +set blackbox_disable_ekf = OFF +set blackbox_disable_learner = ON +set blackbox_high_resolution = ON +set debug_mode = BARO + +# sensor calibration / alignment +set align_board_roll = 0 +set align_board_pitch = 0 +set align_board_yaw = 0 +set acc_calibration = 0,0,0,1 +set acc_offset = -10, -12, 8 + +# motor setup +set dshot_bidir = ON +set dshot_idle_value = 550 +set motor_pwm_protocol = DSHOT600 +set motor_poles = 12 +set motor_output_reordering = 0,1,2,3,4,5,6,7 + +# failsafes +set failsafe_delay = 5 +set small_angle = 20 # angle at which to deny arming + + +## ----- FLIGHT PERFORMANCE + +# scheduling +set pid_process_denom = 4 # relative to imu +set ahrs_process_denom = 4 # relative to imu +set nn_rate_denom = 2 # relative to pid +set blackbox_sample_rate = 1/4 # relative to pid + +# ekf (these are the defaults). source 0: pi, 1: uros, 2: gps +set ekf_use_quat = 1 +set ekf_use_for_manual_flight = 0 +set ekf_meas_source = 0 +set ekf_proc_noise_acc = 500000, 500000, 500000 +set ekf_proc_noise_gyro = 100000, 100000, 100000 +set ekf_proc_noise_acc_bias = 100, 100, 100 +set ekf_proc_noise_gyro_bias = 10, 10, 10 +set ekf_meas_noise_position = 1000, 1000, 1000 +#set ekf_meas_noise_position = 5000, 5000, 40000 +set ekf_meas_noise_quat = 50000, 50000, 50000, 50000 +set ekf_meas_delay = 0 + +# filter setups +set gyro_lpf1_static_hz = 100 +set gyro_lpf2_static_hz = 0 +set dyn_notch_count = 0 +set dyn_notch_q = 500 +set gyro_lpf1_dyn_min_hz = 0 +set gyro_lpf1_dyn_max_hz = 250 +set acc_lpf_hz = 50 +set simplified_gyro_filter_multiplier = 40 +set rpm_filter_min_hz = 60 + +profile 0 + +set dterm_lpf1_dyn_min_hz = 0 +set dterm_lpf1_dyn_max_hz = 75 +set dterm_lpf1_static_hz = 0 +set dterm_lpf2_static_hz = 0 +set yaw_lowpass_hz = 0 +set thrust_linear = 55 +set simplified_dterm_filter = OFF + + +## ----- RATE PROFILES + +rateprofile 0 + +set rateprofile_name = velocidrone +set thr_mid = 50 +set thr_expo = 0 +set rates_type = ACTUAL +set roll_rc_rate = 100 +set pitch_rc_rate = 100 +set yaw_rc_rate = 100 +set roll_expo = 1 +set pitch_expo = 1 +set yaw_expo = 1 +set roll_srate = 70 +set pitch_srate = 70 +set yaw_srate = 70 +set throttle_limit_type = OFF +set throttle_limit_percent = 100 +set roll_rate_limit = 1998 +set pitch_rate_limit = 1998 +set yaw_rate_limit = 1998 + + +## ----- INDI ATTITUDE CONTROLLER + +indiprofile 0 + +# --- attitude control config and limits --- +# Attitude: (deg/s/s) / (deg) * 10 Rate: (deg/s/s) / (deg/s)*10 +# set indi_attitude_gains = 1079, 1079, 1079 +# set indi_rate_gains = 166, 166, 166 +set indi_attitude_gains = 1800, 1800, 1800 +set indi_rate_gains = 198, 198, 198 + +## Gain-scheduling +set indi_use_gain_scheduling = 1 # 1 yes 0 no +set indi_gain_scheduling_type = 0 # 0: 1D Interp, 1: 2D Interp, 2: 1D Poly, 3: 2D Poly +set indi_gain_schedule_ff = 0 # 1 yes 0 no + +# --- feedforward control config +# H(z) = (b0 + b1*z^-1 + b2*z^-2) / (1 + a1*z^-1 + a2*z^-2) +# Coefficient: unitless * 1000000 +# Coefficient order: b0, b1, b2, a1, a2 +# set indi_ff_coefs = 0, 0, 0, 0, 0 +# set indi_ff_coefs = 3676000, -3676000, 0, -991300, 0 +set indi_ff_coefs = 0, 0, 0, 0, 0 + +set indi_use_ff_filter = 1 # 1 use Filter instead of injection feedforward 0 use injection instead of filter feedforward + +# H(s) = K * (s - z) / (s - p) +# Coefficient: unitless * 1000000 +# Coefficient order: K, z, p +set indi_ff_filter_coefs = 1303600, -8300629, -10820700 + +# attitude controller limits: (deg/s) +set indi_attitude_max_tilt_rate = 800 +set indi_attitude_max_yaw_rate = 500 + +# --- manual flight config and limits --- +set indi_manual_max_upwards_accel = 30 # N/kg (or 255 to set to max possible) +set indi_manual_max_tilt = 30 # deg +set indi_manual_use_coordinated_yaw = 1 + +# H(z) = (b0 + b1*z^-1 + b2*z^-2) / (1 + a1*z^-1 + a2*z^-2) +# Coefficient: unitless * 1000000 +# Coefficient order: b0, b1, b2, a1, a2 +set indi_use_attitude_lead_lag = 0 # 1 yes 0 no +# set indi_attitude_coefs = 14320000,-14250000,0,-990600,0 +# set indi_rate_coefs = 15000000,-14890000,0,-989800,0 +set indi_attitude_coefs = 0,-0,0,-0,0 +set indi_rate_coefs = 0,-0,0,0,0 +#set indi_attitude_coefs = 7300000,0,0,0,0 +#set indi_rate_coefs = 18000000,0,0,0,0 + +# --- indi config --- +set indi_use_increment = 1 +set indi_use_accel_for_thrust = 1 +set indi_use_rpm_dot_feedback = 1 +set indi_act_num = 4 +set indi_sync_lowpass_hz = 15 + +# fx, fy, fz, roll, pitch, yaw +set indi_wls_axes_weights = 1,1,50,50,50,5 + +# --- actuators: fields can have up to 8 elements --- +set indi_act_is_motor = 1,1,1,1,0,0,0,0 +set indi_act_is_servo = 0,0,0,0,0,0,0,0 +set indi_act_time_constant_ms = 28, 28, 28, 28 # ms +set indi_act_max_rpm = 40000, 40000, 40000, 40000 # rpm +set indi_act_hover_rpm = 20000, 20000, 20000, 20000 # rpm +set indi_act_nonlinearity = 50, 50, 50, 50 # % +set indi_act_min = 0, 0, 0, 0 # % will be forced 0 for motors anyway +set indi_act_max = 100, 100, 100, 100 # % +set indi_wls_act_preferred_state = 0,0,0,0 +set indi_wls_act_penalties = 1,1,1,1 + +# lead lag filtering for motor Outputs +set indi_use_motor_lead_lag = 0 +set indi_act_time_constant_true_ms = 28,28,28,28 +set indi_act_time_constant_des_ms = 28,28,28,28 + +# fx, fy, fz is N/kg / 1 * 100. Roll, pitch is Nm / (kgm^2) / 1 * 10. Yaw is Nm / (kgm^2) / 1 * 10 +set indi_act_g1_fx = 0,0,0,0 +set indi_act_g1_fy = 0,0,0,0 +set indi_act_g1_fz = -1050, -1050, -1050, -1050 + +set indi_act_g1_roll = -4000, -4000, 4000, 4000 +set indi_act_g1_pitch = -2600, 2600, -2600, 2600 +set indi_act_g1_yaw = -510, 510, 510, -510 +# set indi_act_g1_roll = -3200, -3200, 3200, 3200 +# set indi_act_g1_pitch = -2080, 2080, -2080, 2080 +# set indi_act_g1_yaw = -408, 408, 408, -408 +# set indi_act_g1_roll = -4800, -4800, 4800, 4800 +# set indi_act_g1_pitch = -3120, 3120, -3120, 3120 +# set indi_act_g1_yaw = -612, 612, 612, -612 + +# Nm / (kgm^2) / (rad/s/s) * 1e5 +set indi_act_g2_roll = 0,0,0,0 +set indi_act_g2_pitch = 0,0,0,0 +set indi_act_g2_yaw = -100, 100, 100, -100 + +# Attitude setpoint injection +set indi_inject_attitude_setpoints = 0 # 0: No 1: Yes +set indi_att_sp_injection_type = 0 # 0: None, 1: Impulse, 2: Doublet, 3: Step +set indi_att_sp_injection_amplitude = 0 # amplitude * 10 +set indi_att_sp_injection_duration = 1000 # signal duration in ms +set indi_att_sp_injection_duration_down = 750 # signal duration in ms for down step (doublet only) + +# Input disturbance +# Inject a constant step disturbance on certain motors +set indi_inject_input_dist = 0 # 0: No 1: Yes +set indi_input_dist_amplitude = 0 # Disturbance amplitude * 100 +set indi_apply_dist_to_motor = 0,0,0,0,0,0,0,0 # Which motors to apply an input disturbance + +indiprofile 2 + +# indiprofile 2 --> these are the settings for the learned profile +# commented will be overwritten by the learner, so they dont need to be set +# as the initial condition is currently always zero. This may be changed in the future. +# the default values (values not set here) are the same as for all other profiles + +# these values are slightly more conservative than indiprofile 0 + +set indi_act_num = 4 +# set indi_attitude_gains = 1552,1552,1552 +# set indi_rate_gains = 174,174,174 +set indi_attitude_max_tilt_rate = 500 +set indi_attitude_max_yaw_rate = 300 +set indi_manual_use_coordinated_yaw = 1 +set indi_manual_max_upwards_accel = 20 +set indi_act_is_motor = 1,1,1,1,0,0,0,0 +set indi_act_is_servo = 0,0,0,0,0,0,0,0 +set indi_act_time_constant_ms = 25, 25, 25, 25 # ms +set indi_act_max_rpm = 40000, 40000, 40000, 40000 # rpm +set indi_act_hover_rpm = 20000, 20000, 20000, 20000 # rpm +set indi_act_nonlinearity = 50, 50, 50, 50 # % +set indi_act_min = 0, 0, 0, 0 # % will be forced 0 for motors anyway +set indi_act_max = 100, 100, 100, 100 # % +set indi_wls_act_preferred_state = 0,0,0,0 +set indi_wls_act_penalties = 1,1,1,1 + +# -- roll -56, pitch 81, yaw 170 +#set indi_act_g1_fx = -1037, -1037, -1037, -1037 +#set indi_act_g1_fy = -136, -136, -136, -136 +#set indi_act_g1_fz = -91, -91, -91, -91 +#set indi_act_g1_roll = 183, 1049, -41, -1190 +#set indi_act_g1_pitch = -2617, -4609, 4742, 2485 +#set indi_act_g1_yaw = -4016, 816, -727, 3927 +#set indi_act_g2_roll = -98, 98, 98, -98 +#set indi_act_g2_pitch = -12, 12, 12, -12 +#set indi_act_g2_yaw = -8, 8, 8, -8 + +# --- indi config --- +set indi_use_increment = 1 +set indi_use_accel_for_thrust = 1 +set indi_use_rpm_dot_feedback = 1 +set indi_act_num = 4 +set indi_sync_lowpass_hz = 15 + +# fx, fy, fz, roll, pitch, yaw +set indi_wls_axes_weights = 1,1,50,50,50,5 +set indi_wls_act_penalties = 1,1,1,1 + +## Gain-scheduling +set indi_use_gain_scheduling = 1 # 1 yes 0 no +set indi_gain_scheduling_type = 0 # 0: 1D Interp, 1: 2D Interp, 2: 1D Poly, 3: 2D Poly +set indi_gain_schedule_ff = 1 # 1 yes 0 no +set indi_use_ff_filter = 1 # 1 yes 0 no + +set indi_inject_attitude_setpoints = 0 # 0: No 1: Yes +set indi_att_sp_injection_amplitude = 0 # amplitude * 10 + +## ----- POSITION CONTROL + +positionprofile 0 + +# gains * 10 +set position_horizontal_p = 30 +set position_horizontal_i = 2 +set position_horizontal_d = 30 +set position_vertical_p = 50 +set position_vertical_i = 5 +set position_vertical_d = 40 + +# max velocities in cm/s and cm/s^2 +set position_max_horizontal_speed = 2000 +set position_max_horizontal_accel = 10000 +set position_max_upwards_speed = 500 +set position_max_downwards_speed = 500 +set position_max_upwards_accel = 2000 +set position_max_downwards_accel = 800 + +# max tilt, yaw gain * 10, thrust config +set position_max_tilt = 180 +set position_yaw_p = 50 +set position_use_thrust_attenuation = 1 + + +positionprofile 2 + +# positionprofile 2 --> these are the settings for the learned profile +# commented will be overwritten by the learner, so they dont need to be set +# as the initial condition is currently always zero. This may be changed in the future. +# the default values (values not set here) are the same as for all other profiles + +# these values are more conservative than positionprofile 0 + +# set position_horizontal_p = 63 +# set position_horizontal_i = 4 +# set position_horizontal_d = 45 +set position_max_horizontal_speed = 500 +set position_max_horizontal_accel = 1000 +set position_max_tilt = 40 +# set position_vertical_p = 63 +# set position_vertical_i = 4 +# set position_vertical_d = 45 +set position_max_upwards_speed = 100 +set position_max_downwards_speed = 100 +set position_max_upwards_accel = 2500 +set position_max_downwards_accel = 500 +# set position_yaw_p = 44 +set position_weathervane_p = 0 +set position_use_thrust_attenuation = 1 + + +## ----- SET DEFAULT PROFILES + +profile 0 +rateprofile 0 +indiprofile 0 +positionprofile 0 + + +## ----- DANGERZONE + +# catapult +set catapult_target_altitude = 350 +set catapult_target_x_ned = 0 +set catapult_target_y_ned = 0 +set catapult_rotation_roll = 300 +set catapult_rotation_pitch = 300 +set catapult_rotation_yaw = 100 +set catapult_rotation_randomize = 0 +set catapult_rotation_time = 150 +set catapult_upwards_accel = 20 + +# throw to arm +set throw_to_arm_acc_high = 20 +set throw_to_arm_acc_clip = 45 +set throw_to_arm_acc_low = 20 +set throw_to_arm_gyro_high = 600 +set throw_to_arm_momentum_thresh = 400 +set throw_to_arm_release_delay_ms = 150 +set throw_to_arm_idle_before_throw = 0 +set throw_to_arm_allow_manual_modes = 0 + +# learner 0: none, 1: learn during flight, 2: query and learn after catapult, 4: idem. after throw +set learner_mode = 6 +set learner_num_actuators = 4 +set learner_delay_time_ms = 150 +set learner_step_time_ms = 50 +set learner_ramp_time_ms = 100 +set learner_overlap_time_ms = 50 +set learner_step_amplitude = 35 +set learner_ramp_amplitude = 70 +set learner_gyro_max = 1150 +set learner_imu_lowpass_hz = 10 +set learner_fx_lowpass_hz = 20 +set learner_motor_lowpass_hz = 40 +set learner_zeta_rate = 90 +set learner_zeta_attitude = 80 +set learner_zeta_velocity = 70 +set learner_zeta_position = 90 + +set learner_roll_misalignment = 0 +set learner_pitch_misalignment = 0 +set learner_yaw_misalignment = 0 +set learner_randomize_misalignment = 0 + +set learner_apply_indi_profile = 1 +set learner_apply_position_profile = 1 +set learner_apply_hover_rotation = 0 + + +## ----- ONSCREEN DISPLAY + +set osd_rssi_pos = 2080 +set osd_ah_sbar_pos = 2286 +set osd_ah_pos = 2158 +set osd_mah_drawn_pos = 2528 +set osd_avg_cell_voltage_pos = 2552 +set osd_rtc_date_time_pos = 2091 +set osd_log_status_pos = 2540 +set osd_canvas_width = 30 +set osd_canvas_height = 13 + + +## ----- LED STRIP, BEEPER, BEACON +# led +# ledstrip + +# beeper +beeper -GYRO_CALIBRATED +beeper -RX_LOST_LANDING +beeper -DISARMING +beeper -ARMING +beeper -ARMING_GPS_FIX +beeper -GPS_STATUS +#beeper -RX_SET +beeper -ACC_CALIBRATION +beeper -ACC_CALIBRATION_FAIL +beeper -DISARM_REPEAT +beeper -ARMED +beeper -SYSTEM_INIT +beeper -ON_USB +beeper -BLACKBOX_ERASE +beeper -CRASH_FLIP +beeper -CAM_CONNECTION_OPEN +beeper -CAM_CONNECTION_CLOSE +beeper -RC_SMOOTHING_INIT_FAIL + +# beacon +beacon -RX_LOST +beacon -RX_SET diff --git a/configs/profiles/Meteor75.txt b/configs/profiles/Meteor75.txt index ebd5f4364..c6a10f046 100644 --- a/configs/profiles/Meteor75.txt +++ b/configs/profiles/Meteor75.txt @@ -41,7 +41,9 @@ ## #define USE_CATAPULT #define USE_LEARNER ## #define USE_NN_CONTROL - +#define INJECT_ATTITUDE_SETPOINTS +## #define INJECT_INPUT_DISTURBANCE +#define LEARNER_PARAM_OVERRIDE ################################################ # CRAFT SETTINGS (formerly known as "profile") # @@ -63,29 +65,47 @@ serial 1 64 115200 57600 0 115200 # RX set serialrx_provider = CRSF # map -map TAER1234 +map AETR1234 -# switch configs (Boxer i believe) -aux 0 0 0 900 1300 0 0 -aux 1 1 2 900 1700 0 0 -aux 2 58 2 900 1300 0 0 -aux 3 36 5 1700 2100 0 0 -aux 4 56 1 900 1300 0 0 -aux 5 49 4 1700 2100 0 0 +# switch configs (Radiomaster Pocket demo remote) +aux 0 0 1 1800 2100 0 0 +aux 1 1 3 1200 2100 0 0 +aux 2 2 2 1800 2100 0 0 +aux 3 58 3 1800 2100 0 0 +aux 4 56 0 1700 2100 0 0 # blackbox config +#set blackbox_disable_pids = ON +#set blackbox_disable_setpoint = ON +#set blackbox_disable_alt = ON +#set blackbox_disable_bat = OFF +#set blackbox_disable_rssi = ON +#set blackbox_disable_debug = ON +#set blackbox_disable_indi = OFF +#set blackbox_disable_pos = ON +#set blackbox_disable_ekf = ON +#set blackbox_disable_learner = OFF +#set blackbox_high_resolution = ON +#set debug_mode = NONE + +set blackbox_sample_rate = 1/4 +set blackbox_device = SPIFLASH set blackbox_disable_pids = ON +set blackbox_disable_rc = OFF set blackbox_disable_setpoint = ON -set blackbox_disable_alt = ON set blackbox_disable_bat = OFF -set blackbox_disable_rssi = OFF +set blackbox_disable_alt = ON +set blackbox_disable_rssi = ON +set blackbox_disable_gyro = OFF +set blackbox_disable_acc = OFF set blackbox_disable_debug = ON +set blackbox_disable_motors = OFF +set blackbox_disable_gps = ON set blackbox_disable_indi = OFF -set blackbox_disable_pos = ON -set blackbox_disable_ekf = ON set blackbox_disable_learner = OFF +set blackbox_mode = NORMAL set blackbox_high_resolution = ON -set debug_mode = NONE + # sensor calibration / alignment set align_board_roll = 0 @@ -109,10 +129,15 @@ set small_angle = 45 # angle at which to deny arming ## ----- FLIGHT PERFORMANCE # scheduling -set pid_process_denom = 3 # relative to imu -set ahrs_process_denom = 9 # relative to imu -set nn_rate_denom = 2 # relative to pid -set blackbox_sample_rate = 1/4 # relative to pid +# set pid_process_denom = 5 # relative to imu (this is 640Hz) +# set ahrs_process_denom = 10 # relative to imu (320Hz) +# set nn_rate_denom = 2 # relative to pid (320Hz) +# set blackbox_sample_rate = 1/1 # relative to pid (640Hz) + +set pid_process_denom = 2 # relative to imu (this is 1600Hz) +set ahrs_process_denom = 10 # relative to imu (320Hz) +set nn_rate_denom = 5 # relative to pid (320Hz) +# set blackbox_sample_rate = 1/2 # relative to pid (800Hz) # ekf (these are the defaults) set ekf_use_quat = 1 @@ -136,6 +161,7 @@ set gyro_lpf1_dyn_min_hz = 0 set gyro_lpf1_dyn_max_hz = 250 set acc_lpf_hz = 50 set simplified_gyro_filter_multiplier = 40 +set rpm_filter_harmonics = 0 set rpm_filter_min_hz = 60 profile 0 @@ -179,8 +205,61 @@ indiprofile 0 # --- attitude control config and limits --- # Attitude: (deg/s/s) / (deg) * 10 Rate: (deg/s/s) / (deg/s)*10 -set indi_attitude_gains = 1500, 1500, 1000 -set indi_rate_gains = 150, 150, 150 +# set indi_attitude_gains = 1358, 1358, 1358 +# set indi_rate_gains = 183, 183, 183 + +## Test 310725 settings +# set indi_attitude_gains = 748, 748, 748 +# set indi_rate_gains = 142, 142, 142 + +# set indi_attitude_gains = 749, 749, 749 +# set indi_rate_gains = 141, 141, 141 + +## Test 250825 settings +#set indi_attitude_gains = 749, 749, 749 +#set indi_rate_gains = 141, 141, 141 + +set indi_attitude_gains = 660, 660, 660 +set indi_rate_gains = 132, 132, 132 +#set indi_attitude_gains = 1830, 1830, 1830 +#set indi_rate_gains = 180, 180, 100 + +## Gain-scheduling (CURRENTLY DOES NOT DO ANYTHING HERE. Should be changed in INDIPROFILE 2) +set indi_use_gain_scheduling = 1 # 1 yes 0 no +set indi_gain_scheduling_type = 0 # 0: 1D Interp, 1: 2D Interp, 2: 1D Poly, 3: 2D Poly +set indi_gain_schedule_ff = 0 # 1 yes 0 no + +# H(z) = (b0 + b1*z^-1 + b2*z^-2) / (1 + a1*z^-1 + a2*z^-2) +# Coefficient: unitless * 1000000 +# Coefficient order: b0, b1, b2, a1, a2 +set indi_use_attitude_lead_lag = 0 # 1 yes 0 no +# set indi_attitude_coefs = 14320000,-14250000,0,-990600,0 +# set indi_rate_coefs = 15000000,-14890000,0,-989800,0 +# set indi_attitude_coefs = 30860000,-30590000,0,-969400,0 +# set indi_rate_coefs = 11380000,-10680000,0,-924500,0 +#set indi_attitude_coefs = 7300000,0,0,0,0 +#set indi_rate_coefs = 18000000,0,0,0,0 +set indi_attitude_coefs = 0,0,0,0,0 +set indi_rate_coefs = 0,0,0,0,0 + +# --- feedforward control config +# H(z) = (b0 + b1*z^-1 + b2*z^-2) / (1 + a1*z^-1 + a2*z^-2) +# Coefficient: unitless * 1000000 +# Coefficient order: b0, b1, b2, a1, a2 +## ff used 310725 +# set indi_ff_coefs = 2669000, -2669000, 0, -992000, 0 + +# 250825 settings +set indi_ff_coefs = 0,0,0,0,0 +# set indi_ff_coefs = 2778000, -2778000, 0, -991400, 0 +# set indi_ff_coefs = 291700, -291700, 0, -1949000, 950000 +# set indi_ff_coefs = 10840000, -10840000, 0, -967400, 0 +set indi_use_ff_filter = 0 # 1 use Filter instead of injection feedforward 0 use injection instead of filter feedforward + +# H(s) = K * (s - z) / (s - p) +# Coefficient: unitless * 1000000 +# Coefficient order: K, z, p +set indi_ff_filter_coefs = 1185600, -5011300, -9768400 # attitude controller limits: (deg/s) set indi_attitude_max_tilt_rate = 800 @@ -188,13 +267,13 @@ set indi_attitude_max_yaw_rate = 500 # --- manual flight config and limits --- set indi_manual_max_upwards_accel = 40 # N/kg (or 255 to set to max possible) -set indi_manual_max_tilt = 55 # deg +set indi_manual_max_tilt = 25 # deg set indi_manual_use_coordinated_yaw = 1 # --- indi config --- set indi_use_increment = 1 set indi_use_accel_for_thrust = 1 -set indi_use_rpm_dot_feedback = 1 +set indi_use_rpm_dot_feedback = 0 set indi_act_num = 4 set indi_sync_lowpass_hz = 15 @@ -204,29 +283,60 @@ set indi_wls_axes_weights = 1,1,50,50,50,5 # --- actuators: fields can have up to 8 elements --- set indi_act_is_motor = 1,1,1,1,0,0,0,0 set indi_act_is_servo = 0,0,0,0,0,0,0,0 -set indi_act_time_constant_ms = 35, 35, 35, 35 # ms -set indi_act_max_rpm = 80000, 80000, 80000, 80000 # rpm -set indi_act_hover_rpm = 40000, 40000, 40000, 40000 # rpm -set indi_act_nonlinearity = 50, 50, 50, 50 # % -set indi_act_min = 0, 0, 0, 0 # % will be forced 0 for motors anyway -set indi_act_max = 100, 100, 100, 100 # % +set indi_act_time_constant_ms = 37,37,37,37,25,25,25,25 +set indi_act_max_rpm = 70000,70000,70000,70000,40000,40000,40000,40000 +set indi_act_hover_rpm = 35000,35000,35000,35000,20000,20000,20000,20000 +set indi_act_nonlinearity = 50,50,50,50,50,50,50,50 +set indi_act_min = 0,0,0,0,0,0,0,0 +set indi_act_max = 100,100,100,100,100,100,100,100 set indi_wls_act_preferred_state = 0,0,0,0 set indi_wls_act_penalties = 1,1,1,1 +# lead lag filtering for motor Outputs +set indi_use_motor_lead_lag = 0 +set indi_act_time_constant_true_ms = 33,33,33,33 +set indi_act_time_constant_des_ms = 38,38,38,38 + # fx, fy, fz is N/kg / 1 * 100. Roll, pitch is Nm / (kgm^2) / 1 * 10. Yaw is Nm / (kgm^2) / 1 * 10 -set indi_act_g1_fx = 0,0,0,0 -set indi_act_g1_fy = 0,0,0,0 -set indi_act_g1_fz = -1000, -1000, -1000, -1000 -set indi_act_g1_roll = -6000, -6000, 6000, 6000 -set indi_act_g1_pitch = -5000, 5000, -5000, 5000 -set indi_act_g1_yaw = 1510, -1510, -1510, 1510 +set indi_act_g1_fx = 0,0,0,0,0,0,0,0 +set indi_act_g1_fy = 0,0,0,0,0,0,0,0 +set indi_act_g1_fz = -1770,-1770,-1770,-1770,0,0,0,0 +#set indi_act_g1_roll = -6000,-6000,6000,6000,0,0,0,0 +#set indi_act_g1_pitch = -4800,4800,-4800,4800,0,0,0,0 +#set indi_act_g1_yaw = 1120,-1120,-1120,1120,0,0,0,0 +#set indi_act_g1_roll = -9000,-9000,9000,9000,0,0,0,0 +#set indi_act_g1_pitch = -7200,7200,-7200,7200,0,0,0,0 +#set indi_act_g1_yaw = 1680,-1680,-1680,1680,0,0,0,0 +set indi_act_g1_roll = -7500,-7500,7500,7500,0,0,0,0 +set indi_act_g1_pitch = -6000,6000,-6000,6000,0,0,0,0 +set indi_act_g1_yaw = 1400,-1400,-1400,1400,0,0,0,0 +#set indi_act_g1_roll = -19000, -12000, 35652, 16994 +#set indi_act_g1_pitch = -20873, 23169, -16236, 16842 +#set indi_act_g1_yaw = 2292, -4919, -1376, -3319 + -# Nm / (kgm^2) / (rad/s/s) * 1e5 -set indi_act_g2_roll = 0,0,0,0 -set indi_act_g2_pitch = 0,0,0,0 -set indi_act_g2_yaw = 110, -110, -110, 110 + +# Nm / (kgm^2) / (rad/s/s) * 1e5 +set indi_act_g2_roll = 0,0,0,0,0,0,0,0 +set indi_act_g2_pitch = 0,0,0,0,0,0,0,0 +set indi_act_g2_yaw = 120,-120,-120,120,0,0,0,0 +# set indi_act_g2_yaw = 0,0,0,0,0,0,0,0 + +# Attitude setpoint injection +set indi_inject_attitude_setpoints = 0 # 0: No 1: Yes +set indi_att_sp_injection_type = 2 # 0: None, 1: Impulse, 2: Doublet, 3: Step +set indi_att_sp_injection_amplitude = 150 # amplitude * 10 +set indi_att_sp_injection_duration = 1250 # signal duration in ms +set indi_att_sp_injection_duration_down = 500 # signal duration down (only for doublet) in ms + +# Input disturbance +# Inject a constant step disturbance on certain motors +set indi_inject_input_dist = 0 # 0: No 1: Yes +set indi_input_dist_amplitude = 0 # Disturbance amplitude * 100 +set indi_apply_dist_to_motor = 0,0,0,0,0,0,0,0 # Which motors to apply an input disturbance + indiprofile 2 # indiprofile 2 --> these are the settings for the learned profile @@ -266,6 +376,9 @@ set indi_sync_lowpass_hz = 15 set indi_wls_axes_weights = 1, 1, 50, 50, 50, 5 set indi_wls_act_penalties = 1,1,1,1 +set indi_use_gain_scheduling = 1 # 1 yes 0 no +set indi_gain_scheduling_type = 0 # 0: 1D Interp, 1: 2D Interp, 2: 1D Poly, 3: 2D Poly +set indi_gain_schedule_ff = 0 # 1 yes 0 no ## ----- POSITION CONTROL @@ -355,11 +468,11 @@ set learner_mode = 6 set learner_num_actuators = 4 set learner_delay_time_ms = 10 set learner_step_time_ms = 30 -set learner_ramp_time_ms = 60 -set learner_overlap_time_ms = 30 +set learner_ramp_time_ms = 50 +set learner_overlap_time_ms = 0 set learner_step_amplitude = 20 set learner_ramp_amplitude = 40 -set learner_gyro_max = 1800 +set learner_gyro_max = 2000 set learner_imu_lowpass_hz = 10 set learner_fx_lowpass_hz = 20 set learner_motor_lowpass_hz = 40 diff --git a/make/source.mk b/make/source.mk index b05ceb5d8..3ca45a7ff 100644 --- a/make/source.mk +++ b/make/source.mk @@ -120,6 +120,7 @@ COMMON_SRC = \ flight/nn_control.c \ flight/neural_controllers/nn_controller.c \ flight/neural_controllers/neural_network.c \ + flight/gain_schedule.c \ io/serial_4way.c \ io/serial_4way_avrootloader.c \ io/serial_4way_stk500v2.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 12159e7b2..6d1d116cc 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -300,10 +300,19 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"quatSp", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, {"quatSp", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, + {"quatSpPreFf", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, + {"quatSpPreFf", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, + {"quatSpPreFf", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, + {"quatSpPreFf", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, + {"gyroSp", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, {"gyroSp", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, {"gyroSp", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, + {"gyroFf", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, + {"gyroFf", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, + {"gyroFf", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, + {"alphaSp", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, {"alphaSp", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, {"alphaSp", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(INDI)}, @@ -702,7 +711,9 @@ typedef struct blackboxMainState_s { int16_t quat[4]; int16_t alpha[XYZ_AXIS_COUNT]; int16_t quatSp[4]; + int16_t quatSpPreFf[4]; int16_t gyroSp[XYZ_AXIS_COUNT]; + int16_t gyroFf[XYZ_AXIS_COUNT]; int16_t alphaSp[XYZ_AXIS_COUNT]; int16_t spfSp[XYZ_AXIS_COUNT]; int16_t dv[MAXV]; @@ -1127,7 +1138,9 @@ static void writeIntraframe(void) blackboxWriteSigned16VBArray(blackboxCurrent->quat, 4); blackboxWriteSigned16VBArray(blackboxCurrent->alpha, XYZ_AXIS_COUNT); blackboxWriteSigned16VBArray(blackboxCurrent->quatSp, 4); + blackboxWriteSigned16VBArray(blackboxCurrent->quatSpPreFf, 4); blackboxWriteSigned16VBArray(blackboxCurrent->gyroSp, XYZ_AXIS_COUNT); + blackboxWriteSigned16VBArray(blackboxCurrent->gyroFf, XYZ_AXIS_COUNT); blackboxWriteSigned16VBArray(blackboxCurrent->alphaSp, XYZ_AXIS_COUNT); blackboxWriteSigned16VBArray(blackboxCurrent->spfSp, XYZ_AXIS_COUNT); blackboxWriteSigned16VBArray(blackboxCurrent->dv, MAXV); @@ -1361,10 +1374,21 @@ static void writeInterframe(void) //blackboxWriteTag8_4S16(deltas); blackboxWriteSigned16VBArray(deltas16, 4); + // quat setpoints pre feedforward + arraySubInt16(deltas16, blackboxCurrent->quatSpPreFf, blackboxLast->quatSpPreFf, 4); + //for (int i=0; i < 4; i++) + // deltas[i] = deltas16[i]; + //blackboxWriteTag8_4S16(deltas); + blackboxWriteSigned16VBArray(deltas16, 4); + // rotational rate setpoint arraySubInt16(deltas16, blackboxCurrent->gyroSp, blackboxLast->gyroSp, XYZ_AXIS_COUNT); blackboxWriteSigned16VBArray(deltas16, XYZ_AXIS_COUNT); + // feedforward rotational rate setpoint + arraySubInt16(deltas16, blackboxCurrent->gyroFf, blackboxLast->gyroFf, XYZ_AXIS_COUNT); + blackboxWriteSigned16VBArray(deltas16, XYZ_AXIS_COUNT); + // rotational accel setpoint arraySubInt16(deltas16, blackboxCurrent->alphaSp, blackboxLast->alphaSp, XYZ_AXIS_COUNT); blackboxWriteSigned16VBArray(deltas16, XYZ_AXIS_COUNT); @@ -1388,7 +1412,7 @@ static void writeInterframe(void) for (int i=0; i < MAXU; i++) deltas[i] = deltas16[i]; blackboxWriteTag8_8SVB(deltas, MAXU); - + // omega arraySubUint16(deltas16, blackboxCurrent->omega, blackboxLast->omega, MAXU); for (int i=0; i < MAXU; i++) @@ -1888,9 +1912,16 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->quatSp[1] = lrintf(indiRun.attSpNed.x * UNIT_FLOAT_TO_SIGNED16VB); blackboxCurrent->quatSp[2] = lrintf(indiRun.attSpNed.y * UNIT_FLOAT_TO_SIGNED16VB); blackboxCurrent->quatSp[3] = lrintf(indiRun.attSpNed.z * UNIT_FLOAT_TO_SIGNED16VB); // FRD and not FLU + blackboxCurrent->quatSpPreFf[0] = lrintf(indiRun.attSpNedPreFeedforward.w * UNIT_FLOAT_TO_SIGNED16VB); + blackboxCurrent->quatSpPreFf[1] = lrintf(indiRun.attSpNedPreFeedforward.x * UNIT_FLOAT_TO_SIGNED16VB); + blackboxCurrent->quatSpPreFf[2] = lrintf(indiRun.attSpNedPreFeedforward.y * UNIT_FLOAT_TO_SIGNED16VB); + blackboxCurrent->quatSpPreFf[3] = lrintf(indiRun.attSpNedPreFeedforward.z * UNIT_FLOAT_TO_SIGNED16VB); // FRD and not FLU blackboxCurrent->gyroSp[0] = lrintf(RADIANS_TO_DEGREES(indiRun.rateSpBody.V.X) * blackboxHighResolutionScale); blackboxCurrent->gyroSp[1] = lrintf(RADIANS_TO_DEGREES(indiRun.rateSpBody.V.Y) * blackboxHighResolutionScale); blackboxCurrent->gyroSp[2] = lrintf(RADIANS_TO_DEGREES(indiRun.rateSpBody.V.Z) * blackboxHighResolutionScale); + blackboxCurrent->gyroFf[0] = lrintf(RADIANS_TO_DEGREES(indiRun.ffRateSpBody.V.X) * blackboxHighResolutionScale); + blackboxCurrent->gyroFf[1] = lrintf(RADIANS_TO_DEGREES(indiRun.ffRateSpBody.V.Y) * blackboxHighResolutionScale); + blackboxCurrent->gyroFf[2] = lrintf(RADIANS_TO_DEGREES(indiRun.ffRateSpBody.V.Z) * blackboxHighResolutionScale); blackboxCurrent->alphaSp[0] = lrintf(RADIANS_TO_DEGREES(indiRun.rateDotSpBody.V.X) * 0.1f); blackboxCurrent->alphaSp[1] = lrintf(RADIANS_TO_DEGREES(indiRun.rateDotSpBody.V.Y) * 0.1f); blackboxCurrent->alphaSp[2] = lrintf(RADIANS_TO_DEGREES(indiRun.rateDotSpBody.V.Z) * 0.1f); @@ -2589,7 +2620,39 @@ static bool blackboxWriteSysinfo(void) indiProfile->u_pref[1], indiProfile->u_pref[2], indiProfile->u_pref[3]); -#endif + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_INDI_USE_FF_FILTER, "%d", indiProfile->useFeedforwardFilter); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_INDI_FF_FILTER_COEFS, "%d,%d,%d", indiProfile->feedforwardFilterCoefs[0], + indiProfile->feedforwardFilterCoefs[1], + indiProfile->feedforwardFilterCoefs[2]); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_INDI_FF_COEFS, "%d,%d,%d,%d,%d", indiProfile->feedforwardCoefs[0], + indiProfile->feedforwardCoefs[1], + indiProfile->feedforwardCoefs[2], + indiProfile->feedforwardCoefs[3], + indiProfile->feedforwardCoefs[4]); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_INDI_USE_MOTOR_LEAD_LAG, "%d", indiProfile->useMotorLeadLag); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_INDI_USE_ATT_LEAD_LAG, "%d", indiProfile->useAttLeadLag); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_INDI_ATT_COEFS, "%d,%d,%d,%d,%d", indiProfile->attCoefs[0], + indiProfile->attCoefs[1], + indiProfile->attCoefs[2], + indiProfile->attCoefs[3], + indiProfile->attCoefs[4]); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_INDI_RATE_COEFS, "%d,%d,%d,%d,%d", indiProfile->rateCoefs[0], + indiProfile->rateCoefs[1], + indiProfile->rateCoefs[2], + indiProfile->rateCoefs[3], + indiProfile->rateCoefs[4]); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_INDI_USE_GAIN_SCHEDULING, "%d", indiProfile->useGainScheduling); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_INDI_GAIN_SCHEDULING_TYPE, "%d", indiProfile->gainSchedulingType); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_INDI_GAIN_SCHEDULE_FF, "%d", indiProfile->gainScheduleFf); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_INDI_ACT_TIME_CONSTANT_TRUE_MS, "%d,%d,%d,%d", indiProfile->actTimeConstTrueMs[0], + indiProfile->actTimeConstTrueMs[1], + indiProfile->actTimeConstTrueMs[2], + indiProfile->actTimeConstTrueMs[3]); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_INDI_ACT_TIME_CONSTANT_DES_MS, "%d,%d,%d,%d", indiProfile->actTimeConstDesMs[0], + indiProfile->actTimeConstDesMs[1], + indiProfile->actTimeConstDesMs[2], + indiProfile->actTimeConstDesMs[3]); +#endif // USE_INDI #ifdef USE_LOCAL_POSITION BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_HORIZONTAL_P, "%d", posProfile->horz_p); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_HORIZONTAL_I, "%d", posProfile->horz_i); diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 9b4b48990..9e4daa18d 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -568,6 +568,11 @@ static void printValuePointer(const char *cmdName, const clivalue_t *var, const // uin32_t array cliPrintf("%u", ((uint32_t *)valuePointer)[i]); break; + + case VAR_INT32: + // int32_t array + cliPrintf("%d", ((int32_t *)valuePointer)[i]); + break; } if (i < var->config.array.length - 1) { @@ -597,6 +602,10 @@ static void printValuePointer(const char *cmdName, const clivalue_t *var, const case VAR_UINT32: value = *(uint32_t *)valuePointer; + break; + case VAR_INT32: + value = *(int32_t *)valuePointer; + break; } @@ -681,6 +690,9 @@ static bool valuePtrEqualsDefault(const clivalue_t *var, const void *ptr, const case VAR_UINT32: result = result && (((uint32_t *)ptr)[i] & mask) == (((uint32_t *)ptrDefault)[i] & mask); break; + case VAR_INT32: + result = result && ((int32_t *)ptr)[i] == ((int32_t *)ptrDefault)[i]; + break; } } @@ -982,6 +994,10 @@ static void cliSetVar(const clivalue_t *var, const uint32_t value) case VAR_UINT32: *(uint32_t *)ptr = value; break; + + case VAR_INT32: + *(int32_t *)ptr = value; + break; } } } @@ -4857,6 +4873,16 @@ STATIC_UNIT_TESTED void cliSet(const char *cmdName, char *cmdline) *data = (uint32_t)strtoul((const char*) valPtr, NULL, 10); } + break; + + case VAR_INT32: + { + // fetch data pointer + int32_t *data = (int32_t *)cliGetValuePointer(val) + i; + // store value + *data = (int32_t)strtol((const char*) valPtr, NULL, 10); + } + break; } diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index ad49fdbbe..ab23193c1 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1314,7 +1314,27 @@ const clivalue_t valueTable[] = { { PARAM_NAME_INDI_TAILS_CNWD , VAR_INT16 | PROFILE_INDI_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_INDI_PROFILE, offsetof(indiProfile_t, tails_cnwd) }, { PARAM_NAME_INDI_TAILS_CXD , VAR_INT16 | PROFILE_INDI_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_INDI_PROFILE, offsetof(indiProfile_t, tails_cxd) }, { PARAM_NAME_INDI_TAILS_CMD , VAR_INT16 | PROFILE_INDI_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_INDI_PROFILE, offsetof(indiProfile_t, tails_cmd) }, - { PARAM_NAME_INDI_TAILS_CND , VAR_INT16 | PROFILE_INDI_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_INDI_PROFILE, offsetof(indiProfile_t, tails_cnd) }, + { PARAM_NAME_INDI_TAILS_CND , VAR_INT16 | PROFILE_INDI_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_INDI_PROFILE, offsetof(indiProfile_t, tails_cnd) }, + { PARAM_NAME_INDI_USE_MOTOR_LEAD_LAG, VAR_UINT8 | PROFILE_INDI_VALUE, .config.minmaxUnsigned = { 0, 1 }, PG_INDI_PROFILE, offsetof(indiProfile_t, useMotorLeadLag) }, + { PARAM_NAME_INDI_ACT_TIME_CONSTANT_TRUE_MS, VAR_UINT8 | PROFILE_INDI_VALUE | MODE_ARRAY, .config.array.length = MAXU, PG_INDI_PROFILE, offsetof(indiProfile_t, actTimeConstTrueMs) }, + { PARAM_NAME_INDI_ACT_TIME_CONSTANT_DES_MS, VAR_UINT8 | PROFILE_INDI_VALUE | MODE_ARRAY, .config.array.length = MAXU, PG_INDI_PROFILE, offsetof(indiProfile_t, actTimeConstDesMs) }, + { PARAM_NAME_INDI_FF_COEFS , VAR_INT32 | PROFILE_INDI_VALUE | MODE_ARRAY, .config.array.length = 5, PG_INDI_PROFILE, offsetof(indiProfile_t, feedforwardCoefs) }, + { PARAM_NAME_INDI_USE_FF_FILTER , VAR_UINT8 | PROFILE_INDI_VALUE, .config.minmaxUnsigned = { 0, 1 }, PG_INDI_PROFILE, offsetof(indiProfile_t, useFeedforwardFilter) }, + { PARAM_NAME_INDI_FF_FILTER_COEFS , VAR_INT32 | PROFILE_INDI_VALUE | MODE_ARRAY, .config.array.length = 3, PG_INDI_PROFILE, offsetof(indiProfile_t, feedforwardFilterCoefs) }, + { PARAM_NAME_INDI_USE_GAIN_SCHEDULING, VAR_UINT8 | PROFILE_INDI_VALUE, .config.minmaxUnsigned = { 0, 1 }, PG_INDI_PROFILE, offsetof(indiProfile_t, useGainScheduling) }, + { PARAM_NAME_INDI_GAIN_SCHEDULING_TYPE, VAR_UINT8 | PROFILE_INDI_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_INDI_PROFILE, offsetof(indiProfile_t, gainSchedulingType) }, + { PARAM_NAME_INDI_GAIN_SCHEDULE_FF, VAR_UINT8 | PROFILE_INDI_VALUE, .config.minmaxUnsigned = { 0, 1 }, PG_INDI_PROFILE, offsetof(indiProfile_t, gainScheduleFf) }, + { PARAM_NAME_INDI_ATT_SP_INJECTION_TYPE, VAR_INT8 | PROFILE_INDI_VALUE , .config.minmax = { 0, 5 }, PG_INDI_PROFILE, offsetof(indiProfile_t, attSpInjectionType) }, + { PARAM_NAME_INDI_ATT_SP_INJECTION_AMPLITUDE, VAR_INT16 | PROFILE_INDI_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_INDI_PROFILE, offsetof(indiProfile_t, attSpInjectionAmplitude) }, + { PARAM_NAME_INDI_ATT_SP_INJECTION_DURATION, VAR_INT16 | PROFILE_INDI_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_INDI_PROFILE, offsetof(indiProfile_t, attSpInjectionDuration) }, + { PARAM_NAME_INDI_ATT_SP_INJECTION_DURATION_DOWN, VAR_INT16 | PROFILE_INDI_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_INDI_PROFILE, offsetof(indiProfile_t, attSpInjectionDurationDown) }, + { PARAM_NAME_INDI_INJECT_ATT_SP, VAR_UINT8 | PROFILE_INDI_VALUE, .config.minmaxUnsigned = { 0, 1 }, PG_INDI_PROFILE, offsetof(indiProfile_t, injectAttSp) }, + { PARAM_NAME_INDI_USE_ATT_LEAD_LAG, VAR_UINT8 | PROFILE_INDI_VALUE, .config.minmaxUnsigned = { 0, 1 }, PG_INDI_PROFILE, offsetof(indiProfile_t, useAttLeadLag) }, + { PARAM_NAME_INDI_ATT_COEFS , VAR_INT32 | PROFILE_INDI_VALUE | MODE_ARRAY, .config.array.length = 5, PG_INDI_PROFILE, offsetof(indiProfile_t, attCoefs) }, + { PARAM_NAME_INDI_RATE_COEFS , VAR_INT32 | PROFILE_INDI_VALUE | MODE_ARRAY, .config.array.length = 5, PG_INDI_PROFILE, offsetof(indiProfile_t, rateCoefs) }, + { PARAM_NAME_INDI_INJECT_INPUT_DIST, VAR_UINT8 | PROFILE_INDI_VALUE, .config.minmaxUnsigned = { 0, 1 }, PG_INDI_PROFILE, offsetof(indiProfile_t, injectInputDist) }, + { PARAM_NAME_INDI_INPUT_DIST_AMPLITUDE, VAR_UINT16 | PROFILE_INDI_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_INDI_PROFILE, offsetof(indiProfile_t, inputDistAmplitude) }, + { PARAM_NAME_INDI_APPLY_DIST_TO_MOTOR, VAR_UINT8 | PROFILE_INDI_VALUE | MODE_ARRAY, .config.array.length = MAXU, PG_INDI_PROFILE, offsetof(indiProfile_t, applyDistToMotor) }, #endif #ifdef USE_LOCAL_POSITION diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h index c47921a14..ae89e167c 100644 --- a/src/main/cli/settings.h +++ b/src/main/cli/settings.h @@ -166,6 +166,7 @@ typedef enum { VAR_UINT16 = (2 << VALUE_TYPE_OFFSET), VAR_INT16 = (3 << VALUE_TYPE_OFFSET), VAR_UINT32 = (4 << VALUE_TYPE_OFFSET), + VAR_INT32 = (5 << VALUE_TYPE_OFFSET), // value section, bits 3-5 MASTER_VALUE = (0 << VALUE_SECTION_OFFSET), diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 97687857a..1fce11638 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -168,6 +168,78 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF, 1.0f); } +// Initialize a biquad filter based on chosen gain, zero and pole location +void biquadFilterInitZeroPole(biquadFilter_t *filter, float k, float z, float p, uint32_t refreshRate) +{ + /* Continuous-time filter: + G(s) = K * (s - z) / (s - p) + + Discretization using bilinear transform (s -> (2/Ts) * (1 - z^-1) / (1 + z^-1)): + Numerator: (s - z) -> + (2/Ts * (1 - z^-1) / (1 + z^-1) - z) + Denominator: (s - p) -> + (2/Ts * (1 - z^-1) / (1 + z^-1) - p) + Multiply top and bottom by (1 + z^-1): + G(z) = K * ((c - z) * z + (-c - z)) + -------------------------- + ((c - p) * z + (-c - p)) + where c = 2 / Ts, Ts = refreshRate * 1e-6 + */ + + float Ts = refreshRate * 0.000001f; + float c = 2.0f / Ts; + float b0 = k * (c - z); + float b1 = k * (-c - z); + float a0 = (c - p); + float a1 = (-c - p); + + float b0n = b0 / a0; + float b1n = b1 / a0; + float a1n = a1 / a0; + biquadFilterInitLeadLag(filter, b0n, b1n, 0.0f, a1n, 0.0f); +} + + +// Initialize lead or lag filter A_comp(z) to modify time constant of motor from tauEst [s] to tauDes [s] +// Here the desired actuator dynamics should be A_des(s) = A_est(s) * A_comp(s) where A_est(s) is the estimated +// actuator dynamics modeled as a first order lowpass filter. +void biquadFilterInitMotorLeadLag(biquadFilter_t *filter, float tauEst, float tauDes, uint32_t refreshRate) +{ + /* Compute coefficients based on motor being a first order lowpass filter A(s) with time constant tauEst: + A(s) = 1 / (tauEst * s + 1) + A_des(s) = A(s) * A_comp(s) = A(s) * (tauEst * s + 1) / (tauDes * s + 1) + Discretization of A_comp(s) using bilinear transform yields: + A_comp(z) = ((2 * tau_est / Ts) + 1) * z + (1 - (2 * tau_est / Ts)) + --------------------------------------------------------- + ((2 * tau_des / Ts) + 1) * z + (1 - (2 * tau_des / Ts)) + */ + float Ts = refreshRate * 0.000001f; + float a0 = 2 * tauDes / Ts + 1; + float a1 = 1 - 2 * tauDes / Ts; + float b0 = 2 * tauEst / Ts + 1; + float b1 = 1 - 2 * tauEst / Ts; + + float a1n = a1 / a0; + float b0n = b0 / a0; + float b1n = b1 / a0; + biquadFilterInitLeadLag(filter, b0n, b1n, 0.0f, a1n, 0.0f); +} + +/* Setups up a biquad filter as a lead-lag filter in the form H(z) = (b0 + b1*z^-1 + b2*z^-2) / (1 + a1*z^-1 + a2*z^-2)*/ +void biquadFilterInitLeadLag(biquadFilter_t *filter, float b0, float b1, float b2, float a1, float a2) +{ + filter->x1 = 0.0f; + filter->y1 = 0.0f; + filter->x2 = 0.0f; + filter->y2 = 0.0f; + filter->b0 = b0; + filter->b1 = b1; + filter->b2 = b2; + filter->a1 = a1; + filter->a2 = a2; + filter->weight = 1.0f; +} + void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight) { biquadFilterUpdate(filter, filterFreq, refreshRate, Q, filterType, weight); diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 876a55655..f5e4677c9 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -81,6 +81,9 @@ typedef float (*filterApplyFnPtr)(filter_t *filter, float input); float nullFilterApply(filter_t *filter, float input); void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); +void biquadFilterInitMotorLeadLag(biquadFilter_t *filter, float tauEst, float tauDes, uint32_t refreshRate); +void biquadFilterInitZeroPole(biquadFilter_t *filter, float k, float z, float p, uint32_t refreshRate); +void biquadFilterInitLeadLag(biquadFilter_t *filter, float b0, float b1, float b2, float a1, float a2); void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight); void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight); void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); diff --git a/src/main/common/signal_generator.c b/src/main/common/signal_generator.c new file mode 100644 index 000000000..0e867b878 --- /dev/null +++ b/src/main/common/signal_generator.c @@ -0,0 +1,103 @@ +#include "signal_generator.h" +#include + +int current_signal_mode = SIGNAL_MODE_OFF; +static float signal_amplitude = 0.0f; +static float signal_period_up = 0.0f; +static float signal_period_down = 0.0f; +static timeUs_t signal_start_time = 0; + +void set_signal_mode(signal_mode_t mode, float signalAmplitude, float signalPeriodUp, float signalPeriodDown, timeUs_t currentTimeUs) { + current_signal_mode = mode; + signal_amplitude = signalAmplitude; + signal_period_up = signalPeriodUp; + signal_period_down = signalPeriodDown; + signal_start_time = currentTimeUs; +} + +float generate_signal(timeUs_t currentTimeUs, timeUs_t signalStartTime) { + if (current_signal_mode == SIGNAL_MODE_OFF) { + return 0.0f; + } + + float elapsedTime = (currentTimeUs - signalStartTime) * 1e-6f; // Convert to seconds + + if (current_signal_mode == SIGNAL_MODE_IMPULSE) { + // Impulse signal: a single pulse at the start + // return signal_amplitude; + return (elapsedTime < signal_period_up) ? signal_amplitude : 0.0f; + } + + if (current_signal_mode == SIGNAL_MODE_DOUBLET) { + // Doublet signal + // float cycleTime = fmod(elapsedTime, signal_period); // Get time within current cycle + float cycleTime = elapsedTime; + if (cycleTime < signal_period_up) { + return signal_amplitude; // Positive pulse + } else if (cycleTime < signal_period_up + signal_period_down) { + return -signal_amplitude; // Negative pulse + } else { + return 0.0f; // No pulse + } + } + + if (current_signal_mode == SIGNAL_MODE_STEP) { + // Step signal: a step change at the start + return (elapsedTime < signal_period_up) ? signal_amplitude : 0.0f; + } + return 0.0f; // Default to no signal +} + + + +fp_quaternion_t create_step_quaternion(float angle, char axis) { + fp_quaternion_t quat = QUATERNION_INITIALIZE; + + float half_angle = angle / 2.0f; + float sin_half_angle = sin_approx(half_angle); + + switch (axis) { + case 'x': + quat.w = cos_approx(half_angle); + quat.x = sin_half_angle; + quat.y = 0.0f; + quat.z = 0.0f; + break; + case 'y': + quat.w = cos_approx(half_angle); + quat.x = 0.0f; + quat.y = sin_half_angle; + quat.z = 0.0f; + break; + case 'z': + quat.w = cos_approx(half_angle); + quat.x = 0.0f; + quat.y = 0.0f; + quat.z = sin_half_angle; + break; + default: + // Handle the error case where an invalid axis is passed. + break; + } + + return quat; +} + +// Helper function to normalize a quaternion +void normalize_quaternion(fp_quaternion_t* q) { + float norm = sqrtf(q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z); + q->w /= norm; + q->x /= norm; + q->y /= norm; + q->z /= norm; +} + +// Helper function to multiply two quaternions +fp_quaternion_t multiply_quaternions(const fp_quaternion_t* q1, const fp_quaternion_t* q2) { + fp_quaternion_t result; + result.w = q1->w * q2->w - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z; + result.x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y; + result.y = q1->w * q2->y - q1->x * q2->z + q1->y * q2->w + q1->z * q2->x; + result.z = q1->w * q2->z + q1->x * q2->y - q1->y * q2->x + q1->z * q2->w; + return result; +} diff --git a/src/main/common/signal_generator.h b/src/main/common/signal_generator.h new file mode 100644 index 000000000..b419e5d52 --- /dev/null +++ b/src/main/common/signal_generator.h @@ -0,0 +1,28 @@ +// #ifndef SIGNAL_GENERATOR_H +// #define SIGNAL_GENERATOR_H + +#include +#include "platform.h" +#include "time.h" +#include "maths.h" + +#include "common/time.h" // Assuming you use this for time management + +extern int current_signal_mode; + +typedef enum { + SIGNAL_MODE_OFF, + SIGNAL_MODE_IMPULSE, + SIGNAL_MODE_DOUBLET, + SIGNAL_MODE_STEP +} signal_mode_t; + +// Function to initialize the signal mode +void set_signal_mode(signal_mode_t mode, float signalAmplitude, float signalPeriod, float signalPeriodDown, timeUs_t currentTimeUs); + +// Function to generate the signal for the current time +float generate_signal(timeUs_t currentTimeUs, timeUs_t signalStartTime); + +fp_quaternion_t create_step_quaternion(float angle, char axis); +void normalize_quaternion(fp_quaternion_t* q); +fp_quaternion_t multiply_quaternions(const fp_quaternion_t* q1, const fp_quaternion_t* q2); diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 5f4849e5b..db23d2d4b 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -1770,6 +1770,30 @@ FAST_CODE void taskMainInnerLoop(timeUs_t currentTimeUs) indiUpdateActuatorState( motor_normalized, servo_normalized ); #endif + // apply motor lead-lag filter only after the commands are fed into the actuator state filter +#ifdef USE_INDI + if (indiRun.useMotorLeadLag) { + int m = 0; + for (int i = 0; i < indiRun.actNum; i++) { + if ((indiRun.actIsMotor[i]) && (m < numMotors)) { + float output = biquadFilterApply(&indiRun.motorLeadLagFilter[m], motor_normalized[m]); + motor_normalized[m++] = constrainf(output, 0.f, 1.f); + } + } + } +#endif +#ifdef INJECT_INPUT_DISTURBANCE +#pragma message "WARNING: compiling with dangerous code." + // Ensure this only runs when not injecting an attitude setpoint and in angle mode (disable in horizon mode) + if (indiRun.injectInputDist && !indiRun.injectAttSp && FLIGHT_MODE(ANGLE_MODE)) { + for (int i = 0; i < indiRun.actNum; i++) { + if (indiRun.actIsMotor[i] && indiRun.applyDistToMotor[i]) { + // inject disturbance into the motor output + motor_normalized[i] += indiRun.inputDistAmplitude; + } + } + } +#endif // get real motor outputs for the hardware implementation used for (int i = 0; i < numMotors; i++) { motor[i] = scaleRangef(motor_normalized[i], 0., 1., mixerRuntime.motorOutputLow, mixerRuntime.motorOutputHigh); diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 9e58c7adf..dd3714aac 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -171,6 +171,26 @@ #define PARAM_NAME_INDI_TAILS_CXD "indi_tails_cxd" #define PARAM_NAME_INDI_TAILS_CMD "indi_tails_cmd" #define PARAM_NAME_INDI_TAILS_CND "indi_tails_cnd" +#define PARAM_NAME_INDI_USE_MOTOR_LEAD_LAG "indi_use_motor_lead_lag" +#define PARAM_NAME_INDI_ACT_TIME_CONSTANT_TRUE_MS "indi_act_time_constant_true_ms" +#define PARAM_NAME_INDI_ACT_TIME_CONSTANT_DES_MS "indi_act_time_constant_des_ms" +#define PARAM_NAME_INDI_FF_COEFS "indi_ff_coefs" +#define PARAM_NAME_INDI_USE_FF_FILTER "indi_use_ff_filter" +#define PARAM_NAME_INDI_FF_FILTER_COEFS "indi_ff_filter_coefs" +#define PARAM_NAME_INDI_USE_GAIN_SCHEDULING "indi_use_gain_scheduling" +#define PARAM_NAME_INDI_GAIN_SCHEDULING_TYPE "indi_gain_scheduling_type" +#define PARAM_NAME_INDI_GAIN_SCHEDULE_FF "indi_gain_schedule_ff" +#define PARAM_NAME_INDI_ATT_SP_INJECTION_TYPE "indi_att_sp_injection_type" +#define PARAM_NAME_INDI_ATT_SP_INJECTION_AMPLITUDE "indi_att_sp_injection_amplitude" +#define PARAM_NAME_INDI_ATT_SP_INJECTION_DURATION "indi_att_sp_injection_duration" +#define PARAM_NAME_INDI_ATT_SP_INJECTION_DURATION_DOWN "indi_att_sp_injection_duration_down" +#define PARAM_NAME_INDI_INJECT_ATT_SP "indi_inject_attitude_setpoints" +#define PARAM_NAME_INDI_USE_ATT_LEAD_LAG "indi_use_attitude_lead_lag" +#define PARAM_NAME_INDI_ATT_COEFS "indi_attitude_coefs" +#define PARAM_NAME_INDI_RATE_COEFS "indi_rate_coefs" +#define PARAM_NAME_INDI_INJECT_INPUT_DIST "indi_inject_input_dist" +#define PARAM_NAME_INDI_INPUT_DIST_AMPLITUDE "indi_input_dist_amplitude" +#define PARAM_NAME_INDI_APPLY_DIST_TO_MOTOR "indi_apply_dist_to_motor" #endif #ifdef USE_LOCAL_POSITION diff --git a/src/main/flight/gain_schedule.c b/src/main/flight/gain_schedule.c new file mode 100644 index 000000000..6a3ec0883 --- /dev/null +++ b/src/main/flight/gain_schedule.c @@ -0,0 +1,184 @@ +#include +#include "gain_schedule.h" + +// 1D Polynomials +// static const float kEtaCoeffs1D[] = {2125, -297.4, 13.31}; +static const float kEtaCoeffs1D[] = {12.8, -277.6, 1942.1}; +const Poly1D kEtaPoly1D = {kEtaCoeffs1D, 2}; + +// static const float kOmegaCoeffs1D[] = {2427, -418.3, 25.47}; +static const float kOmegaCoeffs1D[] = {25.7, -426.8, 2505.3}; +const Poly1D kOmegaPoly1D = {kOmegaCoeffs1D, 2}; + +// static const float ffKCoeffs1D[] = {1811, -129.1, 3.952}; +static const float ffKCoeffs1D[] = {3.952, -129.1, 1811}; +const Poly1D ffKPoly1D = {ffKCoeffs1D, 2}; + +// static const float ffPoleCoeffs1D[] = {9819, -757.2, 25.83}; +static const float ffPoleCoeffs1D[] = {25.83, -757.2, 9819}; +const Poly1D ffPolePoly1D = {ffPoleCoeffs1D, 2}; + +// 2D Polynomials (NOT DONE YET) +static const float kEtaCoeffs2D[] = {0.5, 0.1, 0.2, 0.05}; // 0.5 + 0.1x + 0.2y + 0.05xy +const Poly2D kEtaPoly2D = {kEtaCoeffs2D, 1, 1}; + +static const float kOmegaCoeffs2D[] = {1.0, 0.05, 0.03, 0.01}; // 1.0 + 0.05x + 0.03y + 0.01xy +const Poly2D kOmegaPoly2D = {kOmegaCoeffs2D, 1, 1}; + +static const float ffKCoeffs2D[] = {2.0, 0.3, -0.1, 0.02}; // 2.0 + 0.3x - 0.1y + 0.02xy +const Poly2D ffKPoly2D = {ffKCoeffs2D, 1, 1}; + +static const float ffPoleCoeffs2D[] = {10.0, -0.5, 0.2, -0.01}; // 10.0 - 0.5x + 0.2y - 0.01xy +const Poly2D ffPolePoly2D = {ffPoleCoeffs2D, 1, 1}; + + +// 1D Gain interpolation +static const float tauAxis1D[] = { + 0.0100, 0.0124, 0.0148, 0.0172, 0.0197, 0.0221, 0.0245, 0.0269, 0.0293, 0.0317, + 0.0341, 0.0366, 0.0390, 0.0414, 0.0438, 0.0462, 0.0486, 0.0510, 0.0534, 0.0559, + 0.0583, 0.0607, 0.0631, 0.0655, 0.0679, 0.0703, 0.0728, 0.0752, 0.0776, 0.0800 +}; + +static const float kEtaGains1D[] = { + 11.2813, 10.3256, 9.4252, 8.6072, 7.9217, 7.3442, 6.8523, 6.4245, 6.0486, 5.7154, + 5.4176, 5.1507, 4.9086, 4.6890, 4.4870, 4.3040, 4.1343, 3.9821, 3.8419, 3.7115, + 3.5888, 3.4738, 3.3658, 3.2641, 3.1678, 3.0768, 2.9914, 2.9099, 2.8331, 2.7602 +}; +const GainSchedule kEta1D = {tauAxis1D, 30, NULL, 0, kEtaGains1D}; + +static const float kOmegaGains1D[] = { + 21.9912, 21.1220, 20.3892, 19.5539, 18.6463, 17.7433, 16.8781, 16.0713, 15.3218, 14.6292, + 13.9881, 13.3950, 12.8481, 12.3391, 11.8711, 11.4311, 11.0222, 10.6336, 10.2661, 9.9247, + 9.6046, 9.3049, 9.0239, 8.7590, 8.5092, 8.2729, 8.0509, 7.8391, 7.6392, 7.4495 +}; +const GainSchedule kOmega1D = {tauAxis1D, 30, NULL, 0, kOmegaGains1D}; + +static const float ffPoleGains1D[] = { + -8.7723, -8.8351, -8.9011, -9.0603, -9.3615, -9.7481, -10.0061, -10.0710, -9.9689, -9.7649, + -9.5024, -9.2087, -8.9137, -8.6072, -8.3238, -8.0365, -7.7663, -7.4870, -7.2162, -6.9629, + -6.7278, -6.5092, -6.3034, -6.1096, -5.9288, -5.7584, -5.5980, -5.4449, -5.3004, -5.1638 +}; +const GainSchedule ffPole1D = {tauAxis1D, 30, NULL, 0, ffPoleGains1D}; + +static const float ffKGains1D[] = { + 0.9978, 1.0234, 1.0597, 1.1013, 1.1443, 1.1898, 1.2318, 1.2665, 1.2924, 1.3117, + 1.3257, 1.3352, 1.3431, 1.3475, 1.3523, 1.3538, 1.3556, 1.3519, 1.3470, 1.3416, + 1.3373, 1.3341, 1.3305, 1.3270, 1.3245, 1.3225, 1.3204, 1.3181, 1.3160, 1.3142 +}; +const GainSchedule ffK1D = {tauAxis1D, 30, NULL, 0, ffKGains1D}; + + + +// 2D Gain interpolation (NOT DONE YET) +static const float kEtaXAxis2D[] = {0, 2, 4}; +static const float kEtaYAxis2D[] = {0, 1}; +static const float kEtaGains2D[] = {0.3, 0.5, 0.7, 0.4, 0.6, 0.8}; // row-major +const GainSchedule kEta2D = {kEtaXAxis2D, 3, kEtaYAxis2D, 2, kEtaGains2D}; + +static const float kOmegaXAxis2D[] = {0, 3, 6}; +static const float kOmegaYAxis2D[] = {0, 2}; +static const float kOmegaGains2D[] = {0.6, 1.0, 1.4, 0.8, 1.2, 1.6}; // row-major +const GainSchedule kOmega2D = {kOmegaXAxis2D, 3, kOmegaYAxis2D, 2, kOmegaGains2D}; + +static const float ffKXAxis2D[] = {0, 1, 2}; +static const float ffKYAxis2D[] = {0, 3}; +static const float ffKGains2D[] = {1.2, 1.8, 2.4, 1.5, 2.1, 2.7}; // row-major +const GainSchedule ffK2D = {ffKXAxis2D, 3, ffKYAxis2D, 2, ffKGains2D}; + +static const float ffPoleXAxis2D[] = {0, 2, 4}; +static const float ffPoleYAxis2D[] = {0, 1}; +static const float ffPoleGains2D[] = {7.0, 9.0, 11.0, 8.0, 10.0, 12.0}; // row-major +const GainSchedule ffPole2D = {ffPoleXAxis2D, 3, ffPoleYAxis2D, 2, ffPoleGains2D}; + +float lerp(float x0, float x1, float y0, float y1, float x) +{ + if (x1 == x0) return y0; + return y0 + (y1 - y0) * (x - x0) / (x1 - x0); +} + +// Function for 1D gain scheduling +float interpolate1D(const float *xAxis, const float *gains, int size, float x) +{ + // Clamp gains. TODO: make bounds a limit / give error? + if (x <= xAxis[0]) return gains[0]; + if (x >= xAxis[size-1]) return gains[size-1]; + + // Find interval + int i; + for (i = 0; i < size-1; i++) { + if (xAxis[i] <= x && x <= xAxis[i+1]) { + return lerp(xAxis[i], xAxis[i+1], gains[i], gains[i+1], x); + } + } + return gains[size-1]; // should not reach here +} + +// 2D gain scheduling (bilinear interpolation) +float interpolate2D(const float *xAxis, int xSize, + const float *yAxis, int ySize, + const float **gainTable, + float x, float y) +{ + // Clamp gains. TODO: make bounds a limit / give error? + if (x <= xAxis[0]) x = xAxis[0]; + if (x >= xAxis[xSize-1]) x = xAxis[xSize-1]; + if (y <= yAxis[0]) y = yAxis[0]; + if (y >= yAxis[ySize-1]) y = yAxis[ySize-1]; + + // Find x index + int i; + for (i = 0; i < xSize-1; i++) { + if (xAxis[i] <= x && x <= xAxis[i+1]) break; + } + + // Find y index + int j; + for (j = 0; j < ySize-1; j++) { + if (yAxis[j] <= y && y <= yAxis[j+1]) break; + } + + // Corners of cell + float x0 = xAxis[i]; + float x1 = xAxis[i+1]; + float y0 = yAxis[j]; + float y1 = yAxis[j+1]; + + float Q11 = gainTable[j][i]; + float Q21 = gainTable[j][i+1]; + float Q12 = gainTable[j+1][i]; + float Q22 = gainTable[j+1][i+1]; + + // Bilinear interpolation + float R1 = lerp(x0, x1, Q11, Q21, x); + float R2 = lerp(x0, x1, Q12, Q22, x); + return lerp(y0, y1, R1, R2, y); +} + + +float evalPoly1D(const Poly1D *p, float x) +{ + float result = p->coeffs[p->order]; + + // Horner's method for polynomial evaluation + for (int i = p->order - 1; i >= 0; i--) { + result = result * x + p->coeffs[i]; + } + return result; +} + +float evalPoly2D(const Poly2D *p, float x, float y) +{ + float result = 0.0; + for (int j = 0; j <= p->yOrder; j++) { + // polynomial in x for each power of y + float temp = p->coeffs[j * (p->xOrder + 1) + p->xOrder]; + for (int i = p->xOrder - 1; i >= 0; i--) { + temp = temp * x + p->coeffs[j * (p->xOrder + 1) + i]; + } + // accumulate with y^j + float yPow = 1.0; + for (int k = 0; k < j; k++) yPow *= y; + result += temp * yPow; + } + return result; +} \ No newline at end of file diff --git a/src/main/flight/gain_schedule.h b/src/main/flight/gain_schedule.h new file mode 100644 index 000000000..e6c0c9945 --- /dev/null +++ b/src/main/flight/gain_schedule.h @@ -0,0 +1,59 @@ + + +//// Definition of structures for gainscheduling +// Structure definition for GainSchedule +typedef struct { + const float *xAxis; + int xSize; + const float *yAxis; + int ySize; + const float *table; // gain table stored row-major (y-major) +} GainSchedule; + +// Polynomial structures +typedef struct { + const float *coeffs; // [a0, a1, ..., an] + int order; +} Poly1D; + +typedef struct { + const float *coeffs; // flattened row-major: coeff[j*(xOrder+1) + i] + int xOrder; + int yOrder; +} Poly2D; + +float lerp(float x0, float x1, float y0, float y1, float x); +float interpolate1D(const float *xAxis, const float *gains, int size, float x); +float interpolate2D(const float *xAxis, int xSize, const float *yAxis, int ySize, + const float **gainTable, float x, float y); + +float evalPoly1D(const Poly1D *p, float x); +float evalPoly2D(const Poly2D *p, float x, float y); + +extern const Poly1D kEtaPoly1D; +extern const Poly1D kOmegaPoly1D; +extern const Poly1D ffKPoly1D; +extern const Poly1D ffPolePoly1D; + +extern const Poly2D kEtaPoly2D; +extern const Poly2D kOmegaPoly2D; +extern const Poly2D ffKPoly2D; +extern const Poly2D ffPolePoly2D; + +extern const GainSchedule kEta1D; +extern const GainSchedule kOmega1D; +extern const GainSchedule ffK1D; +extern const GainSchedule ffPole1D; +extern const float ffZeroGain1D; + +extern const GainSchedule kEta2D; +extern const GainSchedule kOmega2D; +extern const GainSchedule ffK2D; +extern const GainSchedule ffPole2D; + +typedef enum { + GAIN_SCHEDULE_1D_INTERP = 0, // 1D interpolation (table lookup) + GAIN_SCHEDULE_2D_INTERP = 1, // 2D interpolation (table lookup) + GAIN_SCHEDULE_1D_POLY = 2, // 1D polynomial + GAIN_SCHEDULE_2D_POLY = 3 // 2D polynomial +} GainScheduleType; diff --git a/src/main/flight/indi.c b/src/main/flight/indi.c index e99040065..201ecefe1 100644 --- a/src/main/flight/indi.c +++ b/src/main/flight/indi.c @@ -90,7 +90,7 @@ void indiController(timeUs_t current) { if (flightModeFlags & ~(CATAPULT_MODE | LEARNER_MODE)) { #pragma message "LEARNER_MODE probably shouldnt be here" // any flight mode active other than catapult, learner or acro (acro is all off)? - if ( ((++indiRun.attExecCounter)%indiRun.attRateDenom) == 1 ) { + if ( ((++indiRun.attExecCounter)%indiRun.attRateDenom) == 1 || (indiRun.attRateDenom == 1) ) { // rate limit attitude control getSetpoints(current); indiRun.attExecCounter = 1; @@ -131,6 +131,10 @@ void getSetpoints(timeUs_t current) { indiRun.spfSpBody.V.Y = 0.f; indiRun.spfSpBody.V.Z = 0.f; + indiRun.ffRateSpBody.V.X = 0.f; + indiRun.ffRateSpBody.V.Y = 0.f; + indiRun.ffRateSpBody.V.Z = 0.f; + indiRun.rateSpBodyCommanded.V.X = 0.f; indiRun.rateSpBodyCommanded.V.Y = 0.f; indiRun.rateSpBodyCommanded.V.Z = 0.f; @@ -199,14 +203,61 @@ void getSetpoints(timeUs_t current) { float roll = getRcDeflection(ROLL); float pitch = getRcDeflection(PITCH); float maxTilt = indiRun.manualMaxTilt; - fp_vector_t axis = { .V.X = maxTilt*roll, .V.Y = maxTilt*pitch, .V.Z = 0.f, }; + + + + #ifdef INJECT_ATTITUDE_SETPOINTS + if (FLIGHT_MODE(HORIZON_MODE)) { + indiRun.attSpInjectionStarted = false; + } + if (indiRun.injectAttSp) { + // Only inject setpoints in Angle Mode (so drone can be positioned in horizon mode) + if (FLIGHT_MODE(ANGLE_MODE)) { + if (!indiRun.attSpInjectionStarted) { + set_signal_mode(indiRun.attSpInjectionType, indiRun.attSpInjectionAmplitude, indiRun.attSpInjectionDuration, indiRun.attSpInjectionDurationDown, current); + indiRun.attSpInjectionStartTime = current; + indiRun.attSpInjectionStarted = true; + } + float roll_signal = generate_signal(current, indiRun.attSpInjectionStartTime); + // Add roll signal to RC input signal + axis.V.X += roll_signal; + } + } + #endif + + // Separately log axis original to maintain the original setpoint + fp_vector_t axisOriginal = { + .V.X = axis.V.X, + .V.Y = axis.V.Y, + .V.Z = 0.f, + }; + VEC3_CONSTRAIN_XY_LENGTH(axis, maxTilt); + VEC3_CONSTRAIN_XY_LENGTH(axisOriginal, maxTilt); + + float anglePreFeedforward = VEC3_XY_LENGTH(axisOriginal); + VEC3_NORMALIZE(axisOriginal); + fp_quaternion_t attSpYawPreFeedforward; + quaternion_of_axis_angle(&attSpYawPreFeedforward, &axisOriginal, anglePreFeedforward); + + + // TODO: Need to figure out how to program this properly. Depends on what feedforward is kept. + if (indiRun.useFeedforwardFilter) { + // If feedforward filter is used, apply it to the reference itself + axis.V.X = biquadFilterApply(&indiRun.feedforwardFilter[0], axis.V.X); + axis.V.Y = biquadFilterApply(&indiRun.feedforwardFilter[1], axis.V.Y); + } else { + // Else use injection feedforward + indiRun.ffRateSpBody.V.X = biquadFilterApply(&indiRun.feedforwardFilter[0], axis.V.X); + indiRun.ffRateSpBody.V.Y = biquadFilterApply(&indiRun.feedforwardFilter[1], axis.V.Y); + } float angle = VEC3_XY_LENGTH(axis); + VEC3_NORMALIZE(axis); fp_quaternion_t attSpYaw; quaternion_of_axis_angle(&attSpYaw, &axis, angle); @@ -221,6 +272,7 @@ void getSetpoints(timeUs_t current) { // this is probaby the most expensive operation... can be half the cost if // optimized for .x = 0, .y = 0, unless compiler does that for us? indiRun.attSpNed = chain_quaternion(&yawNed, &attSpYaw); + indiRun.attSpNedPreFeedforward = chain_quaternion(&yawNed, &attSpYawPreFeedforward); // convert throttle indiRun.spfSpBody.V.Z = (rcCommand[THROTTLE] - RC_OFFSET_THROTTLE); @@ -349,7 +401,7 @@ void getAlphaSpBody(timeUs_t current) { quaternion_of_axis_angle(&q_tilt_inv, &tiltAxis, -tiltErrorAngle); // get q_yaw via multiplication. - // TODO: we only need w and sign(z). Surely there is somethign faster than dense quaternion mult + // TODO: we only need w and sign(z). Surely there is something faster than dense quaternion mult fp_quaternion_t q_yaw = chain_quaternion(&indiRun.attErrBody, &q_tilt_inv); q_yaw.w = constrainf(q_yaw.w, -1.f, 1.f); @@ -357,14 +409,26 @@ void getAlphaSpBody(timeUs_t current) { if (yawErrorAngle > M_PIf) yawErrorAngle -= 2.f*M_PIf; // make sure angleErr is [-pi, pi] - // we still have to check if the vector compoenent is negative + // we still have to check if the vector component is negative // this inverts the error angle if (q_yaw.z < 0.f) yawErrorAngle = -yawErrorAngle; - // multiply with gains and mix existing rate setpoint - indiRun.rateSpBody.V.X += indiRun.attGainsCasc.V.X * tiltError.V.X; - indiRun.rateSpBody.V.Y += indiRun.attGainsCasc.V.Y * tiltError.V.Y; + + if (!indiRun.useAttLeadLag) { + // multiply with gains and mix existing rate setpoint + indiRun.rateSpBody.V.X += indiRun.attGainsCasc.V.X * tiltError.V.X; + indiRun.rateSpBody.V.Y += indiRun.attGainsCasc.V.Y * tiltError.V.Y; + } else { + // indiRun.rateSpBody.V.X += indiRun.attGainsCasc.V.X * tiltError.V.X; + // indiRun.rateSpBody.V.Y += indiRun.attGainsCasc.V.Y * tiltError.V.Y; + indiRun.rateSpBody.V.X += biquadFilterApply(&indiRun.attLlFilter[0], tiltError.V.X); + indiRun.rateSpBody.V.Y += biquadFilterApply(&indiRun.attLlFilter[1], tiltError.V.Y); + } + + indiRun.rateSpBody.V.X += indiRun.ffRateSpBody.V.X; + indiRun.rateSpBody.V.Y += indiRun.ffRateSpBody.V.Y; + if (indiRun.trackAttitudeYaw) indiRun.rateSpBody.V.Z += indiRun.attGainsCasc.V.Z * yawErrorAngle; // else: just keep rateSpBody.V.Z that has been set @@ -385,8 +449,15 @@ void getAlphaSpBody(timeUs_t current) { VEC3_SCALAR_MULT_ADD(rateErr, -1.0f, indiRun.rate_f); // alphaSpBody = rateGains * rateErr - indiRun.rateDotSpBody.V.X = indiRun.rateGains.V.X * rateErr.V.X; - indiRun.rateDotSpBody.V.Y = indiRun.rateGains.V.Y * rateErr.V.Y; + if (!indiRun.useAttLeadLag) { + indiRun.rateDotSpBody.V.X = indiRun.rateGains.V.X * rateErr.V.X; + indiRun.rateDotSpBody.V.Y = indiRun.rateGains.V.Y * rateErr.V.Y; + } else { + // indiRun.rateDotSpBody.V.X = indiRun.rateGains.V.X * rateErr.V.X; + // indiRun.rateDotSpBody.V.Y = indiRun.rateGains.V.Y * rateErr.V.Y; + indiRun.rateDotSpBody.V.X = biquadFilterApply(&indiRun.rateLlFilter[0], rateErr.V.X); + indiRun.rateDotSpBody.V.Y = biquadFilterApply(&indiRun.rateLlFilter[1], rateErr.V.Y); + } indiRun.rateDotSpBody.V.Z = indiRun.rateGains.V.Z * rateErr.V.Z; } diff --git a/src/main/flight/indi.h b/src/main/flight/indi.h index f0a0bb52e..b74554059 100644 --- a/src/main/flight/indi.h +++ b/src/main/flight/indi.h @@ -46,6 +46,9 @@ #error "Blackbox will crash. Fix it to accept more that MAXV > 8" #endif +#ifdef INJECT_ATTITUDE_SETPOINTS +#include "common/signal_generator.h" +#endif typedef struct indiProfile_s { // ---- Att/Rate config uint16_t attGains[3]; // attitude error to rotational accel gain * 10 @@ -118,6 +121,40 @@ typedef struct indiProfile_s { uint16_t wlsCondBound; // condition number bound / 1e4 uint16_t wlsTheta; // objective segragation / 1e-4 uint8_t wlsNanLimit; // disarm because of consequtive failures in wls. Keep low FIXME: make this fallback to pinv + + // -------- Parameters for motor lead-lag filter + uint8_t useMotorLeadLag; // bool: use motor lead-lag filter + uint8_t actTimeConstTrueMs[MAXU]; // natural (true) motor time constant for actuator lead-lag in ms + uint8_t actTimeConstDesMs[MAXU]; // desired time constant for actuator lead-lag in ms + + // -------- Parameters for feedforward control + int32_t feedforwardCoefs[5]; // feedforward coefficients for the feedforward INJECTION control law / 1e-6 + uint8_t useFeedforwardFilter; // If true use the FILTER feedforward, if false use INJECTION + int32_t feedforwardFilterCoefs[3]; // K, z, p, feedforward filter coefficients for the feedforward FILTER control law (need to clear this up or choose one) / 1e-6 + + // -------- Parameters for attitude lead lag control + uint8_t useAttLeadLag; // bool: use attitude lead-lag filter + int32_t rateCoefs[5]; // time constant for attitude lead-lag in ms + int32_t attCoefs[5]; // desired time constant for attitude lead-lag in ms + + // -------- Parameters for gain-scheduling + uint8_t useGainScheduling; // bool: use gain scheduling + uint8_t gainSchedulingType; // What type of gain scheduling is used. 0: 1D Interp, 1: 2D Interp, 2: 1D Poly, 3: 2D Poly + uint8_t gainScheduleFf; // Whether to take feedforward values from gain-scheduling + + // -------- Parameters for attitude injection + bool attSpInjectionStarted; // whether the injection of attitude setpoints has started + bool injectAttSp; // whether to inject attitude setpoints + int8_t attSpInjectionType; // What type of injection is used. 0: None, 1: Impulse, 2: Doublet, 3: Step + int16_t attSpInjectionAmplitude; // amplitude of injection in degrees * 10 + int16_t attSpInjectionDuration; // duration of injection in ms + int16_t attSpInjectionDurationDown; // for doublet, duration of negative pulse in ms + + // -------- Parameters for input disturbance injection + bool injectInputDist; // whether to inject input disturbance + int16_t inputDistAmplitude; // amplitude of input disturbance in percent + bool applyDistToMotor[MAXU]; // whether to apply disturbance to motor + } indiProfile_t; // linearization @@ -204,6 +241,7 @@ typedef struct indiRuntime_s { // ---- runtime values -- axes fp_vector_t attGainsCasc; // attitude gains simulating parallel PD fp_quaternion_t attSpNed; // attitude setpoint in NED coordinates + fp_quaternion_t attSpNedPreFeedforward; // attitude setpoint in NED coordinates before feedforward fp_quaternion_t attErrBody; // attitude error in body coordinates fp_vector_t rateSpBody; // rate setpoint in body coordinates fp_vector_t rateSpBodyCommanded; // rate setpoint before attitude control @@ -223,6 +261,7 @@ typedef struct indiRuntime_s { biquadFilter_t omegaFilter[MAXU]; // only support 2nd order butterworth second order section for now biquadFilter_t rateFilter[3]; // only support 2nd order butterworth second order section for now biquadFilter_t spfFilter[3]; // only support 2nd order butterworth second order section for now + // ---- housekeeping float dT; // target looptime in Sec float indiFrequency; // frequency in Hz @@ -232,6 +271,48 @@ typedef struct indiRuntime_s { bool bypassControl; // no control at all. u and d are unmodified by loop bool controlAttitude; // attempt to reach tilt given by attSpNed bool trackAttitudeYaw; // also attempt to reach yaw given by attSpNed + + // ---- actuator time constant modification lead-lag filter + bool useMotorLeadLag; // bool: use motor lead-lag filter + float actTimeConstTrueS[MAXU]; + float actTimeConstDesS[MAXU]; + biquadFilter_t motorLeadLagFilter[MAXU]; + + // ---- feedforward control + float feedforwardCoefs[5]; // feedforward coefficients for the feedforward control law b0, b1, b2, a1, a2 + bool useFeedforwardFilter; // If true use the FILTER feedforward, if false use INJECTION + float feedforwardFilterCoefs[3]; // feedforward coefficients for feedforward control law k, z, p (TODO: Yes its double now) + biquadFilter_t feedforwardFilter[3]; + fp_vector_t ffRateSpBody; + + // ---- leadlag attitude control + bool useAttLeadLag; // bool: use attitude lead-lag filter + float attCoefs[5]; + float rateCoefs[5]; + biquadFilter_t attLlFilter[3]; // attitude control filter + biquadFilter_t rateLlFilter[3]; // rate control filter + + // ---- gain scheduling + bool useGainScheduling; // whether to use gain scheduling + uint8_t gainSchedulingType; // 0: 1D interpolation, 1: 2D interpolation, 2: 1D polynomial, 3: 2D polynomial + bool gainScheduleFf; // Whether to take feedforward values from gain-scheduling + + // ---- attitude injection + #ifdef INJECT_ATTITUDE_SETPOINTS + bool attSpInjectionStarted; // whether the injection of attitude setpoints has started + signal_mode_t attSpInjectionType; // What type of injection is used. 1: None, 2: Impulse, 3: Doublet, 4: Step + float attSpInjectionAmplitude; // amplitude of injection in degrees + float attSpInjectionDuration; // duration of injection in s + float attSpInjectionDurationDown; // for doublet, duration of negative pulse in s + timeUs_t attSpInjectionStartTime; // time when the injection started + bool injectAttSp; // whether to inject attitude setpoints + #endif + + #ifdef INJECT_INPUT_DISTURBANCE + bool injectInputDist; // whether to inject input disturbance + float inputDistAmplitude; // amplitude of input disturbance in percent + bool applyDistToMotor[MAXU]; // whether to apply disturbance to motor + #endif } indiRuntime_t; #define INDI_PROFILE_COUNT 3 diff --git a/src/main/flight/indi_init.c b/src/main/flight/indi_init.c index 6ee022f96..8837f8589 100644 --- a/src/main/flight/indi_init.c +++ b/src/main/flight/indi_init.c @@ -61,10 +61,23 @@ void resetIndiProfile(indiProfile_t *indiProfile) { indiProfile->manualUseCoordinatedYaw = true; indiProfile->manualMaxUpwardsSpf = 30; indiProfile->manualMaxTilt = 45; // degrees + for (int i = 0; i < 5; i++) { + indiProfile->feedforwardCoefs[i] = 0; + } + + indiProfile->feedforwardFilterCoefs[1] = 1; // Theoretically this makes the filter a straight pass-through, for safety probably better to disable if no values given. + for (int i = 1; i < 3; i++) { + indiProfile->feedforwardFilterCoefs[i] = 1; + } + indiProfile->useFeedforwardFilter = false; + // ---- general INDI config indiProfile->useIncrement = true; indiProfile->useAccelForSpfz = true; indiProfile->useRpmDotFeedback = true; + indiProfile->useMotorLeadLag = false; + indiProfile->useAttLeadLag = false; + // ---- INDI actuator config indiProfile->actNum = 4; for (int i = 0; i < MAXU; i++) { @@ -85,6 +98,8 @@ void resetIndiProfile(indiProfile_t *indiProfile) { indiProfile->actG2_roll[i] = 0; indiProfile->actG2_pitch[i] = 0; indiProfile->actG2_yaw[i] = 0; // actuator rate effectiveness + indiProfile->actTimeConstTrueMs[i] = 25; // true time constant in seconds + indiProfile->actTimeConstDesMs[i] = 25; // desired time constant in seconds // ---- WLS config indiProfile->wlsWu[i] = 1; indiProfile->u_pref[i] = 0; @@ -116,7 +131,7 @@ void resetIndiProfile(indiProfile_t *indiProfile) { // -------- inaccessible parameters for now (will always be the values from the reset function) // ---- Att/Rate config - indiProfile->attRateDenom = 4; + indiProfile->attRateDenom = 1; // ---- WLS config indiProfile->useConstantG2 = false; indiProfile->useRpmFeedback = false; @@ -131,6 +146,28 @@ void resetIndiProfile(indiProfile_t *indiProfile) { indiProfile->wlsCondBound = 1 << 15; indiProfile->wlsTheta = 1; indiProfile->wlsNanLimit = 20; + + // ---- gain scheduling + indiProfile->useGainScheduling = false; + indiProfile->gainSchedulingType = 0; // 0: None, 1: 1D interp, 2: 2D interp, 3: 1D poly, 4: 2D poly + indiProfile->gainScheduleFf = false; // Whether to take feedforward values from gain-scheduling + + // ---- attitude injection + indiProfile->injectAttSp = false; // whether to inject attitude setpoints + indiProfile->attSpInjectionStarted = false; + indiProfile->attSpInjectionType = 0; // 0: None, 1: Impulse, 2: Doublet, 3: Step + indiProfile->attSpInjectionAmplitude = 0; // amplitude of injection in degrees * 10 + indiProfile->attSpInjectionDuration = 0; // duration of injection in ms + indiProfile->attSpInjectionDurationDown = 0; // for doublet, duration of negative pulse in ms + + #ifdef INJECT_INPUT_DISTURBANCE + // ---- input disturbance injection + indiProfile->injectInputDist = false; // whether to inject input disturbance + indiProfile->inputDistAmplitude = 0; // amplitude of input disturbance in percent + for (int i = 0; i < MAXU; i++) { + indiProfile->applyDistToMotor[i] = false; // whether to apply disturbance to motor + } + #endif } void initIndiRuntimeParameters(void) { @@ -189,6 +226,9 @@ void initIndiRuntimeParameters(void) { // ---- WLS config indiRun.wlsWu[i] = (float) p->wlsWu[i]; indiRun.u_pref[i] = p->u_pref[i] * 0.01f; + indiRun.useMotorLeadLag = (bool) p->useMotorLeadLag; + indiRun.actTimeConstTrueS[i] = MAX(1UL, p->actTimeConstTrueMs[i]) * 1e-3f; + indiRun.actTimeConstDesS[i] = MAX(1UL, p->actTimeConstDesMs[i]) * 1e-3f; } //indiRun.actG1[0][0] = NAN; // FIXME: crashtesting for (int i = 0; i < MAXV; i++) @@ -251,6 +291,14 @@ void initIndiRuntimeParameters(void) { indiRun.attGainsCasc.A[axis] = indiRun.attGains.A[axis] / indiRun.rateGains.A[axis]; // attitude gains simulating parallel PD } + for (int i = 0; i < 5; i++) { + indiRun.feedforwardCoefs[i] = p->feedforwardCoefs[i] * 1e-6f; + indiRun.attCoefs[i] = p->attCoefs[i] * 1e-6f; + indiRun.rateCoefs[i] = p->rateCoefs[i] * 1e-6f; + } + for (int i = 0; i < 3; i++) { + indiRun.feedforwardFilterCoefs[i] = p->feedforwardFilterCoefs[i] * 1e-6f; + } // ---- housekeeping indiRun.dT = gyro.targetLooptime * 1e-6f; // target looptime in S indiRun.indiFrequency = 1.0f / indiRun.dT; // target looptime in S @@ -258,7 +306,50 @@ void initIndiRuntimeParameters(void) { // ---- control law selection indiRun.bypassControl = false; // no control at all. u and d are unmodified by loop indiRun.controlAttitude = true; // attempt to reach tilt given by attSpNed - indiRun.trackAttitudeYaw = false; // also attempt to reach yaw given by attSpNed + indiRun.trackAttitudeYaw = false; // also attempt to reach yaw given by attSpNedj + indiRun.useAttLeadLag = p->useAttLeadLag; // use lead-lag filter for attitude control + indiRun.useFeedforwardFilter = p->useFeedforwardFilter; // use lead-lag filter for feedforward + + // ---- gain scheduling + indiRun.useGainScheduling = p->useGainScheduling; + indiRun.gainSchedulingType = p->gainSchedulingType; + indiRun.gainScheduleFf = p->gainScheduleFf; + + // ---- attitude injection parameters + #ifdef INJECT_ATTITUDE_SETPOINTS + indiRun.injectAttSp = p->injectAttSp; + indiRun.attSpInjectionStarted = false; + + switch (p->attSpInjectionType) { + case 0: // None + indiRun.attSpInjectionType = SIGNAL_MODE_OFF; + break; + case 1: // Impulse + indiRun.attSpInjectionType = SIGNAL_MODE_IMPULSE; + break; + case 2: // Doublet + indiRun.attSpInjectionType = SIGNAL_MODE_DOUBLET; + break; + case 3: // Step + indiRun.attSpInjectionType = SIGNAL_MODE_STEP; + break; + default: + indiRun.attSpInjectionType = SIGNAL_MODE_OFF; + break; + } + + indiRun.attSpInjectionAmplitude = DEGREES_TO_RADIANS(p->attSpInjectionAmplitude) * 0.1f; // convert to radians and scale + indiRun.attSpInjectionDuration = p->attSpInjectionDuration * 1e-3f; // convert to seconds + indiRun.attSpInjectionDurationDown = p->attSpInjectionDurationDown * 1e-3f; // convert to seconds + #endif + + #ifdef INJECT_INPUT_DISTURBANCE + indiRun.injectInputDist = p->injectInputDist; + indiRun.inputDistAmplitude = constrain(p->inputDistAmplitude, 0, 100) * 0.01f; // Convert to motor setpoint + for (int i = 0; i < MAXU; i++) { + indiRun.applyDistToMotor[i] = p->applyDistToMotor[i]; + } + #endif } void initIndiRuntime(void) { @@ -267,12 +358,12 @@ void initIndiRuntime(void) { // reset runtime values - // ---- runtime values -- actauators + // ---- runtime values -- actuators for (int i = 0; i < indiRun.actNum; i++) { indiRun.d[i] = 0.f; // command issued to the actuators on [-1, 1] scale indiRun.u[i] = 0.f; // control variable proportional to output force, but on [-1, 1], for motors [0, 1] indiRun.uState[i] = 0.f; // estimated force state of the actuators [-1, 1] - indiRun.uState_fs[i] = 0.f; // sync-filtered estiamted force state + indiRun.uState_fs[i] = 0.f; // sync-filtered estimated force state indiRun.omega[i] = 0.f; // unfiltered motor speed rad/s indiRun.omega_fs[i] = 0.f; // sync-filtered motor speed rad/s //indiRun.omegaDot[i] = 0.f; // unfiltered motor rate rad/s/s @@ -289,10 +380,11 @@ void initIndiRuntime(void) { //indiRun.rate_fs.A[axis] = 0.; // sync-filtered gyro in rad/s indiRun.rateDotIMU.A[axis] = 0.; // unfiltered gyro derivative in rad/s/s indiRun.rateDot_fs.A[axis] = 0.; // sync-filtered gyro derivative in rad/s/s - indiRun.spfIMU.A[axis] = 0.; // unfilterd accelerometer (specific force) in N/kg - indiRun.spf_fs.A[axis] = 0.; // sync-filterd accelerometer (specific force) in N/kg + indiRun.spfIMU.A[axis] = 0.; // unfiltered accelerometer (specific force) in N/kg + indiRun.spf_fs.A[axis] = 0.; // sync-filtered accelerometer (specific force) in N/kg } indiRun.attSpNed = (const fp_quaternion_t) { 1.f, 0.f, 0.f, 0.f }; + indiRun.attSpNedPreFeedforward = ( const fp_quaternion_t ) { 1.f, 0.f, 0.f, 0.f }; indiRun.attErrBody = (const fp_quaternion_t) { 1.f, 0.f, 0.f, 0.f }; for (int j = 0; j < MAXV; j++) { indiRun.dv[j] = 0.f; // delta-pseudo controls in N/kg and Nm/(kgm^2) @@ -300,18 +392,46 @@ void initIndiRuntime(void) { // ---- housekeeping indiRun.attExecCounter = 0; // count executions (wrapping) - indiRun.nanCounter = 0; // count times consequtive nans appear in allocation + indiRun.nanCounter = 0; // count times consecutive nans appear in allocation // ---- filters for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { biquadFilterInitLPF(&indiRun.rateFilter[axis], indiRun.imuSyncLp2Hz, gyro.targetLooptime); // only support 2nd order butterworth second order section for now biquadFilterInitLPF(&indiRun.spfFilter[axis], indiRun.imuSyncLp2Hz, gyro.targetLooptime); // only support 2nd order butterworth second order section for now + if (!indiRun.useGainScheduling) { + biquadFilterInitLeadLag(&indiRun.feedforwardFilter[axis], 1.0f, 0.0f, 0.0f, 0.0f, 0.0f); // initialize as static gain of 1 + } else { + if (indiRun.gainScheduleFf) { + // biquadFilterInitZeroPole(&indiRun.feedforwardFilter[axis], 0.0f, 0.0f, 0.0f, gyro.targetLooptime); // wait until gains scheduled for initializing filter fully + // Initialize filter as a static gain + // biquadFilterInitLeadLag(&indiRun.feedforwardFilter[axis], 1.0f, 0.0f, 0.0f, 0.0f, 0.0f); + biquadFilterInitZeroPole(&indiRun.feedforwardFilter[axis], indiRun.feedforwardFilterCoefs[0], indiRun.feedforwardFilterCoefs[1], indiRun.feedforwardFilterCoefs[2], gyro.targetLooptime); + } else if (indiRun.useFeedforwardFilter) { + if (fabsf(indiRun.feedforwardFilterCoefs[2]) < 1e-6f) { + biquadFilterInitLeadLag(&indiRun.feedforwardFilter[axis], 1.0f, 0.0f, 0.0f, 0.0f, 0.0f); + } else { + biquadFilterInitZeroPole(&indiRun.feedforwardFilter[axis], indiRun.feedforwardFilterCoefs[0], indiRun.feedforwardFilterCoefs[1], indiRun.feedforwardFilterCoefs[2], gyro.targetLooptime); + } + } else { + biquadFilterInitLeadLag(&indiRun.feedforwardFilter[axis], indiRun.feedforwardCoefs[0], indiRun.feedforwardCoefs[1], indiRun.feedforwardCoefs[2], indiRun.feedforwardCoefs[3], indiRun.feedforwardCoefs[4]); + } + } + biquadFilterInitLeadLag(&indiRun.rateLlFilter[axis], indiRun.rateCoefs[0], indiRun.rateCoefs[1], indiRun.rateCoefs[2], indiRun.rateCoefs[3], indiRun.rateCoefs[4]); + biquadFilterInitLeadLag(&indiRun.attLlFilter[axis], indiRun.attCoefs[0], indiRun.attCoefs[1], indiRun.attCoefs[2], indiRun.attCoefs[3], indiRun.attCoefs[4]); } for (int i = 0; i < indiRun.actNum; i++) { pt1FilterInit(&indiRun.uLagFilter[i], pt1FilterGain(1.f / (2.f * M_PIf * indiRun.actTimeConstS[i]), indiRun.dT)); // to simulate spinup biquadFilterInitLPF(&indiRun.uStateFilter[i], indiRun.imuSyncLp2Hz, gyro.targetLooptime); // only support 2nd order butterworth second order section for now biquadFilterInitLPF(&indiRun.omegaFilter[i], indiRun.imuSyncLp2Hz, gyro.targetLooptime); // only support 2nd order butterworth second order section for now + if (indiRun.useMotorLeadLag) { + // initialize motor lead-lag filter + // b0 = 1, b1 = 0, b2 = 0, a1 = -exp(-dT/tauEst), a2 = exp(-dT/tauDes) + biquadFilterInitMotorLeadLag(&indiRun.motorLeadLagFilter[i], indiRun.actTimeConstTrueS[i], indiRun.actTimeConstDesS[i], gyro.targetLooptime); + } } + #ifdef INJECT_ATTITUDE_SETPOINTS + indiRun.attSpInjectionStartTime = 0; + #endif } #endif // ifdef USE_INDI diff --git a/src/main/flight/learner.c b/src/main/flight/learner.c index d7b377fd7..158a76a74 100644 --- a/src/main/flight/learner.c +++ b/src/main/flight/learner.c @@ -47,6 +47,7 @@ #include "flight/pos_ctl.h" #include "flight/throw.h" #include "ekf_calc.h" // for dirty override +#include "gain_schedule.h" #include "f2c.h" #include "clapack.h" @@ -575,6 +576,32 @@ void updateLearner(timeUs_t current) { rlsNewSample(&fxRls[i], A, &ySpf[i]); // spf rlsNewSample(&fxRls[i+3], A, &yRateDot[i]); // RateDot } + + // test beun for tiny whoop + # ifdef LEARNER_PARAM_OVERRIDE + const indiProfile_t *p = indiProfiles(0); + + for (int motor = 0; motor < learnerConfig()->numAct; motor++) { + float maxOmega = 2.f * M_PIf / 60.f * p->actMaxRpm[motor]; + float isq = 1.f / sq(maxOmega); + // float k = 0.01f * p->actNonlinearity[motor]; + // motorRls[motor].x[0] = 1e-3f * k * maxOmega; + // motorRls[motor].x[1] = 1e-3f * (1.f - k) * maxOmega; + // motorRls[motor].x[2] = 0.; + // motorRls[motor].x[3] = 1e-3f * 1e4f * 1e-3f * (p->actTimeConstMs[motor]); + + fxRls[0].x[motor] = 10.f * 1e5f * isq * 1e-2f * p->actG1_fx[motor]; + fxRls[1].x[motor] = 10.f * 1e5f * isq * 1e-2f * p->actG1_fy[motor]; + fxRls[2].x[motor] = 10.f * 1e5f * isq * 1e-2f * p->actG1_fz[motor]; + + // fxRls[3].x[motor] = 1.f * 1e5f * isq * 1e-1f * p->actG1_roll[motor]; + // fxRls[4].x[motor] = 1.f * 1e5f * isq * 1e-1f * p->actG1_pitch[motor]; + // fxRls[5].x[motor] = 1.f * 1e5f * isq * 1e-1f * p->actG1_yaw[motor]; + // fxRls[5].x[motor+4] = 1.f * 1e3f * 1e-5f * p->actG2_yaw[motor]; + + // motorRls[motor].x[0] = 1e-3f * 0.01f * maxOmega; + } + # endif } else { was_reset_fx_rls = false; } @@ -582,7 +609,7 @@ void updateLearner(timeUs_t current) { bool motorLearningConditions = fxLearningConditions && (learningQueryState != LEARNING_QUERY_DONE); // motor fortescue didnt work for some reason - if (motorLearningConditions && false) { + if (motorLearningConditions) { for (int act = 0; act < learnerConfig()->numAct; act++) { float A[4] = { learnRun.motorD[act], @@ -605,12 +632,27 @@ void updateLearner(timeUs_t current) { maxTau = MAX(maxTau, motorRls[act].x[3] * 0.1f); maxTau = constrainf(maxTau, 0.01f, 0.2f); - // calculate gains - learnRun.gains[LEARNER_LOOP_RATE] = - 0.25f / (sq(learnRun.zeta[LEARNER_LOOP_RATE]) * maxTau); + if (indiRun.useGainScheduling) { + // Still set velocity and position gains + float tempGain = 0.25f / (sq(learnRun.zeta[LEARNER_LOOP_RATE]) * maxTau); + // float tempGain = rateGain; + for (int loop = LEARNER_LOOP_ATTITUDE; loop < LEARNER_LOOP_COUNT; loop++) { + tempGain = 0.25f * tempGain / sq(learnRun.zeta[loop]); + if (loop > LEARNER_LOOP_ATTITUDE) { + learnRun.gains[loop] = tempGain; + } + } + } else { + // calculate gains + learnRun.gains[LEARNER_LOOP_RATE] = + 0.25f / (sq(learnRun.zeta[LEARNER_LOOP_RATE]) * maxTau); - for (int loop = LEARNER_LOOP_ATTITUDE; loop < LEARNER_LOOP_COUNT; loop++) - learnRun.gains[loop] = 0.25f * learnRun.gains[loop-1] / sq(learnRun.zeta[loop]); + for (int loop = LEARNER_LOOP_ATTITUDE; loop < LEARNER_LOOP_COUNT; loop++) + learnRun.gains[loop] = 0.25f * learnRun.gains[loop-1] / sq(learnRun.zeta[loop]); + } + indiRun.attSpNedPreFeedforward.w = indiRun.feedforwardFilterCoefs[0]; + indiRun.attSpNedPreFeedforward.x = indiRun.feedforwardFilterCoefs[1]; + indiRun.attSpNedPreFeedforward.y = indiRun.feedforwardFilterCoefs[2]; } learnerTimings.gains = cmpTimeUs(micros(), learnerTimings.start); @@ -667,11 +709,109 @@ void updateLearner(timeUs_t current) { } void updateLearnedParameters(indiProfile_t* indi, positionProfile_t* pos) { - for (int axis = 0; axis < 3; axis++) { - indi->rateGains[axis] = (uint16_t) 10.f * learnRun.gains[LEARNER_LOOP_RATE]; - // attGains are expected for parallel PD, but we have cascaded, so - indi->attGains[axis] = (uint16_t) 10.f - * learnRun.gains[LEARNER_LOOP_ATTITUDE] * learnRun.gains[LEARNER_LOOP_RATE]; + // TODO: Put this somewhere more logical + float kEtaMin = 1.0f; + float kOmegaMin = 1.0f; + + // Initialize kFf and pFf + float kFf = 0.0f; + float pFf = 0.0f; + float zFf = 0.0f; + // float margin = 0.05f; // Margin for pole zero cancellation (if below, dont use FF filter) + + // TODO: Add minimum for FF? + if (indiRun.useGainScheduling) { + // get slowest actuator + float maxTau = 0.0f; + for (int act = 0; act < learnerConfig()->numAct; act++) + maxTau = MAX(maxTau, motorRls[act].x[3] * 0.1f); + // TODO: What to do if maxTau stays 0 + + maxTau = constrainf(maxTau, 0.01f, 0.2f); + for (int axis = 0; axis < 3; axis++) { + // Find minimal control coefficient for each axis + float minC = INFINITY; + for (int act = 0; act < learnerConfig()->numAct; act++) { + switch (axis) { + case FD_ROLL: minC = MIN(minC, abs(indi->actG1_roll[act])); break; + case FD_PITCH: minC = MIN(minC, abs(indi->actG1_pitch[act])); break; + case FD_YAW: minC = MIN(minC, abs(indi->actG1_yaw[act])); break; + } + } + + minC = minC * 0.1f; // Convert C unit + switch (indiRun.gainSchedulingType) { + case GAIN_SCHEDULE_1D_INTERP: + indi->rateGains[axis] = (uint16_t)(10.0f * interpolate1D(kOmega1D.xAxis, kOmega1D.table, kOmega1D.xSize, maxTau)); + indi->attGains[axis] = (uint16_t)(interpolate1D(kEta1D.xAxis, kEta1D.table, kEta1D.xSize, maxTau) * indi->rateGains[axis]); + + if (indiRun.gainScheduleFf) { + kFf = interpolate1D(ffK1D.xAxis, ffK1D.table, ffK1D.xSize, maxTau); + pFf = interpolate1D(ffPole1D.xAxis, ffPole1D.table, ffPole1D.xSize, maxTau); + zFf = pFf / kFf; // Based on 0 dB gain at LF + // 1209727, -8168513, -9881670 + indi->feedforwardFilterCoefs[0] = kFf * 1e6f; + indi->feedforwardFilterCoefs[1] = zFf * 1e6f; + indi->feedforwardFilterCoefs[2] = pFf * 1e6f; + indiRun.feedforwardFilterCoefs[0] = kFf; + indiRun.feedforwardFilterCoefs[1] = zFf; + indiRun.feedforwardFilterCoefs[2] = pFf; + } + break; + case GAIN_SCHEDULE_2D_INTERP: + indi->rateGains[axis] = (uint16_t)(10.0f * interpolate2D(kOmega2D.xAxis, kOmega2D.xSize, kOmega2D.yAxis, kOmega2D.ySize, + (const float**)kOmega2D.table, maxTau, minC)); + indi->attGains[axis] = (uint16_t)(interpolate2D(kEta2D.xAxis, kEta2D.xSize, kEta2D.yAxis, kEta2D.ySize, + (const float**)kEta2D.table, maxTau, minC) * indi->rateGains[axis]); + if (indiRun.gainScheduleFf) { + kFf = interpolate2D(ffK2D.xAxis, ffK2D.xSize, ffK2D.yAxis, ffK2D.ySize, + (const float**)ffK2D.table, maxTau, minC); + pFf = -interpolate2D(ffPole2D.xAxis, ffPole2D.xSize, ffPole2D.yAxis, ffPole2D.ySize, + (const float**)ffPole2D.table, maxTau, minC); + biquadFilterInitZeroPole(&indiRun.feedforwardFilter[axis], kFf, 0.0f, pFf, gyro.targetLooptime); + } + break; + case GAIN_SCHEDULE_1D_POLY: + indi->rateGains[axis] = (uint16_t)(10.0f * evalPoly1D(&kOmegaPoly1D, maxTau)); + indi->attGains[axis] = (uint16_t)(evalPoly1D(&kEtaPoly1D, maxTau) * indi->rateGains[axis]); + if (indiRun.gainScheduleFf) { + kFf = evalPoly1D(&ffKPoly1D, maxTau); + pFf = -evalPoly1D(&ffPolePoly1D, maxTau); // ffPolePoly is defined as the polynomial through -p + // zFf = ffZeroGain1D; + if (indiRun.useFeedforwardFilter) { + biquadFilterInitZeroPole(&indiRun.feedforwardFilter[axis], kFf, zFf, pFf, gyro.targetLooptime); + } else { + biquadFilterInitZeroPole(&indiRun.feedforwardFilter[axis], kFf, 0.0f, pFf, gyro.targetLooptime); + } + } + + break; + case GAIN_SCHEDULE_2D_POLY: + indi->rateGains[axis] = (uint16_t)(10.0f * evalPoly2D(&kOmegaPoly2D, maxTau, minC)); + indi->attGains[axis] = (uint16_t)(evalPoly2D(&kEtaPoly2D, maxTau, minC) * indi->rateGains[axis]); + if (indiRun.gainScheduleFf) { + kFf = evalPoly2D(&ffKPoly2D, maxTau, minC); + pFf = -evalPoly2D(&ffPolePoly2D, maxTau, minC); + biquadFilterInitZeroPole(&indiRun.feedforwardFilter[axis], kFf, 0.0f, pFf, gyro.targetLooptime); + } + break; + } + // Ensure gains are above minimum + indi->rateGains[axis] = MAX(indi->rateGains[axis], (uint16_t)(10.0f * kOmegaMin)); + indi->attGains[axis] = MAX(indi->attGains[axis], (uint16_t)(kEtaMin * indi->rateGains[axis])); + learnRun.gains[LEARNER_LOOP_RATE] = indi->rateGains[axis] / 10.0f; + learnRun.gains[LEARNER_LOOP_ATTITUDE] = (float)indi->attGains[axis] / (float)indi->rateGains[axis]; + // Override position and velocity gains to log feedforward values + // learnRun.gains[LEARNER_LOOP_VELOCITY] = kFf; + // learnRun.gains[LEARNER_LOOP_POSITION] = pFf; + } + } else { + for (int axis = 0; axis < 3; axis++) { + indi->rateGains[axis] = (uint16_t) 10.f * learnRun.gains[LEARNER_LOOP_RATE]; + // attGains are expected for parallel PD, but we have cascaded, so + indi->attGains[axis] = (uint16_t) 10.f + * learnRun.gains[LEARNER_LOOP_ATTITUDE] * learnRun.gains[LEARNER_LOOP_RATE]; + } } // same for position @@ -715,6 +855,9 @@ void updateLearnedParameters(indiProfile_t* indi, positionProfile_t* pos) { for (int act = 0; act < indi->actNum ; act++) { // inv y-scale float maxOmega = 1e3f * (motorRls[act].x[0] + motorRls[act].x[1]); + #ifdef LEARNER_PARAM_OVERRIDE + maxOmega = 2.f * M_PIf / 60.f * indiProfiles(0)->actMaxRpm[act]; + #endif indi->actMaxRpm[act] = MAX(100.f, 60.f * 0.5f / M_PIf * maxOmega); // convert to deg/s indi->actHoverRpm[act] = indi->actMaxRpm[act] >> 1; // guess, shouldnt matter since we have useRpmDotFeedback = true // inv y-scale a-scale config-scale