From 4670d97b60c0eb0cd0a95056f73efb6c1b7185cf Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 3 Apr 2015 14:38:03 +0200 Subject: [PATCH] demo ahrs_actuators firmware --- conf/airframes/demo_cc3d.xml | 58 ++++++ conf/firmwares/demo.makefile | 108 ++++++++++ .../firmwares/demo/demo_ahrs_actuators.c | 195 ++++++++++++++++++ sw/airborne/test/test_actuators_pwm.c | 2 + 4 files changed, 363 insertions(+) create mode 100644 conf/airframes/demo_cc3d.xml create mode 100644 conf/firmwares/demo.makefile create mode 100644 sw/airborne/firmwares/demo/demo_ahrs_actuators.c diff --git a/conf/airframes/demo_cc3d.xml b/conf/airframes/demo_cc3d.xml new file mode 100644 index 0000000000..59ba6a10ba --- /dev/null +++ b/conf/airframes/demo_cc3d.xml @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + diff --git a/conf/firmwares/demo.makefile b/conf/firmwares/demo.makefile new file mode 100644 index 0000000000..cb5e311046 --- /dev/null +++ b/conf/firmwares/demo.makefile @@ -0,0 +1,108 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Copyright (C) 2013 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. +# + +################################################################################ +# +# +# Some demo programs +# +################################################################################ + + +SRC_ARCH=arch/$(ARCH) +SRC_BOARD=boards/$(BOARD) +SRC_SUBSYSTEMS=subsystems +SRC_MODULES=modules + +CFG_SHARED=$(PAPARAZZI_SRC)/conf/firmwares/subsystems/shared + + +# +# common test +# +# configuration +# SYS_TIME_LED +# MODEM_PORT +# MODEM_BAUD +# +PERIODIC_FREQUENCY ?= 512 + +COMMON_DEMO_CFLAGS = -I$(SRC_BOARD) -DBOARD_CONFIG=$(BOARD_CFG) +COMMON_DEMO_CFLAGS += -DPERIPHERALS_AUTO_INIT +COMMON_DEMO_SRCS = mcu.c $(SRC_ARCH)/mcu_arch.c +ifneq ($(SYS_TIME_LED),none) + COMMON_DEMO_CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) +endif +COMMON_DEMO_CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY) +COMMON_DEMO_SRCS += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c +ifeq ($(ARCH), linux) +# seems that we need to link agains librt for glibc < 2.17 +$(TARGET).LDFLAGS += -lrt +endif + +COMMON_DEMO_CFLAGS += -DUSE_LED + +ifeq ($(ARCH), lpc21) +COMMON_DEMO_SRCS += $(SRC_ARCH)/armVIC.c +else ifeq ($(ARCH), stm32) +COMMON_DEMO_SRCS += $(SRC_ARCH)/led_hw.c +COMMON_DEMO_SRCS += $(SRC_ARCH)/mcu_periph/gpio_arch.c +endif + +COMMON_DEMO_SRCS += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c +COMMON_DEMO_SRCS += mcu_periph/uart.c +COMMON_DEMO_SRCS += $(SRC_ARCH)/mcu_periph/uart_arch.c +ifeq ($(ARCH), linux) +COMMON_DEMO_SRCS += $(SRC_ARCH)/serial_port.c +endif + +# +# Demo AHRS and actuators +# +# required subsystems: +# - telemetry +# - imu +# - ahrs +# - actuators +# +# configuration +# SYS_TIME_LED +# MODEM_PORT +# MODEM_BAUD +# +demo_ahrs_actuators.ARCHDIR = $(ARCH) +demo_ahrs_actuators.CFLAGS += $(COMMON_DEMO_CFLAGS) +demo_ahrs_actuators.srcs += $(COMMON_DEMO_SRCS) +demo_ahrs_actuators.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c +demo_ahrs_actuators.srcs += subsystems/settings.c $(SRC_ARCH)/subsystems/settings_arch.c +demo_ahrs_actuators.srcs += subsystems/commands.c subsystems/actuators.c +demo_ahrs_actuators.srcs += state.c +demo_ahrs_actuators.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c math/pprz_algebra_int.c math/pprz_algebra_float.c math/pprz_algebra_double.c +demo_ahrs_actuators.srcs += firmwares/demo/demo_ahrs_actuators.c + +ifeq ($(TARGET), demo_ahrs_actuators) + ifeq ($(ACTUATORS),) + $(error ACTUATORS not configured, if your board file has no default, configure in your airframe file) + else + include $(CFG_SHARED)/$(ACTUATORS).makefile + endif +endif diff --git a/sw/airborne/firmwares/demo/demo_ahrs_actuators.c b/sw/airborne/firmwares/demo/demo_ahrs_actuators.c new file mode 100644 index 0000000000..2390ec493e --- /dev/null +++ b/sw/airborne/firmwares/demo/demo_ahrs_actuators.c @@ -0,0 +1,195 @@ +/* + * Copyright (C) 2015 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, see + * . + */ + +/** @file firmwares/demo/demo_ahrs_actuators.c + * Demo prog with ahrs and simple roll/pitch commands to actuators. + */ + +#include + +/* PERIODIC_C_MAIN is defined before generated/periodic_telemetry.h + * in order to implement telemetry_mode_Main_* + */ +#define PERIODIC_C_MAIN +#define ABI_C +#define DATALINK_C + +#include "subsystems/datalink/telemetry.h" +#include "subsystems/datalink/datalink.h" +#include "subsystems/datalink/downlink.h" +#include "subsystems/abi.h" + +#include "generated/airframe.h" +#include "generated/settings.h" + +#include "std.h" +#include "mcu.h" +#include "mcu_periph/sys_time.h" +#include "led.h" + +#include "state.h" +#include "subsystems/imu.h" +#include "subsystems/ahrs.h" +#include "subsystems/ahrs/ahrs_aligner.h" + +#include "subsystems/commands.h" +#include "subsystems/actuators.h" +#include "subsystems/settings.h" + +#include "pprz_version.h" + +#ifndef DEMO_MAX_ROLL +#define DEMO_MAX_ROLL RadOfDeg(65) +#endif + +#ifndef DEMO_MAX_PITCH +#define DEMO_MAX_PITCH RadOfDeg(65) +#endif + +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); + +static void send_alive(struct transport_tx *trans, struct link_device *dev); +static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev); +static void send_actuators(struct transport_tx *trans, struct link_device *dev); +static void send_commands(struct transport_tx *trans, struct link_device *dev); + +uint16_t datalink_time = 0; + +int main(void) +{ + main_init(); + while (1) { + if (sys_time_check_and_ack_timer(0)) { + main_periodic_task(); + } + main_event_task(); + } + return 0; +} + +static inline void main_init(void) +{ + mcu_init(); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); + + stateInit(); + actuators_init(); + + imu_init(); +#if USE_AHRS_ALIGNER + ahrs_aligner_init(); +#endif + ahrs_init(); + + settings_init(); + + mcu_int_enable(); + + downlink_init(); + + register_periodic_telemetry(DefaultPeriodic, "AUTOPILOT_VERSION", send_autopilot_version); + register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive); + register_periodic_telemetry(DefaultPeriodic, "COMMANDS", send_commands); + register_periodic_telemetry(DefaultPeriodic, "ACTUATORS", send_actuators); + + // send body_to_imu from here for now + AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); +} + +static inline void main_periodic_task(void) +{ + /* Simply set current roll/pitch as commands. + * Scale DEMO_MAX_ROLL/PITCH to MAX_PPRZ (the max commands) + */ + commands[COMMAND_ROLL] = stateGetNedToBodyEulers_f()->phi * MAX_PPRZ / DEMO_MAX_ROLL; + commands[COMMAND_PITCH] = stateGetNedToBodyEulers_f()->theta * MAX_PPRZ / DEMO_MAX_ROLL; + + /* generated macro from airframe file, seconds AP_MODE param not used */ + SetActuatorsFromCommands(commands, 0); + + if (sys_time.nb_sec > 1) { + imu_periodic(); + } + RunOnceEvery(10, { LED_PERIODIC();}); + RunOnceEvery(PERIODIC_FREQUENCY, { datalink_time++; }); + periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); +} + +static inline void main_event_task(void) +{ + mcu_event(); + ImuEvent(); + DatalinkEvent(); +} + + +void dl_parse_msg(void) +{ + datalink_time = 0; + uint8_t msg_id = dl_buffer[1]; + switch (msg_id) { + + case DL_PING: { + DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); + } + break; + case DL_SETTING: + if (DL_SETTING_ac_id(dl_buffer) == AC_ID) { + uint8_t i = DL_SETTING_index(dl_buffer); + float val = DL_SETTING_value(dl_buffer); + DlSetting(i, val); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); + } + break; + case DL_GET_SETTING : { + if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; } + uint8_t i = DL_GET_SETTING_index(dl_buffer); + float val = settings_get_value(i); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); + } + break; + default: + break; + } +} + +static void send_alive(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM); +} + +void send_autopilot_version(struct transport_tx *trans, struct link_device *dev) +{ + static uint32_t ap_version = PPRZ_VERSION_INT; + static char *ver_desc = PPRZ_VERSION_DESC; + pprz_msg_send_AUTOPILOT_VERSION(trans, dev, AC_ID, &ap_version, strlen(ver_desc), ver_desc); +} + +static void send_actuators(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators); +} + +static void send_commands(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, commands); +} diff --git a/sw/airborne/test/test_actuators_pwm.c b/sw/airborne/test/test_actuators_pwm.c index 87e174e998..8cdd579da6 100644 --- a/sw/airborne/test/test_actuators_pwm.c +++ b/sw/airborne/test/test_actuators_pwm.c @@ -94,7 +94,9 @@ void dl_parse_msg(void) case DL_SET_ACTUATOR: { uint8_t servo_no = DL_SET_ACTUATOR_no(dl_buffer); uint16_t servo_value = DL_SET_ACTUATOR_value(dl_buffer); +#ifdef LED_2 LED_TOGGLE(2); +#endif if (servo_no < ACTUATORS_PWM_NB) { ActuatorPwmSet(servo_no, servo_value); }