demo ahrs_actuators firmware

conf/airframes/demo_cc3d.xml

@@ -0,0 +1,58 @@
+<!DOCTYPE airframe SYSTEM "airframe.dtd">
+
+<!-- this is a demo file for the OpenPilot CC3D board:
+     * Autopilot:   OpenPilot CC3D         https://www.openpilot.org/product/coptercontrol/
+     * Actuators:   two standard servos    http://wiki.paparazziuav.org/wiki/Subsystem/actuators#PWM
+-->
+
+<airframe name="DemoCC3D">
+
+  <firmware name="demo">
+    <target name="demo_ahrs_actuators" board="cc3d">
+      <subsystem name="telemetry" type="transparent"/>
+      <subsystem name="actuators" type="pwm"/>
+      <subsystem name="imu" type="mpu6000"/>
+      <subsystem name="ahrs" type="int_cmpl_quat"/>
+      <configure name="USE_MAGNETOMETER" value="FALSE"/>
+    </target>
+    <!--define name="USE_PERSISTENT_SETTINGS" value="TRUE"/-->
+    <configure name="AHRS_ALIGNER_LED" value="1"/>
+  </firmware>
+
+  <firmware name="test_progs">
+    <target name="test_sys_time_timer" board="cc3d"/>
+    <target name="test_sys_time_usleep" board="cc3d"/>
+    <target name="test_telemetry" board="cc3d"/>
+    <target name="test_imu" board="cc3d">
+      <subsystem name="imu" type="mpu6000"/>
+    </target>
+    <target name="test_ahrs" board="cc3d">
+      <subsystem name="imu" type="mpu6000"/>
+      <subsystem name="ahrs" type="int_cmpl_quat"/>
+      <configure name="USE_MAGNETOMETER" value="FALSE"/>
+    </target>
+    <target name="test_actuators_pwm" board="cc3d"/>
+  </firmware>
+
+  <servos driver="Pwm">
+    <servo name="ROLL"   no="0" min="1000" neutral="1500" max="2000"/>
+    <servo name="PITCH"  no="1" min="1000" neutral="1500" max="2000"/>
+  </servos>
+
+  <commands>
+    <axis name="ROLL"   failsafe_value="0"/>
+    <axis name="PITCH"  failsafe_value="0"/>
+  </commands>
+
+  <command_laws>
+    <set servo="ROLL"  value="-@ROLL"/>
+    <set servo="PITCH" value="-@PITCH"/>
+  </command_laws>
+
+  <section name="IMU" prefix="IMU_">
+    <define name="BODY_TO_IMU_PHI"   value="180." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="-90." unit="deg"/>
+  </section>
+
+</airframe>

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

sw/airborne/firmwares/demo/demo_ahrs_actuators.c

@@ -0,0 +1,195 @@
+/*
+ * Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/** @file firmwares/demo/demo_ahrs_actuators.c
+ *  Demo prog with ahrs and simple roll/pitch commands to actuators.
+ */
+
+#include <inttypes.h>
+
+/* 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);
+}

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);
       }