diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index a8f80fd4c7..e5620138a5 100644 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -36,6 +36,7 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 CSRCS = attitude_estimator_ekf_main.c \ + attitude_estimator_ekf_params.c \ codegen/eye.c \ codegen/attitudeKalmanfilter.c \ codegen/mrdivide.c \ diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 46c1a66236..41f469ae4e 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -58,25 +58,20 @@ #include #include #include +#include #include #include #include "codegen/attitudeKalmanfilter_initialize.h" #include "codegen/attitudeKalmanfilter.h" +#include "attitude_estimator_ekf_params.h" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); - -// #define N_STATES 6 - -// #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000 -// #define REPROJECTION_COUNTER_LIMIT 125 - static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds static float dt = 1.0f; -/* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ static float z_k[9]; /**< Measurement vector */ static float x_aposteriori_k[12]; /**< */ @@ -94,6 +89,7 @@ static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, }; + static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -107,11 +103,14 @@ static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, }; /**< init: diagonal matrix with big values */ -// static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ + +/* output euler angles */ +static float euler[3] = {0.0f, 0.0f, 0.0f}; + static float Rot_matrix[9] = {1.f, 0, 0, - 0, 1.f, 0, - 0, 0, 1.f - }; /**< init: identity matrix */ + 0, 1.f, 0, + 0, 0, 1.f + }; /**< init: identity matrix */ static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -161,7 +160,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 +220,10 @@ 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); + + /* subscribe to param changes */ + int sub_params = orb_subscribe(ORB_ID(parameter_update)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -236,19 +238,24 @@ 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]; - /* measurement noise covariance */ - float r[9]; - /* output euler angles */ - float euler[3] = {0.0f, 0.0f, 0.0f}; + struct attitude_estimator_ekf_params ekf_params; + + struct attitude_estimator_ekf_param_handles ekf_param_handles; + + /* initialize parameter handles */ + parameters_init(&ekf_param_handles); /* Main loop*/ while (!thread_should_exit) { - struct pollfd fds[1] = { + struct pollfd fds[2] = { { .fd = sub_raw, .events = POLLIN }, + { .fd = sub_params, .events = POLLIN } }; int ret = poll(fds, 1, 1000); @@ -258,152 +265,173 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* XXX this means no sensor data - should be critical or emergency */ printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n"); } else { + + /* only update parameters if they changed */ + if (fds[1].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), sub_params, &update); - orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + /* update parameters */ + parameters_update(&ekf_param_handles, &ekf_params); + } - /* Calculate data time difference in seconds */ - dt = (raw.timestamp - last_measurement) / 1000000.0f; - last_measurement = raw.timestamp; + /* only run filter if sensor values changed */ + if (fds[0].revents & POLLIN) { - /* Fill in gyro measurements */ - z_k[0] = raw.gyro_rad_s[0]; - z_k[1] = raw.gyro_rad_s[1]; - z_k[2] = raw.gyro_rad_s[2]; + /* get latest measurements */ + orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); - /* scale from 14 bit to m/s2 */ - z_k[3] = raw.accelerometer_m_s2[0]; - z_k[4] = raw.accelerometer_m_s2[1]; - z_k[5] = raw.accelerometer_m_s2[2]; + /* 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}; - z_k[6] = raw.magnetometer_ga[0]; - z_k[7] = raw.magnetometer_ga[1]; - z_k[8] = raw.magnetometer_ga[2]; - - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; - - if (time_elapsed > loop_interval_alarm) { - //TODO: add warning, cpu overload here - if (overloadcounter == 20) { - printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - overloadcounter = 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; } - overloadcounter++; + z_k[0] = raw.gyro_rad_s[0]; + z_k[1] = raw.gyro_rad_s[1]; + z_k[2] = raw.gyro_rad_s[2]; + + /* 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]; + + uint64_t now = hrt_absolute_time(); + unsigned int time_elapsed = now - last_run; + last_run = now; + + if (time_elapsed > loop_interval_alarm) { + //TODO: add warning, cpu overload here + // if (overloadcounter == 20) { + // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + // overloadcounter = 0; + // } + + overloadcounter++; + } + + int32_t z_k_sizes = 9; + // float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + + static bool const_initialized = false; + + /* initialize with good values once we have a reasonable dt estimate */ + if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) + { + dt = 0.005f; + parameters_update(&ekf_param_handles, &ekf_params); + + x_aposteriori_k[0] = z_k[0]; + x_aposteriori_k[1] = z_k[1]; + x_aposteriori_k[2] = z_k[2]; + x_aposteriori_k[3] = 0.0f; + x_aposteriori_k[4] = 0.0f; + x_aposteriori_k[5] = 0.0f; + x_aposteriori_k[6] = z_k[3]; + x_aposteriori_k[7] = z_k[4]; + x_aposteriori_k[8] = z_k[5]; + x_aposteriori_k[9] = z_k[6]; + x_aposteriori_k[10] = z_k[7]; + x_aposteriori_k[11] = z_k[8]; + + const_initialized = true; + } + + /* do not execute the filter if not initialized */ + if (!const_initialized) { + continue; + } + + dt = 0.004f; + + uint64_t timing_start = hrt_absolute_time(); + // attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, + // Rot_matrix, x_aposteriori, P_aposteriori); + attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, + euler, Rot_matrix, x_aposteriori, P_aposteriori); + /* swap values for next iteration */ + memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); + memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); + 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]); + + + // } + + 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; + + // // for (int i = 0; i < 9; i++) { + // char name[10]; + // sprintf(name, "xapo #%d", i); + // memcpy(dbg.key, name, sizeof(dbg.key)); + // dbg.value = x_aposteriori[i]; + // orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + + printcounter++; + + if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); + + last_data = raw.timestamp; + + /* send out */ + att.timestamp = raw.timestamp; + att.roll = euler[0]; + att.pitch = euler[1]; + att.yaw = euler[2]; + + att.rollspeed = x_aposteriori[0]; + att.pitchspeed = x_aposteriori[1]; + att.yawspeed = x_aposteriori[2]; + + /* copy offsets */ + memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + + /* copy rotation matrix */ + memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + att.R_valid = true; + + // Broadcast + orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } - - 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}; - - static bool const_initialized = false; - - /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) - { - dt = 0.005f; - q[0] = 1e1f; - q[1] = 1e1f; - q[2] = 1e1f; - /* process noise gyro offset covariance */ - q[3] = 1e-4f; - q[4] = 1e-4f; - q[5] = 1e-4f; - q[6] = 1e-1f; - q[7] = 1e-1f; - q[8] = 1e-1f; - q[9] = 1e-1f; - q[10] = 1e-1f; - q[11] = 1e-1f; - - r[0]= 1e-2f; - r[1]= 1e-2f; - r[2]= 1e-2f; - r[3]= 1e-1f; - r[4]= 1e-1f; - r[5]= 1e-1f; - r[6]= 1e-1f; - r[7]= 1e-1f; - r[8]= 1e-1f; - - x_aposteriori_k[0] = z_k[0]; - x_aposteriori_k[1] = z_k[1]; - x_aposteriori_k[2] = z_k[2]; - x_aposteriori_k[3] = 0.0f; - x_aposteriori_k[4] = 0.0f; - x_aposteriori_k[5] = 0.0f; - x_aposteriori_k[6] = z_k[3]; - x_aposteriori_k[7] = z_k[4]; - x_aposteriori_k[8] = z_k[5]; - x_aposteriori_k[9] = z_k[6]; - x_aposteriori_k[10] = z_k[7]; - x_aposteriori_k[11] = z_k[8]; - - const_initialized = true; - } - - /* do not execute the filter if not initialized */ - if (!const_initialized) { - continue; - } - - dt = 0.004f; - - uint64_t timing_start = hrt_absolute_time(); - // attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, - // Rot_matrix, x_aposteriori, P_aposteriori); - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, q, r, - euler, Rot_matrix, x_aposteriori, P_aposteriori); - /* swap values for next iteration */ - memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); - memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); - 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]); - - - // // } - - // 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)); - // } - - // int i = printcounter % 9; - - // // for (int i = 0; i < 9; i++) { - // char name[10]; - // sprintf(name, "xapo #%d", i); - // memcpy(dbg.key, name, sizeof(dbg.key)); - // dbg.value = x_aposteriori[i]; - // orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - - printcounter++; - - if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); - - last_data = raw.timestamp; - - /* send out */ - att.timestamp = raw.timestamp; - att.roll = euler[0]; - att.pitch = euler[1]; - att.yaw = euler[2]; - - att.rollspeed = x_aposteriori[0]; - att.pitchspeed = x_aposteriori[1]; - att.yawspeed = x_aposteriori[2]; - - // Broadcast - orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } loopcounter++; diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c new file mode 100644 index 0000000000..d95c56368b --- /dev/null +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli + * Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file attitude_estimator_ekf_params.c + * + * Parameters for EKF filter + */ + +#include "attitude_estimator_ekf_params.h" + +/* Extended Kalman Filter covariances */ + +/* gyro process noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_Q0, 1e1f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q1, 1e1f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q2, 1e1f); +/* gyro offsets process noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_Q3, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q4, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q5, 1e-4f); +/* accelerometer process noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_Q6, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q7, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q8, 1e-1f); +/* magnetometer process noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_Q9, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q10, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_Q11, 1e-1f); + +/* gyro measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_R0, 1e-2f); +PARAM_DEFINE_FLOAT(EKF_ATT_R1, 1e-2f); +PARAM_DEFINE_FLOAT(EKF_ATT_R2, 1e-2f); +/* accelerometer measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e-1f); +/* magnetometer measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f); + +int parameters_init(struct attitude_estimator_ekf_param_handles *h) +{ + /* PID parameters */ + h->q0 = param_find("EKF_ATT_Q0"); + h->q1 = param_find("EKF_ATT_Q1"); + h->q2 = param_find("EKF_ATT_Q2"); + h->q3 = param_find("EKF_ATT_Q3"); + h->q4 = param_find("EKF_ATT_Q4"); + h->q5 = param_find("EKF_ATT_Q5"); + h->q6 = param_find("EKF_ATT_Q6"); + h->q7 = param_find("EKF_ATT_Q7"); + h->q8 = param_find("EKF_ATT_Q8"); + h->q9 = param_find("EKF_ATT_Q9"); + h->q10 = param_find("EKF_ATT_Q10"); + h->q11 = param_find("EKF_ATT_Q11"); + + h->r0 = param_find("EKF_ATT_R0"); + h->r1 = param_find("EKF_ATT_R1"); + h->r2 = param_find("EKF_ATT_R2"); + h->r3 = param_find("EKF_ATT_R3"); + h->r4 = param_find("EKF_ATT_R4"); + h->r5 = param_find("EKF_ATT_R5"); + h->r6 = param_find("EKF_ATT_R6"); + h->r7 = param_find("EKF_ATT_R7"); + h->r8 = param_find("EKF_ATT_R8"); + + return OK; +} + +int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p) +{ + param_get(h->q0, &(p->q[0])); + param_get(h->q1, &(p->q[1])); + param_get(h->q2, &(p->q[2])); + param_get(h->q3, &(p->q[3])); + param_get(h->q4, &(p->q[4])); + param_get(h->q5, &(p->q[5])); + param_get(h->q6, &(p->q[6])); + param_get(h->q7, &(p->q[7])); + param_get(h->q8, &(p->q[8])); + param_get(h->q9, &(p->q[9])); + param_get(h->q10, &(p->q[10])); + param_get(h->q11, &(p->q[11])); + + param_get(h->r0, &(p->r[0])); + param_get(h->r1, &(p->r[1])); + param_get(h->r2, &(p->r[2])); + param_get(h->r3, &(p->r[3])); + param_get(h->r4, &(p->r[4])); + param_get(h->r5, &(p->r[5])); + param_get(h->r6, &(p->r[6])); + param_get(h->r7, &(p->r[7])); + param_get(h->r8, &(p->r[8])); + + return OK; +} diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h new file mode 100644 index 0000000000..6a63f97674 --- /dev/null +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli + * Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file attitude_estimator_ekf_params.h + * + * Parameters for EKF filter + */ + +#include + +struct attitude_estimator_ekf_params { + float r[9]; + float q[12]; +}; + +struct attitude_estimator_ekf_param_handles { + param_t r0, r1, r2, r3, r4, r5, r6, r7, r8; + param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct attitude_estimator_ekf_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p); diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index f3c0746e09..ba7f08bc83 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -91,12 +91,12 @@ static void *rate_control_thread_main(void *arg) struct actuator_controls_s actuators; - int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - struct pollfd fds = { .fd = gyro_sub, .events = POLLIN }; + struct pollfd fds = { .fd = att_sub, .events = POLLIN }; - struct gyro_report gyro_report; + struct vehicle_attitude_s vehicle_attitude; struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); float gyro_lp[3] = {0.0f, 0.0f, 0.0f}; @@ -104,16 +104,17 @@ static void *rate_control_thread_main(void *arg) while (!thread_should_exit) { /* rate control at maximum rate */ /* wait for a sensor update, check for exit condition every 1000 ms */ -// int ret = poll(&fds, 1, 1000); -// -// if (ret < 0) { -// /* 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"); -// } else { - /* get data */ - orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report); + int ret = poll(&fds, 1, 1000); + + if (ret < 0) { + /* 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"); + } else { + /* get current angular rate */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &vehicle_attitude); + /* get current rate setpoint */ bool rates_sp_valid = false; orb_check(rates_sp_sub, &rates_sp_valid); if (rates_sp_valid) { @@ -126,14 +127,14 @@ static void *rate_control_thread_main(void *arg) if (state.flag_control_rates_enabled) { /* lowpass gyros */ // XXX - gyro_lp[0] = gyro_report.x; - gyro_lp[1] = gyro_report.y; - gyro_lp[2] = gyro_report.z; + gyro_lp[0] = vehicle_attitude.rollspeed; + gyro_lp[1] = vehicle_attitude.pitchspeed; + gyro_lp[2] = vehicle_attitude.yawspeed; multirotor_control_rates(&rates_sp, gyro_lp, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } -// } + } usleep(2000); } @@ -355,7 +356,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/Makefile b/apps/position_estimator/Makefile index a766f1666b..3502cc402b 100644 --- a/apps/position_estimator/Makefile +++ b/apps/position_estimator/Makefile @@ -47,6 +47,4 @@ CSRCS = position_estimator_main.c \ codegen/rtGetInf.c \ codegen/rtGetNaN.c -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink - include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator/position_estimator_main.c b/apps/position_estimator/position_estimator_main.c index 17482dc0a8..f8390f1002 100644 --- a/apps/position_estimator/position_estimator_main.c +++ b/apps/position_estimator/position_estimator_main.c @@ -35,8 +35,9 @@ * ****************************************************************************/ -/* - * @file Model-identification based position estimator for multirotors +/** + * @file position_estimator_main.c + * Model-identification based position estimator for multirotors */ #include @@ -45,7 +46,6 @@ #include #include #include -#include #include #include #include @@ -58,6 +58,7 @@ #include #include #include +#include #include #include "codegen/position_estimator.h" @@ -251,14 +252,16 @@ int position_estimator_main(int argc, char *argv[]) bool new_initialization = true; - static double lat_current = 0;//[°]] --> 47.0 - static double lon_current = 0; //[°]] -->8.5 + static double lat_current = 0.0d;//[°]] --> 47.0 + static double lon_current = 0.0d; //[°]] -->8.5 + float alt_current = 0.0f; //TODO: handle flight without gps but with estimator /* 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; @@ -269,7 +272,7 @@ int position_estimator_main(int argc, char *argv[]) int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); /* wait until gps signal turns valid, only then can we initialize the projection */ - while (!gps_valid) { + while (gps.fix_type < 3) { struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} }; /* wait for GPS updates, BUT READ VEHICLE STATUS (!) @@ -281,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); } } @@ -290,14 +293,18 @@ int position_estimator_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); lat_current = ((double)(gps.lat)) * 1e-7; lon_current = ((double)(gps.lon)) * 1e-7; + alt_current = gps.alt * 1e-3; + + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ - struct vehicle_global_position_s global_pos = { - .lat = lat_current * 1e7, - .lon = lon_current * 1e7, - .alt = gps.alt + struct vehicle_local_position_s local_pos = { + .x = 0, + .y = 0, + .z = 0 }; - orb_advert_t global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); @@ -320,91 +327,84 @@ int position_estimator_main(int argc, char *argv[]) u[1] = att.pitch; /* initialize map projection with the last estimate (not at full rate) */ - if (counter % PROJECTION_INITIALIZE_COUNTER_LIMIT == 0) { - map_projection_init(lat_current, lon_current); - new_initialization = true; - - } else { - new_initialization = false; - } - - /*check if new gps values are available */ - gps_valid = vstatus.gps_valid; - - - if (gps_valid) { //we are safe to use the gps signal (it has good quality) - - predict_only = 0; + if (gps.fix_type > 2) { /* Project gps lat lon (Geographic coordinate system) to plane*/ - map_projection_project((double)(gps.lat) * 1e-7, (double)(gps.lon) * 1e-7, &(z[0]), &(z[1])); + map_projection_project(((double)(gps.lat)) * 1e-7, ((double)(gps.lon)) * 1e-7, &(z[0]), &(z[1])); - /* copy altitude */ - z[2] = (gps.alt) * 1e-3; - - gps_covariance[0] = gps.eph; //TODO: needs scaling - gps_covariance[1] = gps.eph; - gps_covariance[2] = gps.epv; - - } else { - /* we can not use the gps signal (it is of low quality) */ - predict_only = 1; - } - - // predict_only = 0; //TODO: only for testing, removeme, XXX - // z[0] = sinf(((float)counter)/180.0f*3.14159265f); //TODO: only for testing, removeme, XXX - // usleep(100000); //TODO: only for testing, removeme, XXX + local_pos.x = z[0]; + local_pos.y = z[1]; + /* negative offset from initialization altitude */ + local_pos.z = alt_current - (gps.alt) * 1e-3; - /*Get new estimation (this is calculated in the plane) */ - //TODO: if new_initialization == true: use 0,0,0, else use xapo - if (true == new_initialization) { //TODO,XXX: uncomment! - xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane - xapo[2] = 0; - xapo[4] = 0; - position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); - - } else { - position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); + orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); } + // gps_covariance[0] = gps.eph; //TODO: needs scaling + // gps_covariance[1] = gps.eph; + // gps_covariance[2] = gps.epv; - /* Copy values from xapo1 to xapo */ - int i; + // } else { + // /* we can not use the gps signal (it is of low quality) */ + // predict_only = 1; + // } - for (i = 0; i < N_STATES; i++) { - xapo[i] = xapo1[i]; - } + // // predict_only = 0; //TODO: only for testing, removeme, XXX + // // z[0] = sinf(((float)counter)/180.0f*3.14159265f); //TODO: only for testing, removeme, XXX + // // usleep(100000); //TODO: only for testing, removeme, XXX - if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) { - /* Reproject from plane to geographic coordinate system */ - // map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment! - map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme - // //DEBUG - // if(counter%500 == 0) - // { - // printf("phi_1: %.10f\n", phi_1); - // printf("lambda_0: %.10f\n", lambda_0); - // printf("lat_estimated: %.10f\n", lat_current); - // printf("lon_estimated: %.10f\n", lon_current); - // printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]); - // fflush(stdout); - // - // } - // if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5])) - // { - /* send out */ + // /*Get new estimation (this is calculated in the plane) */ + // //TODO: if new_initialization == true: use 0,0,0, else use xapo + // if (true == new_initialization) { //TODO,XXX: uncomment! + // xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane + // xapo[2] = 0; + // xapo[4] = 0; + // position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); - global_pos.lat = lat_current; - global_pos.lon = lon_current; - global_pos.alt = xapo1[4]; - global_pos.vx = xapo1[1]; - global_pos.vy = xapo1[3]; - global_pos.vz = xapo1[5]; + // } else { + // position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); + // } + + + + // /* Copy values from xapo1 to xapo */ + // int i; + + // for (i = 0; i < N_STATES; i++) { + // xapo[i] = xapo1[i]; + // } + + // if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) { + // /* Reproject from plane to geographic coordinate system */ + // // map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment! + // map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme + // // //DEBUG + // // if(counter%500 == 0) + // // { + // // printf("phi_1: %.10f\n", phi_1); + // // printf("lambda_0: %.10f\n", lambda_0); + // // printf("lat_estimated: %.10f\n", lat_current); + // // printf("lon_estimated: %.10f\n", lon_current); + // // printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]); + // // fflush(stdout); + // // + // // } + + // // if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5])) + // // { + // /* send out */ + + // global_pos.lat = lat_current; + // global_pos.lon = lon_current; + // global_pos.alt = xapo1[4]; + // global_pos.vx = xapo1[1]; + // global_pos.vy = xapo1[3]; + // global_pos.vz = xapo1[5]; /* publish current estimate */ - orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos); + // orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos); // } // else // { @@ -412,7 +412,7 @@ int position_estimator_main(int argc, char *argv[]) // fflush(stdout); // } - } + // } counter++; } diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index f4af17597f..0426dfb9c0 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) diff --git a/apps/uORB/topics/vehicle_attitude.h b/apps/uORB/topics/vehicle_attitude.h index e365a65574..009379920a 100644 --- a/apps/uORB/topics/vehicle_attitude.h +++ b/apps/uORB/topics/vehicle_attitude.h @@ -74,6 +74,7 @@ struct vehicle_attitude_s { float rollspeed; /**< Roll angular speed (rad/s, Tait-Bryan, NED) LOGME */ float pitchspeed; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) LOGME */ float yawspeed; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) LOGME */ + float rate_offsets[3];/**< Offsets of the body angular rates from zero */ float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ float q[4]; /**< Quaternion (NED) */ bool R_valid; /**< Rotation matrix valid */