mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 02:55:07 +08:00
Working on comments.
This commit is contained in:
+15
-15
@@ -138,25 +138,25 @@ int __EXPORT dcmTest()
|
|||||||
Matrix::identity(3)));
|
Matrix::identity(3)));
|
||||||
// quaternion ctor
|
// quaternion ctor
|
||||||
ASSERT(matrixEqual(
|
ASSERT(matrixEqual(
|
||||||
Dcm(Quaternion(0.983347, 0.034271, 0.106021, 0.143572)),
|
Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
|
||||||
Dcm( 0.9362934, -0.2750958, 0.2183507,
|
Dcm( 0.9362934f, -0.2750958f, 0.2183507f,
|
||||||
0.2896295, 0.9564251, -0.0369570,
|
0.2896295f, 0.9564251f, -0.0369570f,
|
||||||
-0.1986693, 0.0978434, 0.9751703)));
|
-0.1986693f, 0.0978434f, 0.9751703f)));
|
||||||
// euler angle ctor
|
// euler angle ctor
|
||||||
ASSERT(matrixEqual(
|
ASSERT(matrixEqual(
|
||||||
Dcm(EulerAngles(0.1, 0.2, 0.3)),
|
Dcm(EulerAngles(0.1f, 0.2f, 0.3f)),
|
||||||
Dcm( 0.9362934, -0.2750958, 0.2183507,
|
Dcm( 0.9362934f, -0.2750958f, 0.2183507f,
|
||||||
0.2896295, 0.9564251, -0.0369570,
|
0.2896295f, 0.9564251f, -0.0369570f,
|
||||||
-0.1986693, 0.0978434, 0.9751703)));
|
-0.1986693f, 0.0978434f, 0.9751703f)));
|
||||||
// rotations
|
// rotations
|
||||||
Vector3 vB(1, 2, 3);
|
Vector3 vB(1, 2, 3);
|
||||||
ASSERT(vectorEqual(Vector3(-2, 1, 3),
|
ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f),
|
||||||
Dcm(EulerAngles(0, 0, M_PI_2_F))*vB));
|
Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB));
|
||||||
ASSERT(vectorEqual(Vector3(3, 2, -1),
|
ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
|
||||||
Dcm(EulerAngles(0, M_PI_2_F, 0))*vB));
|
Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB));
|
||||||
ASSERT(vectorEqual(Vector3(1, -3, 2),
|
ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f),
|
||||||
Dcm(EulerAngles(M_PI_2_F, 0, 0))*vB));
|
Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB));
|
||||||
ASSERT(vectorEqual(Vector3(3, 2, -1),
|
ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
|
||||||
Dcm(EulerAngles(
|
Dcm(EulerAngles(
|
||||||
M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
|
M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
|
||||||
printf("PASS\n");
|
printf("PASS\n");
|
||||||
|
|||||||
@@ -97,27 +97,27 @@ EulerAngles::~EulerAngles()
|
|||||||
int __EXPORT eulerAnglesTest()
|
int __EXPORT eulerAnglesTest()
|
||||||
{
|
{
|
||||||
printf("Test EulerAngles\t: ");
|
printf("Test EulerAngles\t: ");
|
||||||
EulerAngles euler(0.1, 0.2, 0.3);
|
EulerAngles euler(0.1f, 0.2f, 0.3f);
|
||||||
|
|
||||||
// test ctor
|
// test ctor
|
||||||
ASSERT(vectorEqual(Vector3(0.1, 0.2, 0.3), euler));
|
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
|
||||||
ASSERT(equal(euler.getPhi(), 0.1));
|
ASSERT(equal(euler.getPhi(), 0.1f));
|
||||||
ASSERT(equal(euler.getTheta(), 0.2));
|
ASSERT(equal(euler.getTheta(), 0.2f));
|
||||||
ASSERT(equal(euler.getPsi(), 0.3));
|
ASSERT(equal(euler.getPsi(), 0.3f));
|
||||||
|
|
||||||
// test dcm ctor
|
// test dcm ctor
|
||||||
euler = Dcm(EulerAngles(0.1,0.2,0.3));
|
euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
|
||||||
ASSERT(vectorEqual(Vector3(0.1,0.2,0.3),euler));
|
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f),euler));
|
||||||
|
|
||||||
// test quat ctor
|
// test quat ctor
|
||||||
euler = Quaternion(EulerAngles(0.1,0.2,0.3));
|
euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
|
||||||
ASSERT(vectorEqual(Vector3(0.1,0.2,0.3),euler));
|
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f),euler));
|
||||||
|
|
||||||
// test assignment
|
// test assignment
|
||||||
euler.setPhi(0.4);
|
euler.setPhi(0.4f);
|
||||||
euler.setTheta(0.5);
|
euler.setTheta(0.5f);
|
||||||
euler.setPsi(0.6);
|
euler.setPsi(0.6f);
|
||||||
ASSERT(vectorEqual(Vector3(0.4,0.5,0.6),euler));
|
ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f),euler));
|
||||||
|
|
||||||
printf("PASS\n");
|
printf("PASS\n");
|
||||||
return 0;
|
return 0;
|
||||||
|
|||||||
@@ -144,29 +144,29 @@ int __EXPORT quaternionTest()
|
|||||||
printf("Test Quaternion\t\t: ");
|
printf("Test Quaternion\t\t: ");
|
||||||
// test default ctor
|
// test default ctor
|
||||||
Quaternion q;
|
Quaternion q;
|
||||||
ASSERT(equal(q.getA(), 1));
|
ASSERT(equal(q.getA(), 1.0f));
|
||||||
ASSERT(equal(q.getB(), 0));
|
ASSERT(equal(q.getB(), 0.0f));
|
||||||
ASSERT(equal(q.getC(), 0));
|
ASSERT(equal(q.getC(), 0.0f));
|
||||||
ASSERT(equal(q.getD(), 0));
|
ASSERT(equal(q.getD(), 0.0f));
|
||||||
// test float ctor
|
// test float ctor
|
||||||
q = Quaternion(0.1825742, 0.3651484, 0.5477226, 0.7302967);
|
q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
|
||||||
ASSERT(equal(q.getA(), 0.1825742));
|
ASSERT(equal(q.getA(), 0.1825742f));
|
||||||
ASSERT(equal(q.getB(), 0.3651484));
|
ASSERT(equal(q.getB(), 0.3651484f));
|
||||||
ASSERT(equal(q.getC(), 0.5477226));
|
ASSERT(equal(q.getC(), 0.5477226f));
|
||||||
ASSERT(equal(q.getD(), 0.7302967));
|
ASSERT(equal(q.getD(), 0.7302967f));
|
||||||
// test euler ctor
|
// test euler ctor
|
||||||
q = Quaternion(EulerAngles(0.1, 0.2, 0.3));
|
q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
|
||||||
ASSERT(vectorEqual(q, Quaternion(0.983347, 0.034271, 0.106021, 0.143572)));
|
ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)));
|
||||||
// test dcm ctor
|
// test dcm ctor
|
||||||
q = Quaternion(Dcm());
|
q = Quaternion(Dcm());
|
||||||
ASSERT(vectorEqual(q, Quaternion(1, 0, 0, 0)));
|
ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f)));
|
||||||
// TODO test derivative
|
// TODO test derivative
|
||||||
// test accessors
|
// test accessors
|
||||||
q.setA(0.1);
|
q.setA(0.1f);
|
||||||
q.setB(0.2);
|
q.setB(0.2f);
|
||||||
q.setC(0.3);
|
q.setC(0.3f);
|
||||||
q.setD(0.4);
|
q.setD(0.4f);
|
||||||
ASSERT(vectorEqual(q, Quaternion(0.1, 0.2, 0.3, 0.4)));
|
ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f)));
|
||||||
printf("PASS\n");
|
printf("PASS\n");
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -346,11 +346,6 @@ handle_message(mavlink_message_t *msg)
|
|||||||
hil_sensors.adc_voltage_v[1] = 0;
|
hil_sensors.adc_voltage_v[1] = 0;
|
||||||
hil_sensors.adc_voltage_v[2] = 0;
|
hil_sensors.adc_voltage_v[2] = 0;
|
||||||
|
|
||||||
/* battery */
|
|
||||||
hil_sensors.battery_voltage_counter = hil_counter;
|
|
||||||
hil_sensors.battery_voltage_v = 11.1f;
|
|
||||||
hil_sensors.battery_voltage_valid = true;
|
|
||||||
|
|
||||||
/* magnetometer */
|
/* magnetometer */
|
||||||
float mga2ga = 1.0e-3f;
|
float mga2ga = 1.0e-3f;
|
||||||
hil_sensors.magnetometer_counter = hil_counter;
|
hil_sensors.magnetometer_counter = hil_counter;
|
||||||
|
|||||||
@@ -99,7 +99,6 @@ struct sensor_combined_s {
|
|||||||
float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */
|
float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */
|
||||||
float baro_alt_meter; /**< Altitude, already temp. comp. */
|
float baro_alt_meter; /**< Altitude, already temp. comp. */
|
||||||
float baro_temp_celcius; /**< Temperature in degrees celsius */
|
float baro_temp_celcius; /**< Temperature in degrees celsius */
|
||||||
float battery_voltage_v; /**< Battery voltage in volts, filtered */
|
|
||||||
float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */
|
float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */
|
||||||
float mcu_temp_celcius; /**< Internal temperature measurement of MCU */
|
float mcu_temp_celcius; /**< Internal temperature measurement of MCU */
|
||||||
uint32_t baro_counter; /**< Number of raw baro measurements taken */
|
uint32_t baro_counter; /**< Number of raw baro measurements taken */
|
||||||
|
|||||||
Reference in New Issue
Block a user