diff options
Diffstat (limited to 'apps/px4/attitude_estimator_bm/attitude_estimator_bm.c')
| -rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 283 |
1 files changed, 0 insertions, 283 deletions
diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c deleted file mode 100644 index 60737a654..000000000 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ /dev/null @@ -1,283 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Laurens Mackay <mackayl@student.ethz.ch> - * - * 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_bm.c - * Black Magic Attitude Estimator - */ - - - -#include <nuttx/config.h> -#include <stdio.h> -#include <unistd.h> -#include <sys/time.h> -#include <stdbool.h> -#include <fcntl.h> -#include <drivers/drv_hrt.h> -#include <string.h> -#include <poll.h> -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_status.h> -#include <math.h> -#include <errno.h> - -#include "attitude_bm.h" - -static unsigned int loop_interval_alarm = 4500; // loop interval in microseconds - -__EXPORT int attitude_estimator_bm_main(int argc, char *argv[]); - -/**************************************************************************** - * Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * user_start - ****************************************************************************/ -int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b); - -int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b) -{ - float_vect3 gyro_values; - gyro_values.x = raw->gyro_rad_s[0]; - gyro_values.y = raw->gyro_rad_s[1]; - gyro_values.z = raw->gyro_rad_s[2]; - - float_vect3 accel_values; - accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 100; - accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 100; - accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 100; - - float_vect3 mag_values; - mag_values.x = raw->magnetometer_ga[0]*1500.0f; - mag_values.y = raw->magnetometer_ga[1]*1500.0f; - mag_values.z = raw->magnetometer_ga[2]*1500.0f; - - // static int i = 0; - - // if (i == 500) { - // printf("[att estim bm] gyro: %8.4f\t%8.4f\t%8.4f\t accel: %8.4f\t%8.4f\t%8.4f\t mag: %8.4f\t%8.4f\t%8.4f\t\n", - // gyro_values.x, gyro_values.y, gyro_values.z, - // accel_values.x, accel_values.y, accel_values.z, - // mag_values.x, mag_values.y, mag_values.z); - // i = 0; - // } - // i++; - - attitude_blackmagic(&accel_values, &mag_values, &gyro_values); - - /* read out values */ - attitude_blackmagic_get_all(euler, rates, x_n_b, y_n_b, z_n_b); - - return OK; -} - - -int attitude_estimator_bm_main(int argc, char *argv[]) -{ - // print text - printf("Black Magic Attitude Estimator initialized..\n\n"); - fflush(stdout); - - /* data structures to read euler angles and rotation matrix back */ - float_vect3 euler = {.x = 0, .y = 0, .z = 0}; - float_vect3 rates; - float_vect3 x_n_b; - float_vect3 y_n_b; - float_vect3 z_n_b; - - int overloadcounter = 19; - - /* initialize */ - attitude_blackmagic_init(); - - /* store start time to guard against too slow update rates */ - uint64_t last_run = hrt_absolute_time(); - - struct sensor_combined_s sensor_combined_s_local = { .gyro_raw = {0}}; - struct vehicle_attitude_s att = {.roll = 0.0f, .pitch = 0.0f, .yaw = 0.0f, - .rollspeed = 0.0f, .pitchspeed = 0.0f, .yawspeed = 0.0f, - .R = {0}, .timestamp = 0}; - - uint64_t last_data = 0; - - /* 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); - - bool hil_enabled = false; - bool publishing = false; - - /* advertise attitude */ - orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - publishing = true; - - struct pollfd fds[] = { - { .fd = sub_raw, .events = POLLIN }, - }; - - /* subscribe to system status */ - struct vehicle_status_s vstatus = {0}; - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); - - unsigned int loopcounter = 0; - - uint64_t last_checkstate_stamp = 0; - - /* Main loop*/ - while (true) { - - - /* wait for sensor update */ - 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("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); - } else { - orb_copy(ORB_ID(sensor_combined), sub_raw, &sensor_combined_s_local); - - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; - - //#if 0 - - if (time_elapsed > loop_interval_alarm) { - //TODO: add warning, cpu overload here - if (overloadcounter == 20) { - printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - overloadcounter = 0; - } - - overloadcounter++; - } - - //#endif - // now = hrt_absolute_time(); - /* filter values */ - attitude_estimator_bm_update(&sensor_combined_s_local, &euler, &rates, &x_n_b, &y_n_b, &z_n_b); - - // time_elapsed = hrt_absolute_time() - now; - // if (blubb == 20) - // { - // printf("Estimator: %lu\n", time_elapsed); - // blubb = 0; - // } - // blubb++; - - // if (last_data > 0 && sensor_combined_s_local.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", sensor_combined_s_local.timestamp - last_data); - - // printf("%llu -> %llu = %llu\n", last_data, sensor_combined_s_local.timestamp, sensor_combined_s_local.timestamp - last_data); - // last_data = sensor_combined_s_local.timestamp; - - /*correct yaw */ - // euler.z = euler.z + M_PI; - - /* send out */ - - att.timestamp = sensor_combined_s_local.timestamp; - att.roll = euler.x; - att.pitch = euler.y; - att.yaw = euler.z; - - if (att.yaw > M_PI_F) { - att.yaw -= 2.0f * M_PI_F; - } - - if (att.yaw < -M_PI_F) { - att.yaw += 2.0f * M_PI_F; - } - - att.rollspeed = rates.x; - att.pitchspeed = rates.y; - att.yawspeed = rates.z; - - att.R[0][0] = x_n_b.x; - att.R[0][1] = x_n_b.y; - att.R[0][2] = x_n_b.z; - - // Broadcast - if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); - } - - - // XXX add remaining entries - - if (hrt_absolute_time() - last_checkstate_stamp > 500000) { - /* Check HIL state */ - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); - /* switching from non-HIL to HIL mode */ - //printf("[attitude_estimator_bm] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if (vstatus.flag_hil_enabled && !hil_enabled) { - hil_enabled = true; - publishing = false; - int ret = close(pub_att); - printf("Closing attitude: %i \n", ret); - - /* switching from HIL to non-HIL mode */ - - } else if (!publishing && !hil_enabled) { - /* advertise the topic and make the initial publication */ - pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - hil_enabled = false; - publishing = true; - } - last_checkstate_stamp = hrt_absolute_time(); - } - - loopcounter++; - } - - /* Should never reach here */ - return 0; -} - - |