diff --git a/conf/modules/vehicle_interface_datalink.xml b/conf/modules/vehicle_interface_datalink.xml new file mode 100644 index 0000000000..1a3ad68bad --- /dev/null +++ b/conf/modules/vehicle_interface_datalink.xml @@ -0,0 +1,16 @@ + + + +
+ +
+ + + + + + + + + + diff --git a/sw/airborne/modules/vehicle_interface/vi.c b/sw/airborne/modules/vehicle_interface/vi.c new file mode 100644 index 0000000000..0a13b0a0c2 --- /dev/null +++ b/sw/airborne/modules/vehicle_interface/vi.c @@ -0,0 +1,74 @@ +/* + * $Id: $ + * + * Copyright (C) 2008-2010 The Paparazzi Team + * + * 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 "vehicle_interface/vi.h" + +#include "booz/booz_imu.h" +#include "booz/booz_gps.h" +#include "booz/booz_ahrs.h" + +#include "airframe.h" + +struct VehicleInterface vi; + +#ifndef VI_TIMEOUT +#define VI_TIMEOUT 100 +#endif + +void vi_init(void) { + + vi.enabled = FALSE; + vi.timeouted = TRUE; + vi.last_msg = BOOZ_FMS_TIMEOUT; + + vi.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; + INT_EULERS_ZERO(vi.input.h_sp.attitude); + vi.input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB; + vi.input.v_sp.climb = 0; + vi_impl_init(); + +} + +void vi_periodic(void) { +#if (VI_TIMEOUT != 0) + if (vi.last_msg < VI_TIMEOUT) + vi.last_msg++; + else { + vi.timeouted = TRUE; + vi.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; + INT_EULERS_ZERO(vi.input.h_sp.attitude); + vi.input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB; + vi.input.v_sp.climb = 0; + } +#endif + vi_impl_periodic(); +} + +void vi_set_enabled(bool_t enabled) { + vi_impl_set_enabled(enabled); + +} + +void vi_update_info(void) { + +} diff --git a/sw/airborne/modules/vehicle_interface/vi.h b/sw/airborne/modules/vehicle_interface/vi.h new file mode 100644 index 0000000000..d7b166309f --- /dev/null +++ b/sw/airborne/modules/vehicle_interface/vi.h @@ -0,0 +1,106 @@ +/* + * $Id: $ + * + * Copyright (C) 2008-2010 The Paparazzi Team + * + * This file is part of paparazzi. + * + * This is the "external interface" to the autopilot. It allows an external device to + * fetch the vehicle state and input commands at different levels. We should support + * different hardware peripherals like i2c, spi or uart. + * For now we only have an implementation using datalink messages. + * + * This program 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 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef VEHICLE_INTERFACE_H +#define VEHICLE_INTERFACE_H + +#include "std.h" +#include "math/pprz_algebra_int.h" +#include "booz/booz2_autopilot.h" +#include "booz/booz_guidance.h" +#include "booz/booz2_navigation.h" + +struct Vi_imu_info { + struct Int16Vect3 gyro; + struct Int16Vect3 accel; + struct Int16Vect3 mag; +}; + +struct Vi_gps_info { + struct Int32Vect3 pos; + struct Int16Vect3 speed; + int32_t pacc; + uint8_t num_sv; + uint8_t fix; +}; + +struct Vi_ahrs_info { + struct Int16Eulers euler; + struct Int16Eulers rate; +}; + +struct Vi_info { + struct Vi_imu_info imu; + struct Vi_gps_info gps; + struct Vi_ahrs_info ahrs; +}; + +struct Vi_command { + union { + struct Int32Vect3 rate; + struct Int32Eulers attitude; + struct Int32Vect3 speed; //FIXME Warning z is heading rate + struct Int32Vect3 pos; //FIXME Warning z is heading + } h_sp; + union { + int32_t direct; + int32_t climb; + int32_t height; + } v_sp; + uint8_t h_mode; + uint8_t v_mode; +}; + +struct VehicleInterface { + bool_t enabled; + bool_t timeouted; + uint8_t last_msg; + struct Vi_info info; + struct Vi_command input; +}; + +extern struct VehicleInterface vi; + +extern void vi_init(void); +extern void vi_set_enabled(bool_t enabled); +extern void vi_periodic(void); +extern void vi_update_info(void); + +/* must be implemented by specific module */ +extern void vi_impl_init(void); +extern void vi_impl_periodic(void); +extern void vi_impl_set_enabled(bool_t enabled); + + +#define vi_SetEnabled(_val) { \ + vi.enabled = _val; \ + vi_set_enabled(_val); \ + } + +#endif /* VI_H */ + + diff --git a/sw/airborne/modules/vehicle_interface/vi_datalink.c b/sw/airborne/modules/vehicle_interface/vi_datalink.c new file mode 100644 index 0000000000..7ad7c5339f --- /dev/null +++ b/sw/airborne/modules/vehicle_interface/vi_datalink.c @@ -0,0 +1,49 @@ +/* + * $Id: $ + * + * Copyright (C) 2008-2010 The Paparazzi Team + * + * 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 "vehicle_interface/vi_datalink.h" + +void vi_impl_init(void) { +} + +void vi_impl_periodic(void) { +} + +void vi_impl_set_enabled(bool_t enabled __attribute__ ((unused))) { +} + +#define ViMaxHSpeed ((int16_t)SPEED_BFP_OF_REAL(VI_MAX_H_SPEED)) +#define ViMaxVSpeed ((int16_t)SPEED_BFP_OF_REAL(VI_MAX_V_SPEED)) +#define ViMaxHeadingRate ((int16_t)RATE_BFP_OF_REAL(RadOfDeg(VI_MAX_HEADING_RATE))) + +struct Int16Vect3 wp_speed_max = { ViMaxHSpeed, ViMaxHSpeed, ViMaxVSpeed }; + +void vi_update_wp(uint8_t wp_id) { + struct Int16Vect3 wp_speed; + wp_speed.x = ViMaxHSpeed * vi.input.h_sp.speed.x / 128; + wp_speed.y = ViMaxHSpeed * vi.input.h_sp.speed.y / 128; + wp_speed.z = ViMaxVSpeed * vi.input.v_sp.climb / 128; + VECT3_BOUND_BOX(wp_speed, wp_speed_max); + int16_t heading_rate = BoundAbs(vi.input.h_sp.speed.z,ViMaxHeadingRate); + navigation_update_update_wp_from_speed(wp_id , wp_speed, heading_rate); +} diff --git a/sw/airborne/modules/vehicle_interface/vi_datalink.h b/sw/airborne/modules/vehicle_interface/vi_datalink.h new file mode 100644 index 0000000000..912322b906 --- /dev/null +++ b/sw/airborne/modules/vehicle_interface/vi_datalink.h @@ -0,0 +1,112 @@ +/* + * $Id: booz_fms_datalink.h 5216 2010-08-04 17:13:43Z gautier $ + * + * This is the implementation of the "external interface" to the autopilot. + * using datalink messages. + * + * This program 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 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef VEHICLE_INTERFACE_DATALINK_H +#define VEHICLE_INTERFACE_DATALINK_H + +#include "std.h" +#include "vehicle_interface/vi.h" +#include "math/pprz_algebra_int.h" + +#ifndef VI_MAX_H_SPEED +#define VI_MAX_H_SPEED 4. +#endif + +#ifndef VI_MAX_V_SPEED +#define VI_MAX_V_SPEED 2. +#endif + +#ifndef VI_MAX_HEADING_RATE +#define VI_MAX_HEADING_RATE 60. +#endif + +extern void vi_update_wp(uint8_t wp_id); + +#ifdef VI_PHI_THETA_MAX +#define VI_LIMIT_ATTITUDE(_att) { \ + BoundAbs(_att.phi, VI_PHI_THETA_MAX); \ + BoundAbs(_att.theta, VI_PHI_THETA_MAX); \ +} +#else +#define VI_LIMIT_ATTITUDE(_x) {} +#endif + +#define VI_PARSE_DATALINK(_dl_buffer) { \ + vi.last_msg = 0; \ + vi.input.h_mode = DL_BOOZ2_FMS_COMMAND_h_mode(_dl_buffer); \ + vi.input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer); \ + switch (vi.input.h_mode) { \ + case BOOZ2_GUIDANCE_H_MODE_KILL: \ + case BOOZ2_GUIDANCE_H_MODE_RATE : \ + break; \ + case BOOZ2_GUIDANCE_H_MODE_ATTITUDE : \ + { \ + vi.input.h_sp.attitude.phi = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ + vi.input.h_sp.attitude.theta = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ + vi.input.h_sp.attitude.psi = DL_BOOZ2_FMS_COMMAND_h_sp_3(_dl_buffer); \ + ANGLE_REF_NORMALIZE(vi.input.h_sp.attitude.psi); \ + BOOZ_FMS_LIMIT_ATTITUDE(vi.input.h_sp.attitude); \ + } \ + break; \ + case BOOZ2_GUIDANCE_H_MODE_HOVER : \ + { \ + vi.input.h_sp.pos.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ + vi.input.h_sp.pos.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ + } \ + break; \ + case BOOZ2_GUIDANCE_H_MODE_NAV : \ + { \ + vi.input.h_sp.speed.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ + vi.input.h_sp.speed.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ + vi.input.h_sp.speed.z = DL_BOOZ2_FMS_COMMAND_h_sp_3(_dl_buffer); \ + } \ + break; \ + } \ + switch (vi.input.v_mode) { \ + case BOOZ2_GUIDANCE_V_MODE_KILL: \ + case BOOZ2_GUIDANCE_V_MODE_RC_DIRECT: \ + case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB: \ + break; \ + case BOOZ2_GUIDANCE_V_MODE_CLIMB : \ + vi.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ + break; \ + case BOOZ2_GUIDANCE_V_MODE_HOVER : \ + vi.input.v_sp.height = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ + break; \ + case BOOZ2_GUIDANCE_V_MODE_NAV : \ + vi.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ + break; \ + } \ + } + +#define BOOZ_FMS_NAV_STICK_PARSE_DL(_dl_buffer) { \ + vi.last_msg = 0; \ + vi.input.h_mode = BOOZ2_GUIDANCE_H_MODE_NAV; \ + vi.input.v_mode = BOOZ2_GUIDANCE_V_MODE_NAV; \ + vi.input.h_sp.speed.x = DL_BOOZ_NAV_STICK_vx_sp(_dl_buffer); \ + vi.input.h_sp.speed.y = DL_BOOZ_NAV_STICK_vy_sp(_dl_buffer); \ + vi.input.h_sp.speed.z = DL_BOOZ_NAV_STICK_r_sp(_dl_buffer); \ + vi.input.v_sp.climb = DL_BOOZ_NAV_STICK_vz_sp(_dl_buffer); \ +} + +#define NavUpdateWPFromVI(_wp) vi_update_wp(uint8_t _wp); + +#endif /* VI_DATALINK_H */ diff --git a/sw/airborne/modules/vehicle_interface/vi_test_signal.c b/sw/airborne/modules/vehicle_interface/vi_test_signal.c new file mode 100644 index 0000000000..0a2922af6d --- /dev/null +++ b/sw/airborne/modules/vehicle_interface/vi_test_signal.c @@ -0,0 +1,91 @@ +/* + * $Id: booz_fms_test_signal.c 4675 2010-03-13 22:59:46Z poine $ + * + * Copyright (C) 2008-2009 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_fms.h" + +#include "booz2_ins.h" +#include "math/pprz_algebra_int.h" + +#define FMS_TEST_SIGNAL_DEFAULT_MODE STEP_YAW +#define FMS_TEST_SIGNAL_DEFAULT_PERIOD 40 +#define FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE (((int32_t)ANGLE_BFP_OF_REAL(RadOfDeg(5)))<<8) + +struct BoozFmsTestSignal fms_test_signal; + +void booz_fms_impl_init(void) { + fms_test_signal.mode = FMS_TEST_SIGNAL_DEFAULT_MODE; + fms_test_signal.period = FMS_TEST_SIGNAL_DEFAULT_PERIOD; + fms_test_signal.amplitude = FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE; + fms_test_signal.counter = 0; + fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; + fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER; +} + +void booz_fms_impl_periodic(void) { + + switch (fms_test_signal.mode) { + + case STEP_ROLL: { + if (fms_test_signal.counter < fms_test_signal.period) { + EULERS_ASSIGN(fms.input.h_sp.attitude, fms_test_signal.amplitude, 0, 0); + } + else { + EULERS_ASSIGN(fms.input.h_sp.attitude, -fms_test_signal.amplitude, 0, 0); + } + } + break; + case STEP_YAW: { + if (fms_test_signal.counter < fms_test_signal.period) { + EULERS_ASSIGN(fms.input.h_sp.attitude, 0, 0, fms_test_signal.amplitude); + } + else { + EULERS_ASSIGN(fms.input.h_sp.attitude, 0, 0, -fms_test_signal.amplitude); + } + } + break; + case STEP_PITCH: + case STEP_VERT: + break; +#if 0 + case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: { + if (booz2_guidance_v_mode < BOOZ2_GUIDANCE_V_MODE_HOVER) + booz_fms_test_signal_start_z = booz_ins_ltp_pos.z; + else { + booz_fms_input.v_sp.height = (booz_fms_test_signal_counter < booz_fms_test_signal_period) ? + booz_fms_test_signal_start_z : + booz_fms_test_signal_start_z - 256; + //BOOZ_INT_OF_FLOAT(-0.5, IPOS_FRAC) + } + } + break; +#endif + } + fms_test_signal.counter++; + if (fms_test_signal.counter >= (2 * fms_test_signal.period)) + fms_test_signal.counter = 0; +} + +void booz_fms_impl_set_enabled(bool_t enabled) { + if (enabled) + fms_test_signal.counter = 0; +} diff --git a/sw/airborne/modules/vehicle_interface/vi_test_signal.h b/sw/airborne/modules/vehicle_interface/vi_test_signal.h new file mode 100644 index 0000000000..eaf2fedef1 --- /dev/null +++ b/sw/airborne/modules/vehicle_interface/vi_test_signal.h @@ -0,0 +1,57 @@ +/* + * $Id: booz_fms_test_signal.h 3976 2009-08-21 01:30:22Z poine $ + * + * Copyright (C) 2008-2009 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. + */ + +#ifndef BOOZ_FMS_TEST_SIGNAL_H +#define BOOZ_FMS_TEST_SIGNAL_H + +#include "std.h" + + +enum fms_ts_mode { + STEP_ROLL, + STEP_PITCH, + STEP_YAW, + STEP_VERT +}; + +struct BoozFmsTestSignal { + enum fms_ts_mode mode; + uint32_t period; + uint32_t amplitude; + uint32_t counter; +}; + +extern struct BoozFmsTestSignal fms_test_signal; + +#define booz_fms_test_signal_SetPeriod(_val) { \ + fms_test_signal.period = _val; \ + fms_test_signal.counter = 0; \ + } + +#define booz_fms_test_signal_SetMode(_val) { \ + fms_test_signal.mode = (enum fms_ts_mode)(_val); \ + fms_test_signal.counter = 0; \ + } + +#endif /* BOOZ_FMS_TEST_SIGNAL_H */ +