From 4311ed2df45f70c8ca7307783feac5c74d2bd44d Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 29 Sep 2008 15:24:19 +0000 Subject: [PATCH] removing old quad files --- sw/airborne/booz/quad_control.c | 199 ---------- sw/airborne/booz/quad_controller_main.c | 191 ---------- sw/airborne/booz/quad_controller_telemetry.c | 5 - sw/airborne/booz/quad_controller_telemetry.h | 141 ------- sw/airborne/booz/quad_gps.c | 97 ----- sw/airborne/booz/quad_gps.h | 76 ---- sw/airborne/booz/quad_ins.h | 71 ---- sw/airborne/booz/quad_xsens.c | 374 ------------------- 8 files changed, 1154 deletions(-) delete mode 100644 sw/airborne/booz/quad_control.c delete mode 100644 sw/airborne/booz/quad_controller_main.c delete mode 100644 sw/airborne/booz/quad_controller_telemetry.c delete mode 100644 sw/airborne/booz/quad_controller_telemetry.h delete mode 100644 sw/airborne/booz/quad_gps.c delete mode 100644 sw/airborne/booz/quad_gps.h delete mode 100644 sw/airborne/booz/quad_ins.h delete mode 100644 sw/airborne/booz/quad_xsens.c diff --git a/sw/airborne/booz/quad_control.c b/sw/airborne/booz/quad_control.c deleted file mode 100644 index 203c7cdb8f..0000000000 --- a/sw/airborne/booz/quad_control.c +++ /dev/null @@ -1,199 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "booz_control.h" - -#include "airframe.h" -#include "booz_estimator.h" -#include "booz_autopilot.h" -#include "radio_control.h" - -#define BOOZ_CONTROL_MIN_THROTTLE 0.05 - -float booz_control_p_sp; -float booz_control_q_sp; -float booz_control_r_sp; -float booz_control_power_sp; - -float booz_control_rate_pq_pgain; -float booz_control_rate_pq_dgain; -float booz_control_rate_r_pgain; -float booz_control_rate_r_dgain; -float booz_control_rate_last_err_p; -float booz_control_rate_last_err_q; -float booz_control_rate_last_err_r; - -pprz_t booz_control_commands[COMMANDS_NB]; - -float booz_control_attitude_phi_sp; -float booz_control_attitude_phi_dot_sp; -float booz_control_attitude_theta_sp; -float booz_control_attitude_theta_dot_sp; -float booz_control_attitude_psi_sp; -float booz_control_attitude_phi_theta_pgain; -float booz_control_attitude_phi_theta_dgain; -float booz_control_attitude_psi_pgain; -float booz_control_attitude_psi_dgain; - -#define BOOZ_CONTROL_ATTITUDE_XI 1.0 -#define BOOZ_CONTROL_ATTITUDE_W 10.0 -float booz_control_attitude_w; - -#define BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP (1./50.) - -void booz_control_init(void) { - - booz_control_p_sp = 0.; - booz_control_q_sp = 0.; - booz_control_r_sp = 0.; - booz_control_power_sp = 0.; - - booz_control_rate_last_err_p = 0.; - booz_control_rate_last_err_q = 0.; - booz_control_rate_last_err_r = 0.; - - booz_control_rate_pq_pgain = BOOZ_CONTROL_RATE_PQ_PGAIN; - booz_control_rate_pq_dgain = BOOZ_CONTROL_RATE_PQ_DGAIN; - booz_control_rate_r_pgain = BOOZ_CONTROL_RATE_R_PGAIN; - booz_control_rate_r_dgain = BOOZ_CONTROL_RATE_R_DGAIN; - - - booz_control_attitude_phi_sp = 0.; - booz_control_attitude_phi_dot_sp = 0.; - booz_control_attitude_theta_sp = 0.; - booz_control_attitude_theta_dot_sp = 0.; - booz_control_attitude_psi_sp = 0.; - booz_control_attitude_phi_theta_pgain = BOOZ_CONTROL_ATTITUDE_PHI_THETA_PGAIN; - booz_control_attitude_phi_theta_dgain = BOOZ_CONTROL_ATTITUDE_PHI_THETA_DGAIN; - booz_control_attitude_psi_pgain = BOOZ_CONTROL_ATTITUDE_PSI_PGAIN; - booz_control_attitude_psi_dgain = BOOZ_CONTROL_ATTITUDE_PSI_DGAIN; - booz_control_attitude_w = BOOZ_CONTROL_ATTITUDE_W; -} - - -void booz_control_rate_read_setpoints_from_rc(void) { - - booz_control_p_sp = -rc_values[RADIO_ROLL] * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ; - booz_control_q_sp = rc_values[RADIO_PITCH] * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ; - booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ; - booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ; - -} - - -void booz_control_rate_run(void) { - - if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) { - booz_control_commands[COMMAND_P] = 0; - booz_control_commands[COMMAND_Q] = 0; - booz_control_commands[COMMAND_R] = 0; - booz_control_commands[COMMAND_THROTTLE] = 0; - } - else { - const float rate_err_p = booz_estimator_uf_p - booz_control_p_sp; - const float rate_d_err_p = rate_err_p - booz_control_rate_last_err_p; - booz_control_rate_last_err_p = rate_err_p; - const float cmd_p = booz_control_rate_pq_pgain * ( rate_err_p + booz_control_rate_pq_dgain * rate_d_err_p ); - - const float rate_err_q = booz_estimator_uf_q - booz_control_q_sp; - const float rate_d_err_q = rate_err_q - booz_control_rate_last_err_q; - booz_control_rate_last_err_q = rate_err_q; - const float cmd_q = booz_control_rate_pq_pgain * ( rate_err_q + booz_control_rate_pq_dgain * rate_d_err_q ); - - const float rate_err_r = booz_estimator_uf_r - booz_control_r_sp; - const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r; - booz_control_rate_last_err_r = rate_err_r; - const float cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r ); - - booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p); - booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q); - booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r); - booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ)); - } - -} - -void booz_control_attitude_read_setpoints_from_rc(void) { - - booz_control_attitude_phi_dot_sp += BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP * (booz_control_attitude_w*booz_control_attitude_w * - (-rc_values[RADIO_ROLL]*RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ - booz_control_attitude_phi_sp) - - 2.*BOOZ_CONTROL_ATTITUDE_XI*booz_control_attitude_w * booz_control_attitude_phi_dot_sp); - booz_control_attitude_phi_sp += BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP * booz_control_attitude_phi_dot_sp; - - booz_control_attitude_theta_dot_sp += BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP * (booz_control_attitude_w*booz_control_attitude_w * - (rc_values[RADIO_PITCH]*RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ - booz_control_attitude_theta_sp) - - 2*BOOZ_CONTROL_ATTITUDE_XI*booz_control_attitude_w * booz_control_attitude_theta_dot_sp); - booz_control_attitude_theta_sp += BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP * booz_control_attitude_theta_dot_sp; - - if (booz_autopilot_mode == BOOZ_AP_MODE_ATTITUDE) { - booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ; - } - else { /* BOOZ_AP_MODE_HEADING_HOLD */ - if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) - booz_control_attitude_psi_sp = booz_estimator_psi; - else { - booz_control_attitude_psi_sp += -rc_values[RADIO_YAW] * - RadOfDeg(BOOZ_CONTROL_ATTITUDE_PSI_MAX_SP)*BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP/MAX_PPRZ; - } - } - booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ; -} - -void booz_control_attitude_run(void) { - - if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) { - booz_control_commands[COMMAND_P] = 0; - booz_control_commands[COMMAND_Q] = 0; - booz_control_commands[COMMAND_R] = 0; - booz_control_commands[COMMAND_THROTTLE] = 0; - } - else { - const float att_err_phi = booz_estimator_phi - booz_control_attitude_phi_sp; - const float cmd_p = booz_control_attitude_phi_theta_pgain * att_err_phi + - booz_control_attitude_phi_theta_dgain * booz_estimator_p ; - - const float att_err_theta = booz_estimator_theta - booz_control_attitude_theta_sp; - const float cmd_q = booz_control_attitude_phi_theta_pgain * att_err_theta + - booz_control_attitude_phi_theta_dgain * booz_estimator_q; - - float cmd_r; - if (booz_autopilot_mode == BOOZ_AP_MODE_ATTITUDE) { - const float rate_err_r = booz_estimator_r - booz_control_r_sp; - const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r; - booz_control_rate_last_err_r = rate_err_r; - cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r ); - } - else { - float att_err_psi = booz_estimator_psi - booz_control_attitude_psi_sp; - NormRadAngle(att_err_psi); - cmd_r = booz_control_attitude_psi_pgain * att_err_psi + - booz_control_attitude_psi_dgain * booz_estimator_r; - } - - booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p); - booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q); - booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r); - booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ)); - } -} diff --git a/sw/airborne/booz/quad_controller_main.c b/sw/airborne/booz/quad_controller_main.c deleted file mode 100644 index 0e1a627987..0000000000 --- a/sw/airborne/booz/quad_controller_main.c +++ /dev/null @@ -1,191 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "booz_controller_main.h" - -#include "std.h" -#include "init_hw.h" -#include "interrupt_hw.h" -#include "sys_time.h" -#include "led.h" - -#include "booz_energy.h" - -#include "commands.h" -#include "actuators.h" -#include "radio_control.h" - -#include "adc.h" -#include "quad_ins.h" - -#include "booz_estimator.h" -#include "booz_control.h" -#include "booz_nav.h" -#include "booz_autopilot.h" - -#include "uart.h" -#include "messages.h" -#include "downlink.h" -#include "quad_controller_telemetry.h" -#include "datalink.h" - - - -int16_t trim_p = 0; -int16_t trim_q = 0; -int16_t trim_r = 0; -uint8_t vbat = 0; - -uint32_t t0,diff; - -#ifndef SITL -int main( void ) { - booz_controller_main_init(); - while(1) { - if (sys_time_periodic()) - booz_controller_main_periodic_task(); - booz_controller_main_event_task(); - } - return 0; -} -#endif - -STATIC_INLINE void booz_controller_main_init( void ) { - - hw_init(); - led_init(); - sys_time_init(); - - adc_init(); - booz_energy_init(); - -#ifdef USE_UART0 - Uart0Init(); -#endif -#ifdef USE_UART1 - Uart1Init(); -#endif - - actuators_init(); - SetCommands(commands_failsafe); - - ppm_init(); - radio_control_init(); - - quad_ins_init(); - - booz_estimator_init(); - booz_control_init(); - booz_nav_init(); - booz_autopilot_init(); - - int_enable(); - - DOWNLINK_SEND_BOOT(&cpu_time_sec); - - t0 = T0TC; -} - - -#define PeriodicPrescaleBy2( _code_0, _code_1) { \ - static uint8_t _50hz = 0; \ - _50hz++; \ - if (_50hz >= 2) _50hz = 0; \ - switch (_50hz) { \ - case 0: \ - _code_0; \ - break; \ - case 1: \ - _code_1; \ - break; \ - } \ - } - -STATIC_INLINE void booz_controller_main_periodic_task( void ) { - - t0 = T0TC; - - quad_ins_periodic_task(); - /* run control loops */ - booz_autopilot_periodic_task(); - - // commands[COMMAND_P] = 0; - // commands[COMMAND_Q] = 0; - // commands[COMMAND_R] = 0; - // commands[COMMAND_THROTTLE] = MAX_PPRZ/3; - - SetActuatorsFromCommands(commands); - - PeriodicPrescaleBy2( \ - { \ - radio_control_periodic_task(); \ - if (rc_status != RC_OK) \ - booz_autopilot_mode = BOOZ_AP_MODE_FAILSAFE; \ - }, \ - { \ - booz_energy_periodic(); \ - booz_controller_telemetry_periodic_task(); \ - } \ - ); \ - - diff = (T0TC - t0)/SYS_TICS_OF_USEC(1); - DOWNLINK_SEND_TIME(&diff); -} - -STATIC_INLINE void booz_controller_main_event_task( void ) { - - // FIXME -#ifndef SITL -#if DATALINK == PPRZ - if (PprzBuffer()) { - ReadPprzBuffer(); - if (pprz_msg_received) { - pprz_parse_payload(); - pprz_msg_received = FALSE; - } - } -#elif DATALINK == XBEE - if (XBeeBuffer()) { - ReadXBeeBuffer(); - if (xbee_msg_received) { - xbee_parse_payload(); - xbee_msg_received = FALSE; - } - } -#elif -#error "Unknown DATALINK" -#endif - - if (dl_msg_available) { - dl_parse_msg(); - dl_msg_available = FALSE; - } - //DlEventCheckAndHandle(); -#endif - - QuadInsEventCheckAndHandle(booz_estimator_read_inter_mcu_state); - - RadioControlEventCheckAndHandle(booz_autopilot_on_rc_event); - -} diff --git a/sw/airborne/booz/quad_controller_telemetry.c b/sw/airborne/booz/quad_controller_telemetry.c deleted file mode 100644 index 0316313598..0000000000 --- a/sw/airborne/booz/quad_controller_telemetry.c +++ /dev/null @@ -1,5 +0,0 @@ -#include "quad_controller_telemetry.h" - - -uint8_t telemetry_mode_Controller; - diff --git a/sw/airborne/booz/quad_controller_telemetry.h b/sw/airborne/booz/quad_controller_telemetry.h deleted file mode 100644 index a13a5c206d..0000000000 --- a/sw/airborne/booz/quad_controller_telemetry.h +++ /dev/null @@ -1,141 +0,0 @@ -#ifndef BOOZ_CONTROLLER_TELEMETRY_H -#define BOOZ_CONTROLLER_TELEMETRY_H - -#include "std.h" -#include "messages.h" -#include "periodic.h" -#include "uart.h" - -#include "booz_energy.h" -#include "radio_control.h" -#include "actuators.h" -#include "booz_estimator.h" -#include "booz_autopilot.h" -#include "booz_control.h" -#include "booz_nav.h" -#include "quad_gps.h" - -#include "settings.h" - -#include "downlink.h" - -#define PERIODIC_SEND_ALIVE() DOWNLINK_SEND_ALIVE(16, MD5SUM) - -#define PERIODIC_SEND_ATTITUDE() { \ - int16_t phi = DegOfRad(booz_estimator_phi); \ - int16_t psi = DegOfRad(booz_estimator_psi); \ - int16_t theta = DegOfRad(booz_estimator_theta); \ - DOWNLINK_SEND_ATTITUDE(&phi, &psi, &theta); \ -} - -#define PERIODIC_SEND_ESTIMATOR() DOWNLINK_SEND_ESTIMATOR(&booz_estimator_z, &booz_estimator_vz) - -#define PERIODIC_SEND_BAT() { \ - int16_t t = 0; \ - uint16_t time = 0; \ - uint8_t kill = 0; if (booz_autopilot_mode == BOOZ_AP_MODE_KILL) kill = 1; \ - DOWNLINK_SEND_BAT(&t, &booz_energy_bat, &booz_estimator_flight_time, &kill, &time, &time, &booz_energy_current); \ -} - -#define PERIODIC_SEND_PPRZ_MODE() { \ - uint8_t pprz_mode; \ - switch (booz_autopilot_mode) { \ - case BOOZ_AP_MODE_RATE: \ - pprz_mode = 0; /*MANUAL*/ break; \ - case BOOZ_AP_MODE_ATTITUDE: \ - case BOOZ_AP_MODE_HEADING_HOLD: \ - pprz_mode = 1; /*AUTO1*/ break; \ - case BOOZ_AP_MODE_NAV: \ - pprz_mode = 2; /*AUTO2*/ break; \ - case BOOZ_AP_MODE_FAILSAFE: \ - case BOOZ_AP_MODE_KILL: \ - pprz_mode = 5; /*FAIL*/ break; \ - } \ - uint8_t gaz = 0; \ - uint8_t lat = 0; \ - uint8_t hor = 0; \ - uint8_t calib = 0; \ - uint8_t rc; \ - switch (rc_status) { \ - case RC_OK: \ - rc = 1; /*OK*/ break; \ - case RC_LOST: \ - rc = 2; /*LOST*/ break; \ - case RC_REALLY_LOST: \ - rc = 0; /*NONE*/ break; \ - } \ - DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &gaz, &lat, &hor, &calib, &rc); \ -} - -#define PERIODIC_SEND_BOOZ_STATUS() \ - DOWNLINK_SEND_QUAD_STATUS( \ - &rc_status, \ - &booz_autopilot_mode, \ - &booz_energy_bat, \ - &cpu_time_sec) - -#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(PPM_NB_PULSES, rc_values) - -#define PERIODIC_SEND_BOOZ_FD() \ - DOWNLINK_SEND_BOOZ_FD(&booz_estimator_p, \ - &booz_estimator_q, \ - &booz_estimator_r, \ - &booz_estimator_phi, \ - &booz_estimator_theta, \ - &booz_estimator_psi); - -#define PERIODIC_SEND_ACTUATORS() \ - DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators); - -#define PERIODIC_SEND_BOOZ_CONTROL() { \ - switch (booz_autopilot_mode) { \ - case BOOZ_AP_MODE_RATE: \ - DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_uf_p, &booz_control_p_sp, \ - &booz_estimator_uf_q, &booz_control_q_sp, \ - &booz_estimator_uf_r, &booz_control_r_sp, \ - &booz_control_power_sp); \ - break; \ - case BOOZ_AP_MODE_ATTITUDE: \ - case BOOZ_AP_MODE_HEADING_HOLD: \ - case BOOZ_AP_MODE_NAV: \ - DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_attitude_phi_sp, \ - &booz_estimator_theta, &booz_control_attitude_theta_sp, \ - &booz_estimator_psi, &booz_control_attitude_psi_sp, \ - &booz_control_power_sp); \ - break; \ - } \ - } - -#define PERIODIC_SEND_BOOZ_VERT_LOOP() { \ - float sp = -booz_nav_hover_z_sp; \ - float z = -booz_estimator_z; \ - float vz = -booz_estimator_w; \ - DOWNLINK_SEND_BOOZ_VERT_LOOP(&sp,&vz,&z, \ - &booz_nav_hover_power_command); \ - } - -#define PERIODIC_SEND_BOOZ_HOV_LOOP() { \ - DOWNLINK_SEND_BOOZ_HOV_LOOP(&booz_nav_hover_x_sp, \ - &booz_nav_hover_y_sp, \ - &booz_estimator_u, \ - &booz_estimator_x, \ - &booz_estimator_v, \ - &booz_estimator_y, \ - &booz_nav_hover_phi_command, \ - &booz_nav_hover_theta_command); \ - } - -#define PERIODIC_SEND_BOOZ_CMDS() DOWNLINK_SEND_BOOZ_CMDS(&actuators[SERVO_MOTOR_FRONT],&actuators[SERVO_MOTOR_BACK],&actuators[SERVO_MOTOR_RIGHT],&actuators[SERVO_MOTOR_LEFT]) - -#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue() - -#define PERIODIC_SEND_GPS() gps_downlink(); - -extern uint8_t telemetry_mode_Controller; - -static inline void booz_controller_telemetry_periodic_task(void) { - PeriodicSendController() -} - - -#endif /* BOOZ_CONTROLLER_TELEMETRY_H */ diff --git a/sw/airborne/booz/quad_gps.c b/sw/airborne/booz/quad_gps.c deleted file mode 100644 index e717445769..0000000000 --- a/sw/airborne/booz/quad_gps.c +++ /dev/null @@ -1,97 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2005 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** \file gps.c - * \brief GPS hardware independent handling - * - */ - -#include - -#include "quad_gps.h" -//#include "latlong.h" -#include "sys_time.h" -#include "airframe.h" - -#include "uart.h" -#include "messages.h" -#include "downlink.h" - -uint8_t gps_mode; -uint32_t gps_itow; -int32_t gps_alt; -uint16_t gps_gspeed; -int16_t gps_climb; -int16_t gps_course; -int32_t gps_utm_east, gps_utm_north; -uint8_t gps_utm_zone; -int32_t gps_lat, gps_lon; -uint16_t gps_PDOP; -uint32_t gps_Pacc, gps_Sacc; -uint8_t gps_numSV; -uint8_t gps_configuring; - -uint16_t gps_reset; - -uint16_t last_gps_msg_t; -bool_t gps_verbose_downlink; - -volatile bool_t gps_msg_received; -bool_t gps_pos_available; -uint8_t gps_nb_ovrn; - -uint8_t gps_nb_channels; - -struct svinfo gps_svinfos[GPS_NB_CHANNELS]; - - -void gps_downlink( void ) { - static uint8_t _4Hz; - _4Hz++; if (_4Hz > 3) _4Hz = 0; - -#if defined DOWNLINK_GPS_1Hz - if (_4Hz == 0) -#endif - DOWNLINK_SEND_GPS(&gps_mode, &gps_utm_east, &gps_utm_north, &gps_course, &gps_alt, &gps_gspeed,&gps_climb, &gps_itow, &gps_utm_zone, &gps_nb_ovrn); - - static uint8_t i; - static uint8_t last_cnos[GPS_NB_CHANNELS]; - if (i >= gps_nb_channels) i = 0; - if (i < gps_nb_channels && gps_svinfos[i].cno > 0 && gps_svinfos[i].cno != last_cnos[i]) { - DOWNLINK_SEND_SVINFO(&i, &gps_svinfos[i].svid, &gps_svinfos[i].flags, &gps_svinfos[i].qi, &gps_svinfos[i].cno, &gps_svinfos[i].elev, &gps_svinfos[i].azim); - last_cnos[i] = gps_svinfos[i].cno; - } - - if (gps_verbose_downlink) { - uint8_t j; - for(j = 0; j < gps_nb_channels; j++) { - uint8_t cno = gps_svinfos[j].cno; - if (cno > 0 && j != i && abs(cno-last_cnos[j]) >= 2) { - DOWNLINK_SEND_SVINFO(&j, &gps_svinfos[j].svid, &gps_svinfos[j].flags, &gps_svinfos[j].qi, &cno, &gps_svinfos[j].elev, &gps_svinfos[j].azim); - last_cnos[j] = gps_svinfos[j].cno; - } - } - } - i++; -} diff --git a/sw/airborne/booz/quad_gps.h b/sw/airborne/booz/quad_gps.h deleted file mode 100644 index 0db58566b1..0000000000 --- a/sw/airborne/booz/quad_gps.h +++ /dev/null @@ -1,76 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2003 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** \file booz_gps.h - * \brief Device independent GPS code - * -*/ - - -#ifndef BOOZ_GPS_H -#define BOOZ_GPS_H - -#include "std.h" - -#define GPS_NB_CHANNELS 16 - -extern uint8_t gps_mode; /* Receiver status */ -extern uint32_t gps_itow; /* ms */ -extern int32_t gps_alt; /* cm */ -extern uint16_t gps_gspeed; /* cm/s */ -extern int16_t gps_climb; /* m/s */ -extern int16_t gps_course; /* decideg */ -extern int32_t gps_utm_east, gps_utm_north; /** cm */ -extern uint8_t gps_utm_zone; -extern int32_t gps_lat, gps_lon; /* 1e7 deg */ -extern uint16_t gps_PDOP; -extern uint32_t gps_Pacc, gps_Sacc; -extern uint8_t gps_numSV; -extern uint8_t gps_configuring; - -extern uint16_t last_gps_msg_t; /** cputime of the last gps message */ -extern bool_t gps_verbose_downlink; - -extern volatile uint8_t gps_msg_received; -extern bool_t gps_pos_available; -extern uint8_t gps_nb_ovrn; - -/** Number of scanned satellites */ -extern uint8_t gps_nb_channels; - -/** Space Vehicle Information */ -struct svinfo { - uint8_t svid; - uint8_t flags; - uint8_t qi; - uint8_t cno; - int8_t elev; /** deg */ - int16_t azim; /** deg */ -}; - -extern struct svinfo gps_svinfos[GPS_NB_CHANNELS]; - -extern void gps_downlink( void ); - -#endif /* BOOZ_GPS_H */ diff --git a/sw/airborne/booz/quad_ins.h b/sw/airborne/booz/quad_ins.h deleted file mode 100644 index 2a0d8b9034..0000000000 --- a/sw/airborne/booz/quad_ins.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2003 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** \file quad_ins.h - * \brief Device independent INS code - * -*/ - - -#ifndef QUAD_INS_H -#define QUAD_INS_H - -#include "std.h" - -extern volatile uint8_t ins_msg_received; - -extern void quad_ins_init( void ); -extern void quad_ins_periodic_task( void ); -void parse_ins_msg( void ); -void parse_ins_buffer( uint8_t ); - -#ifndef SITL -#include "uart.h" - -#define __InsLink(dev, _x) dev##_x -#define _InsLink(dev, _x) __InsLink(dev, _x) -#define InsLink(_x) _InsLink(INS_LINK, _x) - -#define InsBuffer() InsLink(ChAvailable()) -#define ReadInsBuffer() { while (InsLink(ChAvailable())&&!ins_msg_received) parse_ins_buffer(InsLink(Getch())); } -#define InsUartSend1(c) InsLink(Transmit(c)) -#define InsUartInitParam(_a,_b,_c) InsLink(InitParam(_a,_b,_c)) -#define InsUartRunning InsLink(TxRunning) - -#endif /** !SITL */ - -#define QuadInsEventCheckAndHandle(handler) { \ - if (InsBuffer()) { \ - ReadInsBuffer(); \ - } \ - if (ins_msg_received) { \ - LED_TOGGLE(2); \ - parse_ins_msg(); \ - handler(); \ - ins_msg_received = FALSE; \ - } \ -} - - -#endif /* QUAD_INS_H */ diff --git a/sw/airborne/booz/quad_xsens.c b/sw/airborne/booz/quad_xsens.c deleted file mode 100644 index 17886706c6..0000000000 --- a/sw/airborne/booz/quad_xsens.c +++ /dev/null @@ -1,374 +0,0 @@ -/* - * Paparazzi mcu0 $Id$ - * - * Copyright (C) 2003 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** \file xsens.c - * \brief Parser for the Xsens protocol - */ - -#include "quad_ins.h" - -#include - -#include "airframe.h" -#include "led.h" -#include "booz_estimator.h" -#include "quad_gps.h" -#include "quad_vfilter.h" -#include "agl_vfilter.h" -#include "booz_inter_mcu.h" -#include "booz_autopilot.h" - -#include "downlink.h" -#include "messages.h" - -#include "latlong.h" - -/* comes from booz_inter_mcu.h */ -struct booz_inter_mcu_state inter_mcu_state; - -volatile uint8_t ins_msg_received; - -#define XsensInitCheksum() { send_ck = 0; } -#define XsensUpdateChecksum(c) { send_ck += c; } - -#define XsensSend1(c) { uint8_t i8=c; InsUartSend1(i8); XsensUpdateChecksum(i8); } -#define XsensSend1ByAddr(x) { XsensSend1(*x); } -#define XsensSend2ByAddr(x) { XsensSend1(*(x+1)); XsensSend1(*x); } -#define XsensSend4ByAddr(x) { XsensSend1(*(x+3)); XsensSend1(*(x+2)); XsensSend1(*(x+1)); XsensSend1(*x); } - -#define XsensHeader(msg_id, len) { \ - InsUartSend1(XSENS_START); \ - XsensInitCheksum(); \ - XsensSend1(XSENS_BID); \ - XsensSend1(msg_id); \ - XsensSend1(len); \ -} -#define XsensTrailer() { uint8_t i8=0x100-send_ck; InsUartSend1(i8); } - -/** Includes macros generated from xsens_MTi-G.xml */ -#include "xsens_protocol.h" - - -#define XSENS_MAX_PAYLOAD 254 -uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]; - -/* output mode : calibrated, orientation, position, velocity, status */ -#ifndef XSENS_OUTPUT_MODE -#define XSENS_OUTPUT_MODE 0x0836 -#endif -/* output settings : timestamp, euler, acc, rate, mag, float, no aux, lla, m/s, NED */ -#ifndef XSENS_OUTPUT_SETTINGS -#define XSENS_OUTPUT_SETTINGS 0x80000C05 -#endif - -#ifndef XSENS_NED -#define XSENS_NED 1 -#endif - -#define UNINIT 0 -#define GOT_START 1 -#define GOT_BID 2 -#define GOT_MID 3 -#define GOT_LEN 4 -#define GOT_DATA 5 -#define GOT_CHECKSUM 6 - -uint8_t xsens_errorcode; -uint8_t xsens_msg_status; -uint16_t xsens_time_stamp; -uint16_t xsens_output_mode; -uint32_t xsens_output_settings; - -static uint8_t xsens_id; -static uint8_t xsens_status; -static uint8_t xsens_len; -static uint8_t xsens_msg_idx; -static uint8_t ck; -uint8_t send_ck; - -void quad_ins_init( void ) { - -//#ifdef NO_INTERNAL_GPS -// // disconnect the internal GPS -// Configure_GPS_RESET_Pin(); -// Set_GPS_RESET_Pin_LOW(); -//#endif - - xsens_status = UNINIT; - - xsens_msg_status = 0; - xsens_time_stamp = 0; - xsens_output_mode = XSENS_OUTPUT_MODE; - xsens_output_settings = XSENS_OUTPUT_SETTINGS; - /* send mode and settings to MT */ - XSENS_GoToConfig(); - XSENS_SetOutputMode(xsens_output_mode); - XSENS_SetOutputSettings(xsens_output_settings); - //XSENS_GoToMeasurment(); -} - -void quad_ins_periodic_task( void ) { - static uint8_t _1hz = 0; - _1hz++; - if (_1hz >= 100) _1hz = 0; - switch (_1hz) { - case 0: - //XSENS_GoToConfig(); - XSENS_ReqGPSStatus(); - //XSENS_GoToMeasurment(); - break; - } -} - -void parse_ins_msg( void ) { - uint8_t offset = 0; - if (xsens_id == XSENS_ReqOutputModeAck_ID) { - xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf); - } - else if (xsens_id == XSENS_ReqOutputSettings_ID) { - xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf); - } - else if (xsens_id == XSENS_Error_ID) { - xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf); - } - else if (xsens_id == XSENS_GPSStatus_ID) { - //DOWNLINK_SEND_DEBUG(xsens_len,xsens_msg_buf); - gps_nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf); - uint8_t i; - for(i = 0; i < Min(gps_nb_channels, GPS_NB_CHANNELS); i++) { - uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i); - if (ch > GPS_NB_CHANNELS) continue; - gps_svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i); - gps_svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i); - gps_svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i); - gps_svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i); - //DOWNLINK_SEND_SVINFO(&ch, &gps_svinfos[ch].svid, &gps_svinfos[ch].flags, &gps_svinfos[ch].qi, &gps_svinfos[ch].cno,&gps_svinfos[ch].elev, &gps_svinfos[ch].azim); - } - } - else if (xsens_id == XSENS_MTData_ID) { - //uint8_t offset = 0; - /* test RAW modes else calibrated modes */ - //if ((XSENS_MASK_RAWInertial(xsens_output_mode)) || (XSENS_MASK_RAWGPS(xsens_output_mode))) { - if (XSENS_MASK_RAWInertial(xsens_output_mode)) { - booz_estimator_uf_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf,offset); - booz_estimator_p = booz_estimator_uf_p; - booz_estimator_uf_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf,offset); - booz_estimator_q = booz_estimator_uf_q; - booz_estimator_uf_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf,offset); - booz_estimator_r = booz_estimator_uf_r; - //booz_autopilot_mode = BOOZ_AP_MODE_RATE; - offset += XSENS_DATA_RAWInertial_LENGTH; - } - if (XSENS_MASK_RAWGPS(xsens_output_mode)) { - gps_itow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset); - gps_lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset); - gps_lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset); -#ifdef NAV_HORIZONTAL - /* Set the real UTM zone */ - gps_utm_zone = (gps_lon/1e7+180) / 6 + 1; - latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), gps_utm_zone); - /* utm */ - gps_utm_east = latlong_utm_x * 100; - gps_utm_north = latlong_utm_y * 100; - booz_estimator_x = latlong_utm_x; - booz_estimator_y = latlong_utm_y; -#endif - gps_alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 10; - booz_estimator_z = -(float)gps_alt / 100.; - booz_estimator_vx = (float)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.; - booz_estimator_vy = (float)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.; - booz_estimator_vz = (float)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.; - gps_climb = -XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 10; - gps_Pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset); - gps_Sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset); - offset += XSENS_DATA_RAWGPS_LENGTH; - } - //} else { - if (XSENS_MASK_Temp(xsens_output_mode)) { - offset += XSENS_DATA_Temp_LENGTH; - } - if (XSENS_MASK_Calibrated(xsens_output_mode)) { - uint8_t l = 0; - if (!XSENS_MASK_AccOut(xsens_output_settings)) { - inter_mcu_state.accel[AXIS_X] = XSENS_DATA_Calibrated_accX(xsens_msg_buf,offset); - inter_mcu_state.accel[AXIS_Y] = XSENS_NED * XSENS_DATA_Calibrated_accY(xsens_msg_buf,offset); - inter_mcu_state.accel[AXIS_Z] = XSENS_NED * XSENS_DATA_Calibrated_accZ(xsens_msg_buf,offset); -#ifdef USE_VFILTER -#ifdef USE_BARO_MS5534A - inter_mcu_state.pos[AXIS_Z] = quad_vf_z; - inter_mcu_state.speed[AXIS_Z] = quad_vf_zdot; - gps_alt = inter_mcu_state.pos[AXIS_Z] * 100; - gps_climb = (int16_t)(inter_mcu_state.speed[AXIS_Z] * 100); -#endif -#ifdef USE_TELEMETER - booz_estimator_agl_z = agl_vf_z; - booz_estimator_agl_zdot = agl_vf_zdot; -#endif -#endif - l++; - } - if (!XSENS_MASK_GyrOut(xsens_output_settings)) { - inter_mcu_state.r_rates[AXIS_P] = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf,offset) * RATE_PI_S/M_PI; - inter_mcu_state.r_rates[AXIS_Q] = XSENS_NED * XSENS_DATA_Calibrated_gyrY(xsens_msg_buf,offset) * RATE_PI_S/M_PI; - inter_mcu_state.r_rates[AXIS_R] = XSENS_NED * XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf,offset) * RATE_PI_S/M_PI; - inter_mcu_state.f_rates[AXIS_P] = inter_mcu_state.r_rates[AXIS_P]; - inter_mcu_state.f_rates[AXIS_Q] = inter_mcu_state.r_rates[AXIS_Q]; - inter_mcu_state.f_rates[AXIS_R] = inter_mcu_state.r_rates[AXIS_R]; - l++; - } - if (!XSENS_MASK_MagOut(xsens_output_settings)) { - l++; - } - offset += l * XSENS_DATA_Calibrated_LENGTH / 3; - } - if (XSENS_MASK_Orientation(xsens_output_mode)) { - if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) { - float q0 = XSENS_DATA_Quaternion_q0(xsens_msg_buf,offset); - float q1 = XSENS_DATA_Quaternion_q1(xsens_msg_buf,offset); - float q2 = XSENS_DATA_Quaternion_q2(xsens_msg_buf,offset); - float q3 = XSENS_DATA_Quaternion_q3(xsens_msg_buf,offset); - float dcm00 = 1.0 - 2 * (q2*q2 + q3*q3); - float dcm01 = 2 * (q1*q2 + q0*q3); - float dcm02 = 2 * (q1*q3 - q0*q2); - float dcm12 = 2 * (q2*q3 + q0*q1); - float dcm22 = 1.0 - 2 * (q1*q1 + q2*q2); - inter_mcu_state.f_eulers[AXIS_X] = atan2(dcm12, dcm22) * ANGLE_PI/M_PI; - inter_mcu_state.f_eulers[AXIS_Y] = -asin(dcm02) * ANGLE_PI/M_PI; - inter_mcu_state.f_eulers[AXIS_Z] = atan2(dcm01, dcm00) * ANGLE_PI/M_PI; - gps_course = inter_mcu_state.f_eulers[AXIS_Z] * 1800 / ANGLE_PI; - offset += XSENS_DATA_Quaternion_LENGTH; - } - if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) { - inter_mcu_state.f_eulers[AXIS_X] = XSENS_DATA_Euler_roll(xsens_msg_buf,offset) * ANGLE_PI/180; - inter_mcu_state.f_eulers[AXIS_Y] = XSENS_NED * XSENS_DATA_Euler_pitch(xsens_msg_buf,offset) * ANGLE_PI/180; - inter_mcu_state.f_eulers[AXIS_Z] = XSENS_NED * XSENS_DATA_Euler_yaw(xsens_msg_buf,offset) * ANGLE_PI/180; - gps_course = (int16_t)(XSENS_NED * XSENS_DATA_Euler_yaw(xsens_msg_buf,offset) * 10); - offset += XSENS_DATA_Euler_LENGTH; - } - if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) { - offset += XSENS_DATA_Matrix_LENGTH; - } - } - if (XSENS_MASK_Auxiliary(xsens_output_mode)) { - uint8_t l = 0; - if (!XSENS_MASK_Aux1Out(xsens_output_settings)) { - l++; - } - if (!XSENS_MASK_Aux2Out(xsens_output_settings)) { - l++; - } - offset += l * XSENS_DATA_Auxiliary_LENGTH / 2; - } - if (XSENS_MASK_Position(xsens_output_mode)) { - //DOWNLINK_SEND_DEBUG(xsens_len,xsens_msg_buf); - float lat = XSENS_DATA_Position_lat(xsens_msg_buf,offset); - float lon = XSENS_DATA_Position_lon(xsens_msg_buf,offset); - gps_lat = (int32_t)(lat * 1e7); - gps_lon = (int32_t)(lon * 1e7); -//#ifdef NAV_HORIZONTAL - gps_utm_zone = (lon+180) / 6 + 1; - latlong_utm_of(RadOfDeg(lat), RadOfDeg(lon), gps_utm_zone); -//#endif - inter_mcu_state.pos[AXIS_X] = latlong_utm_x; - inter_mcu_state.pos[AXIS_Y] = latlong_utm_y; - gps_utm_east = inter_mcu_state.pos[AXIS_X] * 100; - gps_utm_north = inter_mcu_state.pos[AXIS_Y] * 100; -#ifndef USE_VFILTER - inter_mcu_state.pos[AXIS_Z] = -XSENS_DATA_Position_alt(xsens_msg_buf,offset); - gps_alt = inter_mcu_state.pos[AXIS_Z] * 100; -#endif - offset += XSENS_DATA_Position_LENGTH; - } - if (XSENS_MASK_Velocity(xsens_output_mode)) { - //DOWNLINK_SEND_DEBUG(xsens_len,xsens_msg_buf); - inter_mcu_state.speed[AXIS_X] = XSENS_DATA_Velocity_vx(xsens_msg_buf,offset); - inter_mcu_state.speed[AXIS_Y] = XSENS_NED * XSENS_DATA_Velocity_vy(xsens_msg_buf,offset); -#ifndef USE_VFILTER - inter_mcu_state.speed[AXIS_Z] = XSENS_NED * XSENS_DATA_Velocity_vz(xsens_msg_buf,offset); - gps_climb = (int16_t)(-inter_mcu_state.speed[AXIS_Z] * 100); -#endif - gps_gspeed = (uint16_t)(sqrt(inter_mcu_state.speed[AXIS_X]*inter_mcu_state.speed[AXIS_X] + inter_mcu_state.speed[AXIS_Y]*inter_mcu_state.speed[AXIS_Y]) * 100); - offset += XSENS_DATA_Velocity_LENGTH; - } - if (XSENS_MASK_Status(xsens_output_mode)) { - xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf,offset); - if (bit_is_set(xsens_msg_status,2)) gps_mode = 0x03; // gps fix - else gps_mode = 0; - offset += XSENS_DATA_Status_LENGTH; - } - if (XSENS_MASK_TimeStamp(xsens_output_settings)) { - xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset); - gps_itow = xsens_time_stamp; - offset += XSENS_DATA_TimeStamp_LENGTH; - } - //} - } - //DOWNLINK_SEND_QUAD_INS(&offset,&xsens_len,&xsens_msg_status,&xsens_time_stamp); -} - - -void parse_ins_buffer( uint8_t c ) { - ck += c; - switch (xsens_status) { - case UNINIT: - if (c != XSENS_START) - goto error; - xsens_status++; - ck = 0; - break; - case GOT_START: - if (c != XSENS_BID) - goto error; - xsens_status++; - break; - case GOT_BID: - xsens_id = c; - xsens_status++; - break; - case GOT_MID: - xsens_len = c; - if (xsens_len > XSENS_MAX_PAYLOAD) - goto error; - xsens_msg_idx = 0; - xsens_status++; - break; - case GOT_LEN: - xsens_msg_buf[xsens_msg_idx] = c; - xsens_msg_idx++; - if (xsens_msg_idx >= xsens_len) - xsens_status++; - break; - case GOT_DATA: - if (ck != 0) - goto error; - ins_msg_received = TRUE; - goto restart; - break; - } - return; - error: - restart: - xsens_status = UNINIT; - return; -}