Skip to content

Commit 506a9e3

Browse files
RobertJN64RobertJN64
committed
add bias system
1 parent c2caea2 commit 506a9e3

3 files changed

Lines changed: 42 additions & 4 deletions

File tree

‎UI/include/flight_data.h‎

Lines changed: 20 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -42,11 +42,20 @@ struct flight_history_t {
4242
float state_vel_north;
4343
float state_vel_west;
4444
float state_vel_up;
45+
float gyro_bias_yaw[FLIGHT_HISTORY_LENGTH * 2];
46+
float gyro_bias_pitch[FLIGHT_HISTORY_LENGTH * 2];
47+
float gyro_bias_roll[FLIGHT_HISTORY_LENGTH * 2];
48+
float accel_bias_x[FLIGHT_HISTORY_LENGTH * 2];
49+
float accel_bias_y[FLIGHT_HISTORY_LENGTH * 2];
50+
float accel_bias_z[FLIGHT_HISTORY_LENGTH * 2];
51+
float mag_bias_x[FLIGHT_HISTORY_LENGTH * 2];
52+
float mag_bias_y[FLIGHT_HISTORY_LENGTH * 2];
53+
float mag_bias_z[FLIGHT_HISTORY_LENGTH * 2];
4554

4655
float gimbal_yaw_raw;
4756
float gimbal_pitch_raw;
4857
float thrust_N;
49-
float roll_roll_rad_sec_squared;
58+
float roll_rad_sec_squared;
5059

5160
float target_pos_north;
5261
float target_pos_west;
@@ -94,11 +103,20 @@ struct flight_packet_t {
94103
float state_vel_north;
95104
float state_vel_west;
96105
float state_vel_up;
106+
float gyro_bias_yaw;
107+
float gyro_bias_pitch;
108+
float gyro_bias_roll;
109+
float accel_bias_x;
110+
float accel_bias_y;
111+
float accel_bias_z;
112+
float mag_bias_x;
113+
float mag_bias_y;
114+
float mag_bias_z;
97115

98116
float gimbal_yaw_raw;
99117
float gimbal_pitch_raw;
100118
float thrust_N;
101-
float roll_roll_rad_sec_squared;
119+
float roll_rad_sec_squared;
102120

103121
float target_pos_north;
104122
float target_pos_west;

‎UI/include/ui.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,9 @@ void render_loop();
77
#define IMU_ACCEL_PANEL "imu_accel_panel"
88
#define IMU_GYRO_PANEL "imu_gyro_panel"
99
#define MAG_PANEL "mag_panel"
10+
#define GYRO_BIAS_PANEL "gyro_bias_panel"
11+
#define MAG_BIAS_PANEL "mag_bias_panel"
12+
#define ACCEL_BIAS_PANEL "accel_bias_panel"
1013
#define GPS_POS_PANEL "gps_pos_panel"
1114
#define GPS_VEL_PANEL "gps_vel_panel"
1215
#define GPS_VERT_PANEL "gps_vert_panel"

‎UI/src/flight_data.cpp‎

Lines changed: 19 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -54,18 +54,35 @@ void commit_packet() {
5454
FlightHistory.state_q_vec_0 = active_packet.state_q_vec_0;
5555
FlightHistory.state_q_vec_1 = active_packet.state_q_vec_1;
5656
FlightHistory.state_q_vec_2 = active_packet.state_q_vec_2;
57-
5857
FlightHistory.state_pos_north = active_packet.state_pos_north;
5958
FlightHistory.state_pos_west = active_packet.state_pos_west;
6059
FlightHistory.state_pos_up = active_packet.state_pos_up;
6160
FlightHistory.state_vel_north = active_packet.state_vel_north;
6261
FlightHistory.state_vel_west = active_packet.state_vel_west;
6362
FlightHistory.state_vel_up = active_packet.state_vel_up;
63+
FlightHistory.gyro_bias_yaw[FlightHistory.write_pos] = active_packet.gyro_bias_yaw;
64+
FlightHistory.gyro_bias_yaw[FlightHistory.write_pos + FLIGHT_HISTORY_LENGTH] = active_packet.gyro_bias_yaw;
65+
FlightHistory.gyro_bias_pitch[FlightHistory.write_pos] = active_packet.gyro_bias_pitch;
66+
FlightHistory.gyro_bias_pitch[FlightHistory.write_pos + FLIGHT_HISTORY_LENGTH] = active_packet.gyro_bias_pitch;
67+
FlightHistory.gyro_bias_roll[FlightHistory.write_pos] = active_packet.gyro_bias_roll;
68+
FlightHistory.gyro_bias_roll[FlightHistory.write_pos + FLIGHT_HISTORY_LENGTH] = active_packet.gyro_bias_roll;
69+
FlightHistory.accel_bias_x[FlightHistory.write_pos] = active_packet.accel_bias_x;
70+
FlightHistory.accel_bias_x[FlightHistory.write_pos + FLIGHT_HISTORY_LENGTH] = active_packet.accel_bias_x;
71+
FlightHistory.accel_bias_y[FlightHistory.write_pos] = active_packet.accel_bias_y;
72+
FlightHistory.accel_bias_y[FlightHistory.write_pos + FLIGHT_HISTORY_LENGTH] = active_packet.accel_bias_y;
73+
FlightHistory.accel_bias_z[FlightHistory.write_pos] = active_packet.accel_bias_z;
74+
FlightHistory.accel_bias_z[FlightHistory.write_pos + FLIGHT_HISTORY_LENGTH] = active_packet.accel_bias_z;
75+
FlightHistory.mag_bias_x[FlightHistory.write_pos] = active_packet.mag_bias_x;
76+
FlightHistory.mag_bias_x[FlightHistory.write_pos + FLIGHT_HISTORY_LENGTH] = active_packet.mag_bias_x;
77+
FlightHistory.mag_bias_y[FlightHistory.write_pos] = active_packet.mag_bias_y;
78+
FlightHistory.mag_bias_y[FlightHistory.write_pos + FLIGHT_HISTORY_LENGTH] = active_packet.mag_bias_y;
79+
FlightHistory.mag_bias_z[FlightHistory.write_pos] = active_packet.mag_bias_z;
80+
FlightHistory.mag_bias_z[FlightHistory.write_pos + FLIGHT_HISTORY_LENGTH] = active_packet.mag_bias_z;
6481

6582
FlightHistory.gimbal_yaw_raw = active_packet.gimbal_yaw_raw;
6683
FlightHistory.gimbal_pitch_raw = active_packet.gimbal_pitch_raw;
6784
FlightHistory.thrust_N = active_packet.thrust_N;
68-
FlightHistory.roll_roll_rad_sec_squared = active_packet.roll_roll_rad_sec_squared;
85+
FlightHistory.roll_rad_sec_squared = active_packet.roll_rad_sec_squared;
6986

7087
FlightHistory.target_pos_north = active_packet.target_pos_north;
7188
FlightHistory.target_pos_west = active_packet.target_pos_west;

0 commit comments

Comments
 (0)