Skip to content

Commit

Permalink
[firmware] add rover firmware
Browse files Browse the repository at this point in the history 
Basic support for a rover firmware based on ChibiOS and generated AP:
- simple control loop
- new navigation layer based on registered functions
- xbee_api for telemetry, gps_passthrough for ins
- basic telemetry and settings files
  • Loading branch information
@gautierhattenberger
gautierhattenberger committed Jun 23, 2018
1 parent 0b13a74 commit 2114b8a
Show file tree
Hide file tree
Showing 30 changed files with 3,082 additions and 1 deletion.
118 changes: 118 additions & 0 deletions conf/airframes/ENAC/rover.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,118 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">

<airframe name="Rover Demo">

<firmware name="rover">
<autopilot name="rover.xml"/>

<configure name="AHRS_ALIGNER_LED" value="2"/>
<define name="IMU_MPU9250_READ_MAG" value="0"/>
<define name="LOW_NOISE_THRESHOLD" value="3500"/>
<define name="LOW_NOISE_TIME" value="10"/>

<target name="ap" board="chimera_1.0">
</target>

<module name="radio_control" type="datalink">
<!--define name="RADIO_KILL_SWITCH" value="RADIO_GAIN1"/-->
</module>

<module name="actuators" type="md25"/>

<module name="telemetry" type="xbee_api"/>
<module name="imu" type="chimera"/>
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<module name="gps" type="datalink"/>

<!--module name="stabilization" type="int_quat"/-->

<!-- Option 1) Ins(Accel + Gyro + Mag + Baro) + no GPS -->
<module name="ins" type="gps_passthrough"/>
<define name="INS_GP_USE_GPS_ACCEL" value="TRUE"/>
<module name="ahrs" type="int_cmpl_quat"/>

<module name="nav" type="rover_base"/>
<module name="guidance" type="rover"/>
</firmware>

<servos driver="MD25">
<servo name="MOTOR_RIGHT" no="0" min="0" neutral="128" max="255"/>
<servo name="MOTOR_LEFT" no="1" min="0" neutral="128" max="255"/>
</servos>

<commands>
<axis name="SPEED" failsafe_value="0"/>
<axis name="TURN" failsafe_value="0"/>
</commands>

<rc_commands>
<set command="SPEED" value="@PITCH"/>
<set command="TURN" value="@ROLL"/>
</rc_commands>

<section name="MIXER">
<define name="TURN_RATIO" value="0.5"/>
</section>

<command_laws>
<set servo="MOTOR_RIGHT" value="@SPEED - TURN_RATIO * @TURN"/>
<set servo="MOTOR_LEFT" value="@SPEED + TURN_RATIO * @TURN"/>
</command_laws>

<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>

<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module if loaded -->
<!-- Toulouse -->
<define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/>
<define name="USE_GPS_HEADING" value="TRUE"/>
<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
</section>

<section name="ROVER_GUIDANCE" prefix="ROVER_GUIDANCE_">
<define name="SPEED_PGAIN" value="0.1"/>
<define name="SPEED_DGAIN" value="0.1"/>
<define name="SPEED_IGAIN" value="0."/>
<define name="TURN_PGAIN" value="0.1"/>
<define name="TURN_DGAIN" value="0.1"/>
<define name="TURN_IGAIN" value="0."/>
</section>

<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="HOOPERFLY/hooperfly_teensyfly_quad" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
<define name="JS_AXIS_MODE" value="4"/>
</section>

<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_DIRECT"/> <!-- for compilation -->
<define name="MODE_AUTO1" value="AP_MODE_DIRECT"/> <!-- for compilation -->
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>

<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.8" unit="V"/>
<define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
<define name="MILLIAMP_AT_FULL_THROTTLE" value="20000" unit="mA"/>
<define name="MILLIAMP_AT_IDLE_THROTTLE" value="1000" value="mA"/>
<define name="CURRENT_ESTIMATION_NONLINEARITY" value="1.0"/>
</section>

