diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_pwm_supervision.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_pwm_supervision.makefile new file mode 100644 index 00000000000..149d6673a9f --- /dev/null +++ b/conf/autopilot/subsystems/rotorcraft/actuators_pwm_supervision.makefile @@ -0,0 +1,8 @@ + +# add actuatos arch to include directories +ap.CFLAGS += -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH) +ap.CFLAGS += -DSERVO_HZ=200 + +ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c +ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm_supervision.c +ap.srcs += $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c new file mode 100644 index 00000000000..8a3ab4ad960 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c @@ -0,0 +1,74 @@ +/* + * $Id$ + * + * Copyright (C) 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 "firmwares/rotorcraft/actuators.h" +#include "actuators_pwm_supervision.h" +#include "booz/booz2_commands.h" +#include "subsystems/radio_control.h" + +#include "downlink.h" +#include "messages.h" +#include "mcu_periph/uart.h" + +/* let's start butchery now and use the actuators_pwm arch functions */ +#include "firmwares/rotorcraft/actuators/actuators_pwm.h" + +/* get SetActuatorsFromCommands() macro */ +#include "generated/airframe.h" + +/* define the glue between control and SetActuatorsFromCommands */ +#define actuators actuators_pwm_values +#define SERVOS_TICS_OF_USEC(_v) (_v) + +#define Actuator(_x) actuators_pwm_values[_x] +#define ChopServo(x,a,b) Chop(x, a, b) +#define ActuatorsCommit actuators_pwm_commit + +int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; + +void actuators_init(void) +{ + supervision_init(); + actuators_pwm_arch_init(); +} + +#define PWM_GAIN_SCALE 5 +#define PWM_OFF 1000 + +void actuators_set(bool_t motors_on) { + booz2_commands[COMMAND_PITCH] = booz2_commands[COMMAND_PITCH] * PWM_GAIN_SCALE; + booz2_commands[COMMAND_ROLL] = booz2_commands[COMMAND_ROLL] * PWM_GAIN_SCALE; + booz2_commands[COMMAND_YAW] = booz2_commands[COMMAND_YAW] * PWM_GAIN_SCALE; + supervision_run(motors_on, FALSE, booz2_commands); + + if (motors_on) { + for (int i = 0; i < SUPERVISION_NB_MOTOR; i++) + actuators_pwm_values[i] = supervision.commands[i]; + } else { + for (int i = 0; i < ACTUATORS_PWM_NB; i++) + actuators_pwm_values[i] = PWM_OFF; + } + actuators_pwm_commit(); + +} + diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.h b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.h new file mode 100644 index 00000000000..cf10740cc2f --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.h @@ -0,0 +1,30 @@ +/* + * $Id: actuators_pwm_supervision$ + * + * Copyright (C) 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. + * + */ + +#ifndef ACTUATORS_PWM_SUPERVISION_H +#define ACTUATORS_PWM_SUPERVISION_H + +#include "firmwares/rotorcraft/actuators/supervision.h" + +#endif /* ACTUATORS_PWM_SUPERVISION_H */