fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 23:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Acceleration convertion from UAV frame to NED frame

Browse Source
This commit is contained in:
Anton
2013-03-29 13:08:56 +04:00
parent b837692d48
commit fb663bbb0c
1 changed files with 46 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/position_estimator_inav/position_estimator_inav_main.c
+46 -29
View File
@@ -87,18 +87,19 @@ static void usage(const char *reason);
static void usage(const char *reason) { static void usage(const char *reason) {
if (reason) if (reason)
fprintf(stderr, "%s\n", reason); fprintf(stderr, "%s\n", reason);
fprintf(stderr,"usage: position_estimator_inav {start|stop|status} [-p <additional params>]\n\n"); fprintf(stderr,
"usage: position_estimator_inav {start|stop|status} [-p <additional params>]\n\n");
exit(1); exit(1);
} }
/** /**
* The position_estimator_inav_thread only briefly exists to start * The position_estimator_inav_thread only briefly exists to start
* the background job. The stack size assigned in the * the background job. The stack size assigned in the
* Makefile does only apply to this management task. * Makefile does only apply to this management task.
* *
* The actual stack size should be set in the call * The actual stack size should be set in the call
* to task_create(). * to task_create().
*/ */
int position_estimator_inav_main(int argc, char *argv[]) { int position_estimator_inav_main(int argc, char *argv[]) {
if (argc < 1) if (argc < 1)
usage("missing command"); usage("missing command");
@@ -147,7 +148,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); mavlink_log_info(mavlink_fd, "[position_estimator_inav] started");
/* initialize values */ /* initialize values */
static float k[3][2] = { { 0.0f, 0.0f }, { 0.0f, 0.0f }, { 0.0f, 0.0f } }; static float k[3][2] = { { 0.002f, 0.0f }, { 0.001f, 0.00002f }, { 0.0f,
0.99f } };
static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float x_est[3] = { 0.0f, 0.0f, 0.0f };
static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f };
static float z_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f };
@@ -167,7 +169,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
/* declare and safely initialize all structs */ /* declare and safely initialize all structs */
struct vehicle_status_s vehicle_status; struct vehicle_status_s vehicle_status;
memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */ memset(&vehicle_status, 0, sizeof(vehicle_status));
/* make sure that baroINITdone = false */
struct sensor_combined_s sensor; struct sensor_combined_s sensor;
memset(&sensor, 0, sizeof(sensor)); memset(&sensor, 0, sizeof(sensor));
struct vehicle_gps_position_s gps; struct vehicle_gps_position_s gps;
@@ -239,19 +242,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
} }
printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n", printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n",
lat_current, lon_current); lat_current, lon_current);
uint64_t last_time = 0; hrt_abstime last_time = 0;
thread_running = true; thread_running = true;
/** main loop */ /** main loop */
struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { struct pollfd fds[3] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN } };
.fd = vehicle_status_sub, .events = POLLIN }, { .fd =
vehicle_attitude_sub, .events = POLLIN }, { .fd =
sensor_combined_sub, .events = POLLIN }, { .fd =
vehicle_gps_position_sub, .events = POLLIN }, };
printf("[position_estimator_inav] main loop started\n"); printf("[position_estimator_inav] main loop started\n");
static int printatt_counter = 0; static int printatt_counter = 0;
while (!thread_should_exit) { while (!thread_should_exit) {
int ret = poll(fds, 5, 20); //wait maximal this 20 ms = 50 Hz minimum rate int ret = poll(fds, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate
if (ret < 0) { if (ret < 0) {
/* poll error */ /* poll error */
} else { } else {
@@ -271,16 +270,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
&vehicle_status); &vehicle_status);
} }
/* vehicle attitude */ /* vehicle attitude */
if (fds[2].revents & POLLIN) { //if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
} //}
/* sensor combined */ /* sensor combined */
if (fds[3].revents & POLLIN) { if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
} }
if (use_gps) { if (use_gps) {
/* vehicle GPS position */ /* vehicle GPS position */
if (fds[4].revents & POLLIN) { //if (fds[4].revents & POLLIN) {
/* new GPS value */ /* new GPS value */
orb_copy(ORB_ID(vehicle_gps_position), orb_copy(ORB_ID(vehicle_gps_position),
vehicle_gps_position_sub, &gps); vehicle_gps_position_sub, &gps);
@@ -290,7 +289,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]),
&(local_pos_gps[1])); &(local_pos_gps[1]));
local_pos_gps[2] = (float) (gps.alt * 1e-3); local_pos_gps[2] = (float) (gps.alt * 1e-3);
} //}
} }
// barometric pressure estimation at start up // barometric pressure estimation at start up
if (!local_flag_baroINITdone) { if (!local_flag_baroINITdone) {
@@ -312,16 +311,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
mavlink_log_info(mavlink_fd, str); mavlink_log_info(mavlink_fd, str);
} }
} }
/* TODO convert accelerations from UAV frame to NED frame */
float accel_NED[3]; hrt_abstime t = hrt_absolute_time();
accel_NED[2] = sensor.accelerometer_m_s2[2] + const_earth_gravity; float dt = (t - last_time) / 1000000.0;
last_time = t;
/* convert accelerations from UAV frame to NED frame */
float accel_NED[3] = { 0.0f, 0.0f, 0.0f };
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
}
}
accel_NED[2] += const_earth_gravity;
/* prediction */ /* prediction */
kalman_filter_inertial_predict(dT_const_120, z_est); kalman_filter_inertial_predict(dt, z_est);
/* prepare vectors for kalman filter correction */ /* prepare vectors for kalman filter correction */
float z_meas[2]; // pos, accel float z_meas[2]; // pos, accel
bool use_z[2] = { false, true }; bool use_z[2] = { false, true };
if (local_flag_baroINITdone) { if (local_flag_baroINITdone) {
z_meas[0] = sensor.baro_alt_meter - baro_alt0; z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt
use_z[0] = true; use_z[0] = true;
} }
z_meas[1] = accel_NED[2]; z_meas[1] = accel_NED[2];
@@ -329,14 +337,23 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
kalman_filter_inertial_update(z_est, z_meas, k, use_z); kalman_filter_inertial_update(z_est, z_meas, k, use_z);
if (printatt_counter == 100) { if (printatt_counter == 100) {
printatt_counter = 0; printatt_counter = 0;
printf("[pos_est_inav] dt = %.6f\n", dt);
printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n",
att.pitch, att.roll, att.yaw); att.pitch, att.roll, att.yaw);
printf("[pos_est_inav] accel_UAV = %.3f, %.3f, %.3f\n",
sensor.accelerometer_m_s2[0],
sensor.accelerometer_m_s2[1],
sensor.accelerometer_m_s2[2]);
printf("[pos_est_inav] accel_NED = %.3f, %.3f, %.3f\n",
accel_NED[0], accel_NED[1], accel_NED[2]);
/*
printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n",
att.R[0][0], att.R[0][1], att.R[0][2]); att.R[0][0], att.R[0][1], att.R[0][2]);
printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n",
att.R[1][0], att.R[1][1], att.R[1][2]); att.R[1][0], att.R[1][1], att.R[1][2]);
printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n",
att.R[2][0], att.R[2][1], att.R[2][2]); att.R[2][0], att.R[2][1], att.R[2][2]);
*/
printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n",
z_est[0], z_est[1], z_est[2]); z_est[0], z_est[1], z_est[2]);
} }