<section name="GCS">
<define name="ALT_SHIFT_PLUS_PLUS" value="5"/>
<define name="ALT_SHIFT_PLUS" value="1"/>
<define name="ALT_SHIFT_MINUS" value="-1"/>
<define name="AC_ICON" value="quadrotor_x"/>
</section>

</airframe>
105 changes: 105 additions & 0 deletions conf/autopilot/rover.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,105 @@
<!DOCTYPE autopilot SYSTEM "autopilot.dtd">

<autopilot name="Quadrotor Autopilot (Basic version)">

<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">

<includes>
<include name="generated/airframe.h"/>
<include name="autopilot.h"/>
<include name="autopilot_rc_helpers.h"/>
<include name="subsystems/gps.h"/>
<include name="subsystems/actuators.h"/>
<include name="navigation.h"/>
<include name="guidance/rover_guidance.h"/>
<include name="subsystems/radio_control.h"/>
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
</includes>

<settings>
<dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
</settings>

<exceptions>
<exception cond="nav.too_far_from_home" deroute="HOME"/>
<exception cond="kill_switch_is_on()" deroute="KILL"/>
</exceptions>

<mode name="DIRECT" shortname="MANUAL">
<select cond="RCMode0() || RCMode1()"/>
<control freq="NAV_FREQ">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="SetCommandsFromRC(commands, radio_control.values)"/>
<call fun="SetActuatorsFromCommands(commands, autopilot_get_mode())"/>
</control>
<exception cond="RCLost()" deroute="KILL"/>
</mode>

<mode name="NAV">
<select cond="RCMode2()" exception="HOME"/>
<control freq="NAV_FREQ">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="rover_guidance_periodic()"/>
<call fun="SetActuatorsFromCommands(commands, autopilot_get_mode())"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="KILL"/>
</mode>

<mode name="HOME">
<control freq="NAV_FREQ">
<call fun="nav_home()"/>
</control>
<control>
<call fun="rover_guidance_periodic()"/>
<call fun="SetActuatorsFromCommands(commands, autopilot_get_mode())"/>
</control>
<exception cond="GpsIsLost()" deroute="KILL"/>
</mode>

<!-- Kill throttle mode -->
<mode name="KILL">
<select cond="$DEFAULT_MODE"/>
<select cond="kill_switch_is_on()"/>
<on_enter>
<call fun="autopilot_set_in_flight(false)"/>
<call fun="autopilot_set_motors_on(false)"/>
</on_enter>
<control>
<call fun="SetCommands(commands_failsafe)"/>
</control>
</mode>

</state_machine>

<state_machine name="guidance" freq="PERIODIC_FREQUENCY">

<includes>
<include name="navigation.h"/>
</includes>

<mode name="NAV_DIRECT">
<select cond="nav.mode == NAV_MODE_MANUAL"/>
<control>
<!-- copy manual nav controls to commands -->
<call fun="commands[COMMAND_SPEED] = TRIM_PPRZ(nav.speed * MAX_PPRZ)"/>
<call fun="commands[COMMAND_TURN] = TRIM_PPRZ(nav.turn * MAX_PPRZ)"/>
</control>
</mode>

<mode name="NAV_AUTO">
<select cond="nav.mode != NAV_MODE_MANUAL"/>
<control>
<call fun="VECT2_COPY(rover_guidance.sp.pos, nav.carrot)"/>
<call fun="rover_guidance_run(&nav.heading)"/>
<call fun="commands[COMMAND_SPEED] = TRIM_PPRZ(rover_guidance.cmd.motor_speed)"/>
<call fun="commands[COMMAND_TURN] = TRIM_PPRZ(rover_guidance.cmd.motor_turn)"/>
</control>
</mode>

</state_machine>

</autopilot>
167 changes: 167 additions & 0 deletions conf/firmwares/rover.makefile
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,167 @@
# Hey Emacs, this is a -*- makefile -*-
#
# Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
#
# 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.
#
#

CFG_SHARED=$(PAPARAZZI_SRC)/conf/firmwares/subsystems/shared

