|
|
@ -101,16 +101,19 @@ void rot2quat(const float R[9], float Q[4]) |
|
|
|
q2_2 = (R[5] - R[7]) / 4.0F / q0_2; |
|
|
|
q2_2 = (R[5] - R[7]) / 4.0F / q0_2; |
|
|
|
q3_2 = (R[6] - R[2]) / 4.0F / q0_2; |
|
|
|
q3_2 = (R[6] - R[2]) / 4.0F / q0_2; |
|
|
|
q0_2 = (R[1] - R[3]) / 4.0F / q0_2; |
|
|
|
q0_2 = (R[1] - R[3]) / 4.0F / q0_2; |
|
|
|
|
|
|
|
|
|
|
|
} else if (idx == 1) { |
|
|
|
} else if (idx == 1) { |
|
|
|
q2_2 = q0_2; |
|
|
|
q2_2 = q0_2; |
|
|
|
q1_2 = (R[5] - R[7]) / 4.0F / q0_2; |
|
|
|
q1_2 = (R[5] - R[7]) / 4.0F / q0_2; |
|
|
|
q3_2 = (R[3] + R[1]) / 4.0F / q0_2; |
|
|
|
q3_2 = (R[3] + R[1]) / 4.0F / q0_2; |
|
|
|
q0_2 = (R[6] + R[2]) / 4.0F / q0_2; |
|
|
|
q0_2 = (R[6] + R[2]) / 4.0F / q0_2; |
|
|
|
|
|
|
|
|
|
|
|
} else if (idx == 2) { |
|
|
|
} else if (idx == 2) { |
|
|
|
q3_2 = q0_2; |
|
|
|
q3_2 = q0_2; |
|
|
|
q1_2 = (R[6] - R[2]) / 4.0F / q0_2; |
|
|
|
q1_2 = (R[6] - R[2]) / 4.0F / q0_2; |
|
|
|
q2_2 = (R[3] + R[1]) / 4.0F / q0_2; |
|
|
|
q2_2 = (R[3] + R[1]) / 4.0F / q0_2; |
|
|
|
q0_2 = (R[7] + R[5]) / 4.0F / q0_2; |
|
|
|
q0_2 = (R[7] + R[5]) / 4.0F / q0_2; |
|
|
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
} else { |
|
|
|
q1_2 = (R[1] - R[3]) / 4.0F / q0_2; |
|
|
|
q1_2 = (R[1] - R[3]) / 4.0F / q0_2; |
|
|
|
q2_2 = (R[6] + R[2]) / 4.0F / q0_2; |
|
|
|
q2_2 = (R[6] + R[2]) / 4.0F / q0_2; |
|
|
@ -151,5 +154,5 @@ void quat2rot(const float Q[4], float R[9]) |
|
|
|
|
|
|
|
|
|
|
|
float get_air_density(float static_pressure, float temperature_celsius) |
|
|
|
float get_air_density(float static_pressure, float temperature_celsius) |
|
|
|
{ |
|
|
|
{ |
|
|
|
return static_pressure/(air_gas_constant * (temperature_celsius + absolute_null_kelvin)); |
|
|
|
return static_pressure / (air_gas_constant * (temperature_celsius + absolute_null_kelvin)); |
|
|
|
} |
|
|
|
} |
|
|
|