diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile index a4d248a8be..d7aec02ce9 100644 --- a/conf/autopilot/rotorcraft.makefile +++ b/conf/autopilot/rotorcraft.makefile @@ -197,6 +197,7 @@ endif ap.srcs += $(SRC_FIRMWARE)/autopilot.c ap.srcs += $(SRC_FIRMWARE)/stabilization.c +ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_none.c ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c ap.CFLAGS += -DUSE_NAVIGATION diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile index 00b92d675e..52f040875e 100644 --- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile +++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile @@ -115,6 +115,8 @@ sim.srcs += $(SRC_FIRMWARE)/autopilot.c sim.srcs += $(SRC_FIRMWARE)/stabilization.c sim.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c +sim.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_none.c + NUM_TYPE=integer #NUM_TYPE=float diff --git a/sw/airborne/firmwares/rotorcraft/stabilization.h b/sw/airborne/firmwares/rotorcraft/stabilization.h index b3c2d2d268..fae6fa0b06 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization.h @@ -28,9 +28,11 @@ #include "generated/airframe.h" +#include "firmwares/rotorcraft/stabilization/stabilization_none.h" #include "firmwares/rotorcraft/stabilization/stabilization_rate.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" + extern void stabilization_init(void); extern int32_t stabilization_cmd[COMMANDS_NB]; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c new file mode 100644 index 0000000000..760c5a0a0e --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c @@ -0,0 +1,69 @@ +/* + * $Id$ + * + * Copyright (C) 2008-2009 Antoine Drouin + * Copyright (C) 2010 Felix Ruess + * + * 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 "firmwares/rotorcraft/stabilization.h" +#include "firmwares/rotorcraft/stabilization/stabilization_none.h" + +#include "subsystems/radio_control.h" +#include "generated/airframe.h" + +#define F_UPDATE_RES 9 +#define REF_DOT_FRAC 11 +#define REF_FRAC 16 + + +#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) +#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b)) + +struct Int32Rates stabilization_none_rc_cmd; + +void stabilization_none_init(void) { + + INT_RATES_ZERO(stabilization_none_rc_cmd); + +} + + +void stabilization_none_read_rc( void ) { + + + stabilization_none_rc_cmd.p = (int32_t)-radio_control.values[RADIO_ROLL]; + + stabilization_none_rc_cmd.q = (int32_t)radio_control.values[RADIO_PITCH]; + + stabilization_none_rc_cmd.r = (int32_t)-radio_control.values[RADIO_YAW]; +} + +void stabilization_none_enter(void) { + INT_RATES_ZERO(stabilization_none_rc_cmd); +} + +void stabilization_none_run(bool_t in_flight) { + + /* sum to final command */ + stabilization_cmd[COMMAND_ROLL] = stabilization_none_rc_cmd.p * SUPERVISION_SCALE / MAX_PPRZ; + stabilization_cmd[COMMAND_PITCH] = stabilization_none_rc_cmd.q * SUPERVISION_SCALE / MAX_PPRZ; + stabilization_cmd[COMMAND_YAW] = stabilization_none_rc_cmd.r * SUPERVISION_SCALE / MAX_PPRZ; + +} diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.h new file mode 100644 index 0000000000..515d4e844a --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.h @@ -0,0 +1,37 @@ +/* + * $Id$ + * + * 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 STABILIZATION_NONE +#define STABILIZATION_NONE + +#include "math/pprz_algebra_int.h" + +extern void stabilization_none_init(void); +extern void stabilization_none_read_rc(void); +extern void stabilization_none_run(bool_t in_flight); +extern void stabilization_none_enter(void); + +extern struct Int32Rates stabilization_none_rc_cmd; + + +#endif /* STABILIZATION_NONE */