SRC_BOARD=boards/$(BOARD)
SRC_FIRMWARE=firmwares/rover
SRC_SUBSYSTEMS=subsystems
SRC_MODULES=modules

SRC_ARCH=arch/$(ARCH)

ROTORCRAFT_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOARD)

ap.ARCHDIR = $(ARCH)


VPATH += $(PAPARAZZI_HOME)/var/share

######################################################################
##
## COMMON ROVER ALL TARGETS (AP)
##

$(TARGET).CFLAGS += $(ROTORCRAFT_INC)
$(TARGET).CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
$(TARGET).CFLAGS += -DPERIPHERALS_AUTO_INIT
$(TARGET).srcs += mcu.c
$(TARGET).srcs += $(SRC_ARCH)/mcu_arch.c

# frequency of main periodic
PERIODIC_FREQUENCY ?= 100
$(TARGET).CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY)

ifdef AHRS_PROPAGATE_FREQUENCY
$(TARGET).CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
endif

ifdef AHRS_CORRECT_FREQUENCY
$(TARGET).CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
endif

ifdef AHRS_MAG_CORRECT_FREQUENCY
$(TARGET).CFLAGS += -DAHRS_MAG_CORRECT_FREQUENCY=$(AHRS_MAG_CORRECT_FREQUENCY)
endif


#
# Systime
#
$(TARGET).srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c
ifeq ($(ARCH), linux)
# seems that we need to link against librt for glibc < 2.17
$(TARGET).LDFLAGS += -lrt
endif


#
# Math functions
#
$(TARGET).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 math/pprz_stat.c

$(TARGET).srcs += subsystems/settings.c
$(TARGET).srcs += $(SRC_ARCH)/subsystems/settings_arch.c

$(TARGET).srcs += subsystems/actuators.c
$(TARGET).srcs += subsystems/commands.c

$(TARGET).srcs += state.c

#
# BARO_BOARD (if existing/configured)
#
include $(CFG_SHARED)/baro_board.makefile

#
# Main
#
# based on ChibiOS
#
ifeq ($(RTOS), chibios)
$(TARGET).srcs += $(SRC_FIRMWARE)/main_chibios.c
endif # RTOS
$(TARGET).srcs += $(SRC_FIRMWARE)/main_ap.c
$(TARGET).srcs += autopilot.c
$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_firmware.c
$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_utils.c
ifeq ($(USE_GENERATED_AUTOPILOT), TRUE)
$(TARGET).srcs += $(SRC_FIRMWARE)/autopilot_generated.c
$(TARGET).CFLAGS += -DUSE_GENERATED_AUTOPILOT=1
else
$(error "Rover firmware should use generated autopilot")
endif



######################################################################
##
## COMMON HARDWARE SUPPORT FOR ALL TARGETS
##

$(TARGET).srcs += mcu_periph/i2c.c
$(TARGET).srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c

include $(CFG_SHARED)/uart.makefile


#
# Electrical subsystem / Analog Backend
#
$(TARGET).CFLAGS += -DUSE_ADC
$(TARGET).srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
$(TARGET).srcs += subsystems/electrical.c


######################################################################
##
## HARDWARE SUPPORT FOR ALL NON-SIMULATION TARGETS (ap)
##
ifeq ($(ARCH), stm32)
ns_srcs += $(SRC_ARCH)/mcu_periph/gpio_arch.c
endif

ifeq ($(ARCH), chibios)
ns_srcs += $(SRC_ARCH)/mcu_periph/gpio_arch.c
endif

#
# LEDs
#
ns_CFLAGS += -DUSE_LED
ifneq ($(SYS_TIME_LED),none)
ns_CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
endif

ifeq ($(ARCH), stm32)
ns_srcs += $(SRC_ARCH)/led_hw.c
endif

######################################################################
##
## Final Target Allocations
##

ap.CFLAGS += $(ns_CFLAGS)
ap.srcs += $(ns_srcs)

Loading

0 comments on commit 2114b8a

Please sign in to comment.