From affa3af4e6415d1f68dd6242d0ded6e5ed8a1b1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 3 Oct 2012 13:39:26 +0200 Subject: Clean 250 Hz updates in filter, partial updates enabled --- .../attitude_estimator_ekf_main.c | 57 ++++++++++++++++------ .../multirotor_att_control_main.c | 4 +- apps/position_estimator/position_estimator_main.c | 5 +- apps/sdlog/sdlog.c | 37 +++++++++++++- 4 files changed, 82 insertions(+), 21 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 295cc4b54..6368c5157 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -161,7 +161,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) thread_should_exit = false; attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", SCHED_RR, - SCHED_PRIORITY_DEFAULT, + SCHED_PRIORITY_MAX - 5, 20000, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -221,7 +221,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* rate-limit raw data updates to 200Hz */ - orb_set_interval(sub_raw, 5); + orb_set_interval(sub_raw, 4); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -236,6 +236,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); + /* keep track of sensor updates */ + uint32_t sensor_last_count[3] = {0, 0, 0}; + uint64_t sensor_last_timestamp[3] = {0, 0, 0}; + float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; /* process noise covariance */ float q[12]; @@ -264,17 +268,38 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; + uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ + if (sensor_last_count[0] != raw.gyro_counter) { + update_vect[0] = 1; + sensor_last_count[0] = raw.gyro_counter; + sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); + sensor_last_timestamp[0] = raw.timestamp; + } + z_k[0] = raw.gyro_rad_s[0]; z_k[1] = raw.gyro_rad_s[1]; z_k[2] = raw.gyro_rad_s[2]; - /* scale from 14 bit to m/s2 */ + /* update accelerometer measurements */ + if (sensor_last_count[1] != raw.accelerometer_counter) { + update_vect[1] = 1; + sensor_last_count[1] = raw.accelerometer_counter; + sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); + sensor_last_timestamp[1] = raw.timestamp; + } z_k[3] = raw.accelerometer_m_s2[0]; z_k[4] = raw.accelerometer_m_s2[1]; z_k[5] = raw.accelerometer_m_s2[2]; + /* update magnetometer measurements */ + if (sensor_last_count[2] != raw.magnetometer_counter) { + update_vect[2] = 1; + sensor_last_count[2] = raw.magnetometer_counter; + sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); + sensor_last_timestamp[2] = raw.timestamp; + } z_k[6] = raw.magnetometer_ga[0]; z_k[7] = raw.magnetometer_ga[1]; z_k[8] = raw.magnetometer_ga[2]; @@ -293,7 +318,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) overloadcounter++; } - uint8_t update_vect[3] = {1, 1, 1}; int32_t z_k_sizes = 9; // float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; @@ -361,21 +385,22 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) uint64_t timing_diff = hrt_absolute_time() - timing_start; // /* print rotation matrix every 200th time */ - // if (printcounter % 200 == 0) { - // // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n", - // // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2], - // // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5], - // // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]); + if (printcounter % 200 == 0) { + // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n", + // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2], + // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5], + // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]); - // // } + // } - // printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); - // printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]); - // // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), - // // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), - // // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); - // } + printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); + printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]); + printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]); + // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), + // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), + // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); + } // int i = printcounter % 9; diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 9acdc6fd3..8bd9ebc96 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -110,7 +110,7 @@ static void *rate_control_thread_main(void *arg) /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* XXX this means no sensor data - should be critical or emergency */ - printf("[mc att control] WARNING: Not getting gyro data, no rate control\n"); + //printf("[mc att control] WARNING: Not getting gyro data, no rate control\n"); } else { /* get data */ orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report); @@ -354,7 +354,7 @@ int multirotor_att_control_main(int argc, char *argv[]) mc_task = task_spawn("multirotor_att_control", SCHED_RR, SCHED_PRIORITY_MAX - 15, - 2048, + 4096, mc_thread_main, NULL); exit(0); diff --git a/apps/position_estimator/position_estimator_main.c b/apps/position_estimator/position_estimator_main.c index e061256fc..f8390f100 100644 --- a/apps/position_estimator/position_estimator_main.c +++ b/apps/position_estimator/position_estimator_main.c @@ -261,6 +261,7 @@ int position_estimator_main(int argc, char *argv[]) /* subscribe to vehicle status, attitude, gps */ struct vehicle_gps_position_s gps; + gps.fix_type = 0; struct vehicle_status_s vstatus; struct vehicle_attitude_s att; @@ -283,8 +284,8 @@ int position_estimator_main(int argc, char *argv[]) /* Wait for the GPS update to propagate (we have some time) */ usleep(5000); /* Read wether the vehicle status changed */ - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); - gps_valid = vstatus.gps_valid; + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); + gps_valid = (gps.fix_type > 2); } } diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index f4af17597..0426dfb9c 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -60,6 +60,9 @@ #include #include #include +#include +#include +#include #include @@ -273,6 +276,9 @@ int sdlog_thread_main(int argc, char *argv[]) { struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; struct vehicle_command_s cmd; + struct vehicle_local_position_s local_pos; + struct vehicle_global_position_s global_pos; + struct vehicle_gps_position_s gps_pos; } buf; memset(&buf, 0, sizeof(buf)); @@ -283,6 +289,9 @@ int sdlog_thread_main(int argc, char *argv[]) { int spa_sub; int act_0_sub; int controls0_sub; + int local_pos_sub; + int global_pos_sub; + int gps_pos_sub; } subs; /* --- MANAGEMENT - LOGGING COMMAND --- */ @@ -329,6 +338,27 @@ int sdlog_thread_main(int argc, char *argv[]) { fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- LOCAL POSITION --- */ + /* subscribe to ORB for local position */ + subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + fds[fdsc_count].fd = subs.local_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- GLOBAL POSITION --- */ + /* subscribe to ORB for global position */ + subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + fds[fdsc_count].fd = subs.global_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- GPS POSITION --- */ + /* subscribe to ORB for global position */ + subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + fds[fdsc_count].fd = subs.gps_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -435,6 +465,9 @@ int sdlog_thread_main(int argc, char *argv[]) { /* copy actuator data into local buffer */ orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); + orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); #pragma pack(push, 1) struct { @@ -450,6 +483,7 @@ int sdlog_thread_main(int argc, char *argv[]) { float actuators[8]; float vbat; float adc[3]; + float local_pos[3]; } sysvector = { .timestamp = buf.raw.timestamp, .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, @@ -462,7 +496,8 @@ int sdlog_thread_main(int argc, char *argv[]) { .actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]}, .vbat = buf.raw.battery_voltage_v, - .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]} + .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, + .local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z} }; #pragma pack(pop) -- cgit v1.2.3