Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 00:37:37 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[firmware] add rover firmware

Browse Source 
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
This commit is contained in:
Gautier Hattenberger
2018-06-23 21:51:48 +02:00
parent 0b13a742b7
commit 2114b8ae4e
30 changed files with 3082 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/rover.xml
+118
View File
@@ -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>
conf/autopilot/rover.xml
+105
View File
@@ -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>
conf/firmwares/rover.makefile
+167
View File
@@ -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)
conf/modules/guidance_rover.xml
+28
View File
@@ -0,0 +1,28 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="guidance_rover" dir="guidance">
<doc>
<description>
Base guidance code for rover
</description>
</doc>
<settings target="ap">
<dl_settings>
<dl_settings NAME="Guidance">
<dl_setting var="rover_guidance.speed_pid.p" min="0." step="0.1" max="10." shortname="s_kp" param="ROVER_GUIDANCE_SPEED_PGAIN" type="float" persistent="true"/>
<dl_setting var="rover_guidance.speed_pid.d" min="0." step="0.1" max="10." shortname="s_kd" param="ROVER_GUIDANCE_SPEED_DGAIN" type="float" persistent="true"/>
<dl_setting var="rover_guidance.speed_pid.i" min="0." step="0.01" max="1." shortname="s_ki" handler="set_speed_igain" param="ROVER_GUIDANCE_SPEED_IGAIN" type="float" persistent="true" module="guidance/rover_guidance"/>
<dl_setting var="rover_guidance.turn_pid.p" min="0." step="0.1" max="10." shortname="t_kp" param="ROVER_GUIDANCE_TURN_PGAIN" type="float" persistent="true"/>
<dl_setting var="rover_guidance.turn_pid.d" min="0." step="0.1" max="10." shortname="t_kd" param="ROVER_GUIDANCE_TURN_DGAIN" type="float" persistent="true"/>
<dl_setting var="rover_guidance.turn_pid.i" min="0." step="0.01" max="1." shortname="t_ki" handler="set_turn_igain" param="ROVER_GUIDANCE_TURN_IGAIN" type="float" persistent="true" module="guidance/rover_guidance"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="rover_guidance.h"/>
</header>
<init fun="rover_guidance_init()"/>
<makefile target="ap" firmware="rover">
<file name="rover_guidance.c" dir="$(SRC_FIRMWARE)/guidance"/>
</makefile>
</module>
conf/modules/ins_gps_passthrough.xml
+5
View File
@@ -21,4 +21,9 @@
<file name="ins.c" dir="subsystems"/>
<file name="ins_gps_passthrough_utm.c" dir="subsystems/ins"/>
</makefile>
<makefile target="ap" firmware="rover">
<define name="INS_TYPE_H" value="subsystems/ins/ins_gps_passthrough.h" type="string"/>
<file name="ins.c" dir="subsystems"/>
<file name="ins_gps_passthrough.c" dir="subsystems/ins"/>
</makefile>
</module>
conf/modules/nav_rover_base.xml
+31
View File
@@ -0,0 +1,31 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="nav_rover_base" dir="nav">
<doc>
<description>
Basic navigation functions for Rovers
</description>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="nav">
<dl_setting MAX="100." MIN="-100." STEP="0.1" VAR="nav.radius"/>
<dl_setting MAX="359." MIN="0." STEP="5." VAR="nav.heading" unit="rad" alt_unit="deg"/>
<dl_setting MAX="20." MIN="-20." STEP="1." VAR="nav.shift" module="firmwares/rover/navigation" handler="IncreaseShift" shortname="inc. shift"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="nav_rover_base.h"/>
</header>
<init fun="nav_init()"/>
<init fun="nav_rover_init()"/>
<makefile>
<file name="nav_rover_base.c"/>
<file name="navigation.c" dir="firmwares/rover"/>
<file name="common_flight_plan.c" dir="subsystems/navigation"/>
<file name="waypoints.c" dir="subsystems/navigation"/>
<define name="USE_NAVIGATION"/>
</makefile>
</module>
conf/modules/telemetry_xbee_api.xml
+4
View File
@@ -43,5 +43,9 @@
<file name="rotorcraft_datalink.c" dir="$(SRC_FIRMWARE)"/>
<file name="rotorcraft_telemetry.c" dir="$(SRC_FIRMWARE)"/>
</makefile>
<makefile target="ap" firmware="rover">
<file name="rover_datalink.c" dir="$(SRC_FIRMWARE)"/>
<file name="rover_telemetry.c" dir="$(SRC_FIRMWARE)"/>
</makefile>
</module>
conf/settings/rover_basic.xml
+16
View File
@@ -0,0 +1,16 @@
<!DOCTYPE settings SYSTEM "settings.dtd">
<settings target="ap">
<dl_settings>
<dl_settings NAME="System">
<dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
<dl_setting var="autopilot.power_switch" min="0" step="1" max="1" module="autopilot" values="OFF|ON" handler="SetPowerSwitch">
<strip_button name="POWER ON" icon="on.png" value="1" group="power_switch"/>
<strip_button name="POWER OFF" icon="off.png" value="0" group="power_switch"/>
</dl_setting>
<dl_setting var="autopilot.mode" min="0" step="1" max="19" module="autopilot" shortname="mode" handler="SetModeHandler"/>
</dl_settings>
</dl_settings>
</settings>
conf/telemetry/default_rover.xml
+56
View File
@@ -0,0 +1,56 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
<telemetry>
<process name="Main">
<mode name="default" key_press="d">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="ROVER_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period=".25"/>
<message name="INS" period=".25"/>
<message name="INS_REF" period="5.1"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="1.3"/>
<message name="SURVEY" period="2.5"/>
<message name="OPTIC_FLOW_EST" period="0.05"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
</mode>
<mode name="rc">
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
<message name="ROVER_STATUS" period="1.2"/>
</mode>
<mode name="raw_sensors">
<message name="ROVER_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
</mode>
<mode name="aligner">
<message name="ALIVE" period="0.9"/>
<message name="FILTER_ALIGNER" period="0.02"/>
</mode>
</process>
</telemetry>
sw/airborne/boards/chimera/chibios/v1.0/chimera.h
+31
View File
@@ -477,20 +477,51 @@
#define I2C_FAST_400KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR (STM32_TIMINGR_PRESC(0U) | \
STM32_TIMINGR_SCLDEL(10U) | STM32_TIMINGR_SDADEL(0U) | \
STM32_TIMINGR_SCLH(34U) | STM32_TIMINGR_SCLL(86U))
#define I2C_STD_100KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR (STM32_TIMINGR_PRESC(1U) | \
STM32_TIMINGR_SCLDEL(9U) | STM32_TIMINGR_SDADEL(0U) | \
STM32_TIMINGR_SCLH(105U) | STM32_TIMINGR_SCLL(153U))
#ifndef I2C1_CLOCK_SPEED
#define I2C1_CLOCK_SPEED 400000
#endif
#if I2C1_CLOCK_SPEED == 400000
#define I2C1_CFG_DEF { \
.timingr = I2C_FAST_400KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR, \
.cr1 = STM32_CR1_DNF(0), \
.cr2 = 0 \
}
#elif I2C1_CLOCK_SPEED == 100000
#define I2C1_CFG_DEF { \
.timingr = I2C_STD_100KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR, \
.cr1 = STM32_CR1_DNF(0), \
.cr2 = 0 \
}
#else
#error "Unknown I2C1 clock speed"
#endif
#ifndef I2C2_CLOCK_SPEED
#define I2C2_CLOCK_SPEED 400000
#endif
#if I2C2_CLOCK_SPEED == 400000
#define I2C2_CFG_DEF { \
.timingr = I2C_FAST_400KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR, \
.cr1 = STM32_CR1_DNF(0), \
.cr2 = 0 \
}
#elif I2C2_CLOCK_SPEED == 100000
#define I2C2_CFG_DEF { \
.timingr = I2C_STD_100KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR, \
.cr1 = STM32_CR1_DNF(0), \
.cr2 = 0 \
}
#else
#error "Unknown I2C2 clock speed"
#endif
/**
* SPI Config
sw/airborne/firmwares/rover/autopilot_firmware.c
+144
View File
@@ -0,0 +1,144 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/rover/autopilot.c
*
* Rover specific autopilot interface
* and initialization
*/
#include "firmwares/rover/autopilot_firmware.h"
#include "generated/modules.h"
#include <stdint.h>
#include "subsystems/electrical.h"
#include "subsystems/datalink/telemetry.h"
#include "subsystems/radio_control.h"
#if USE_GPS
#include "subsystems/gps.h"
#else
#if NO_GPS_NEEDED_FOR_NAV
#define GpsIsLost() FALSE
#else
#define GpsIsLost() TRUE
#endif
#endif
/* Geofence exceptions */
#include "modules/nav/nav_geofence.h"
#if USE_MOTOR_MIXING
#include "subsystems/actuators/motor_mixing.h"
#endif
static void send_status(struct transport_tx *trans, struct link_device *dev)
{
#if USE_GPS
uint8_t fix = gps.fix;
#else
uint8_t fix = 0;
#endif
uint8_t motors_on = autopilot.motors_on;
uint16_t time_sec = sys_time.nb_sec;
pprz_msg_send_ROVER_STATUS(trans, dev, AC_ID,
&radio_control.status, &radio_control.frame_rate,
&fix, &autopilot.mode, &motors_on,
&electrical.vsupply, &time_sec);
}
static void send_energy(struct transport_tx *trans, struct link_device *dev)
{
uint16_t e = electrical.energy;
if (fabs(electrical.energy) >= INT16_MAX) {
e = INT16_MAX;
}
float vsup = ((float)electrical.vsupply) / 10.0f;
float curs = ((float)electrical.current) / 1000.0f;
float power = vsup * curs;
pprz_msg_send_ENERGY(trans, dev, AC_ID, &vsup, &curs, &e, &power);
}
static void send_fp(struct transport_tx *trans, struct link_device *dev)
{
int32_t carrot_east = POS_BFP_OF_REAL(nav.carrot.x);
int32_t carrot_north = POS_BFP_OF_REAL(nav.carrot.y);
int32_t carrot_up = 0;
int32_t carrot_heading = ANGLE_BFP_OF_REAL(nav.heading);
int32_t cmd = (int32_t)(ABS(commands[COMMAND_SPEED]));
pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
&(stateGetPositionEnu_i()->x),
&(stateGetPositionEnu_i()->y),
&(stateGetPositionEnu_i()->z),
&(stateGetSpeedEnu_i()->x),
&(stateGetSpeedEnu_i()->y),
&(stateGetSpeedEnu_i()->z),
&(stateGetNedToBodyEulers_i()->phi),
&(stateGetNedToBodyEulers_i()->theta),
&(stateGetNedToBodyEulers_i()->psi),
&carrot_east,
&carrot_north,
&carrot_up,
&carrot_heading,
&cmd,
&autopilot.flight_time);
}
#ifdef RADIO_CONTROL
static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev)
{
#ifdef RADIO_KILL_SWITCH
int16_t _kill_switch = radio_control.values[RADIO_KILL_SWITCH];
#else
int16_t _kill_switch = 42;
#endif
pprz_msg_send_ROTORCRAFT_RADIO_CONTROL(trans, dev, AC_ID,
&radio_control.values[RADIO_ROLL],
&radio_control.values[RADIO_PITCH],
&radio_control.values[RADIO_YAW],
&radio_control.values[RADIO_THROTTLE],
&radio_control.values[RADIO_MODE],
&_kill_switch,
&radio_control.status);
}
#endif
void autopilot_firmware_init(void)
{
// register messages
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROVER_STATUS, send_status);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ENERGY, send_energy);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp);
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc);
#endif
}
/** autopilot event function
*
*/
void autopilot_event(void)
{
}
sw/airborne/firmwares/rover/autopilot_firmware.h
+36
View File
@@ -0,0 +1,36 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/rover/autopilot_firmware.h
*
* Rover specific autopilot interface
* and initialization
*/
#ifndef AUTOPILOT_FIRMWARE_H
#define AUTOPILOT_FIRMWARE_H
#include "std.h"
#include "autopilot.h"
extern void autopilot_firmware_init(void);
#endif /* AUTOPILOT_FIRMWARE_H */
sw/airborne/firmwares/rover/autopilot_generated.c
+101
View File
@@ -0,0 +1,101 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/rover/autopilot_generated.c
*
* Generated autopilot implementation.
*
*/
#define AUTOPILOT_CORE_AP_C
#include "firmwares/rotorcraft/autopilot_generated.h"
#include "autopilot.h"
#include "subsystems/radio_control.h"
#include "subsystems/commands.h"
#include "subsystems/actuators.h"
#include "subsystems/settings.h"
#include "subsystems/datalink/telemetry.h"
#include "generated/settings.h"
void autopilot_generated_init(void)
{
// call generated init
autopilot_core_ap_init();
// copy generated mode to public mode (set at startup)
autopilot.mode = autopilot_mode_ap;
}
void autopilot_generated_periodic(void)
{
autopilot_core_ap_periodic_task();
// copy generated mode to public mode (may be changed on internal exceptions)
autopilot.mode = autopilot_mode_ap;
/* Reset ground detection _after_ running flight plan
*/
if (!autopilot_in_flight()) {
autopilot.ground_detected = false;
autopilot.detect_ground_once = false;
}
}
/** AP mode setting handler
*
* FIXME generated something else for this?
*/
void autopilot_generated_SetModeHandler(float mode)
{
autopilot_generated_set_mode(mode);
}
void autopilot_generated_set_mode(uint8_t new_autopilot_mode)
{
autopilot_core_ap_set_mode(new_autopilot_mode);
// copy generated mode to public mode
autopilot.mode = autopilot_mode_ap;
}
void autopilot_generated_set_motors_on(bool motors_on)
{
if (ap_ahrs_is_aligned() && motors_on
#ifdef AP_MODE_KILL
&& autopilot.mode != AP_MODE_KILL
#endif
) {
autopilot.motors_on = true;
} else {
autopilot.motors_on = false;
}
}
void autopilot_generated_on_rc_frame(void)
{
}
sw/airborne/firmwares/rover/autopilot_generated.h
+45
View File
@@ -0,0 +1,45 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/rover/autopilot_generated.h
*
* Autopilot generated implementation
* Calls the code generated from autopilot XML file
*
*/
#ifndef AUTOPILOT_GENERATED_H
#define AUTOPILOT_GENERATED_H
#include "std.h"
#include "generated/autopilot_core_ap.h"
#include "generated/airframe.h"
#include "state.h"
extern void autopilot_generated_init(void);
extern void autopilot_generated_periodic(void);
extern void autopilot_generated_on_rc_frame(void);
extern void autopilot_generated_set_mode(uint8_t new_autopilot_mode);
extern void autopilot_generated_SetModeHandler(float new_autopilot_mode); // handler for dl_setting
extern void autopilot_generated_set_motors_on(bool motors_on);
#endif /* AUTOPILOT_GENERATED_H */
sw/airborne/firmwares/rover/autopilot_rc_helpers.h
+115
View File
@@ -0,0 +1,115 @@
/*
* Copyright (C) 2012 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.
*/
/**
* @file firmwares/rotorcraft/autopilot_rc_helpers.h
*
* Some helper functions to check RC sticks.
*/
#ifndef AUTOPILOT_RC_HELPERS_H
#define AUTOPILOT_RC_HELPERS_H
#include "generated/airframe.h"
#include "subsystems/radio_control.h"
#define AUTOPILOT_THROTTLE_THRESHOLD (MAX_PPRZ / 20)
#define AUTOPILOT_YAW_THRESHOLD (MAX_PPRZ * 19 / 20)
#ifndef AUTOPILOT_STICK_CENTER_THRESHOLD
#define AUTOPILOT_STICK_CENTER_THRESHOLD (MAX_PPRZ * 1 / 20)
#endif
#define THROTTLE_STICK_DOWN() \
(radio_control.values[RADIO_THROTTLE] < AUTOPILOT_THROTTLE_THRESHOLD)
#define YAW_STICK_PUSHED() \
(radio_control.values[RADIO_YAW] > AUTOPILOT_YAW_THRESHOLD || \
radio_control.values[RADIO_YAW] < -AUTOPILOT_YAW_THRESHOLD)
#define YAW_STICK_CENTERED() \
(radio_control.values[RADIO_YAW] < AUTOPILOT_STICK_CENTER_THRESHOLD && \
radio_control.values[RADIO_YAW] > -AUTOPILOT_STICK_CENTER_THRESHOLD)
#define PITCH_STICK_CENTERED() \
(radio_control.values[RADIO_PITCH] < AUTOPILOT_STICK_CENTER_THRESHOLD && \
radio_control.values[RADIO_PITCH] > -AUTOPILOT_STICK_CENTER_THRESHOLD)
#define ROLL_STICK_CENTERED() \
(radio_control.values[RADIO_ROLL] < AUTOPILOT_STICK_CENTER_THRESHOLD && \
radio_control.values[RADIO_ROLL] > -AUTOPILOT_STICK_CENTER_THRESHOLD)
/** RC mode switch position helper
* switch positions threshold are evenly spaced
*
* @param[in] chan RC mode channel number
* @param[in] pos switch position to be tested
* @param[in] max maximum number of position of the switch
* @return true if current position is the same as requested (and RC status is OK)
*/
static inline bool rc_mode_switch(uint8_t chan, uint8_t pos, uint8_t max)
{
if (radio_control.status != RC_OK) return false;
if (pos >= max) return false;
int32_t v = (int32_t)radio_control.values[chan] - MIN_PPRZ;
// round final value
int32_t p = (((((int32_t)max - 1) * 10 * v) / (MAX_PPRZ - MIN_PPRZ)) + 5) / 10;
Bound(p, 0, max - 1); // just in case
return pos == (uint8_t)p;
}
/** Convenience macro for 3way switch
*/
#ifdef RADIO_MODE
#define RCMode0() rc_mode_switch(RADIO_MODE, 0, 3)
#define RCMode1() rc_mode_switch(RADIO_MODE, 1, 3)
#define RCMode2() rc_mode_switch(RADIO_MODE, 2, 3)
#endif
static inline bool rc_attitude_sticks_centered(void)
{
return ROLL_STICK_CENTERED() && PITCH_STICK_CENTERED() && YAW_STICK_CENTERED();
}
#ifdef RADIO_KILL_SWITCH
static inline bool kill_switch_is_on(void)
{
if (radio_control.values[RADIO_KILL_SWITCH] < 0) {
return true;
} else {
return false;
}
}
#else
static inline bool kill_switch_is_on(void)
{
return false;
}
#endif
static inline uint8_t percent_from_rc(int channel)
{
int per = (MAX_PPRZ + (int32_t)radio_control.values[channel]) * 50 / MAX_PPRZ;
if (per < 0) {
per = 0;
} else if (per > 100) {
per = 100;
}
return per;
}
#endif /* AUTOPILOT_RC_HELPERS_H */
sw/airborne/firmwares/rover/autopilot_utils.c
+66
View File
@@ -0,0 +1,66 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/rover/autopilot_utils.c
*
* Utility functions and includes for autopilots
*
*/
#include "firmwares/rotorcraft/autopilot_utils.h"
#include "autopilot.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
#include "state.h"
#include "subsystems/radio_control.h"
/** Display descent speed in failsafe mode if needed */
PRINT_CONFIG_VAR(FAILSAFE_DESCENT_SPEED)
// Utility functions
#ifndef AUTOPILOT_DISABLE_AHRS_KILL
bool ap_ahrs_is_aligned(void)
{
return stateIsAttitudeValid();
}
#else
PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
bool ap_ahrs_is_aligned(void)
{
return true;
}
#endif
#define THRESHOLD_1_PPRZ (MIN_PPRZ / 2)
#define THRESHOLD_2_PPRZ (MAX_PPRZ / 2)
/** get autopilot mode as set by RADIO_MODE 3-way switch */
uint8_t ap_mode_of_3way_switch(void)
{
if (radio_control.values[RADIO_MODE] > THRESHOLD_2_PPRZ) {
return MODE_AUTO2;
} else if (radio_control.values[RADIO_MODE] > THRESHOLD_1_PPRZ) {
return MODE_AUTO1;
} else {
return MODE_MANUAL;
}
}
sw/airborne/firmwares/rover/autopilot_utils.h
+43
View File
@@ -0,0 +1,43 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/rover/autopilot_utils.h
*
* Utility functions and includes for autopilots
*
*/
#ifndef AUTOPILOT_UTILS_H
#define AUTOPILOT_UTILS_H
#include "std.h"
#include "subsystems/commands.h"
extern bool ap_ahrs_is_aligned(void);
extern uint8_t ap_mode_of_3way_switch(void);
extern void set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight, bool motors_on);
// in case, backward compatibility macro
#define SetRotorcraftCommands(_cmd, _in_flight, _motors_on) set_rotorcraft_commands(commands, _cmd, _in_flight, _motors_on)
#endif // AUTOPILOT_UTILS_H
sw/airborne/firmwares/rover/guidance/rover_guidance.c
+250
View File
@@ -0,0 +1,250 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/** @file firmwares/rover/guidance/rover_guidance.c
* Basic guidance for rover.
* Implement standard PID control loop to track a navigation target.
* Guidance "modes" are using the autopilot generation with the "guidance"
* state machine.
*/
#define AUTOPILOT_CORE_GUIDANCE_C
#include "firmwares/rover/guidance/rover_guidance.h"
#include "generated/airframe.h"
#include "generated/autopilot_core_guidance.h"
#include "state.h"
struct RoverGuidance rover_guidance;
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
// TODO rover guidance messages
#endif
void rover_guidance_init(void)
{
FLOAT_VECT2_ZERO(rover_guidance.sp.pos);
rover_guidance.sp.heading = 0.0f;
rover_guidance.speed_pid.p = ROVER_GUIDANCE_SPEED_PGAIN;
rover_guidance.speed_pid.i = ROVER_GUIDANCE_SPEED_IGAIN;
rover_guidance.speed_pid.d = ROVER_GUIDANCE_SPEED_DGAIN;
rover_guidance.speed_pid.err = 0.f;
rover_guidance.speed_pid.d_err = 0.f;
rover_guidance.speed_pid.sum_err = 0.f;
rover_guidance.turn_pid.p = ROVER_GUIDANCE_TURN_PGAIN;
rover_guidance.turn_pid.i = ROVER_GUIDANCE_TURN_IGAIN;
rover_guidance.turn_pid.d = ROVER_GUIDANCE_TURN_DGAIN;
rover_guidance.turn_pid.err = 0.f;
rover_guidance.turn_pid.d_err = 0.f;
rover_guidance.turn_pid.sum_err = 0.f;
#if PERIODIC_TELEMETRY
// TODO register messages
#endif
// from code generation
autopilot_core_guidance_init();
}
void rover_guidance_periodic(void)
{
// from code generation
autopilot_core_guidance_periodic_task();
}
static float compute_pid(struct RoverGuidancePID *pid)
{
return pid->p * pid->err + pid->d * pid->d_err + pid->i * pid->sum_err;
}
#define MAX_POS_ERR 10.f // max position error
#define MAX_SPEED_ERR 10.f // max speed error
#define MAX_INTEGRAL_CMD (MAX_PPRZ / 10.f) // 10% of max command
#define PROXIMITY_DIST 0.5f // proximity distance TODO improve
void rover_guidance_run(float *heading_sp)
{
// compute position error
struct FloatVect2 pos_err;
VECT2_DIFF(pos_err, rover_guidance.sp.pos, *stateGetPositionEnu_f());
rover_guidance.speed_pid.err = float_vect2_norm(&pos_err);
BoundAbs(rover_guidance.speed_pid.err, MAX_POS_ERR);
// speed and heading update when far enough
if (rover_guidance.speed_pid.err > PROXIMITY_DIST) {
// compute speed error
rover_guidance.speed_pid.d_err = - stateGetHorizontalSpeedNorm_f();
BoundAbs(rover_guidance.speed_pid.d_err, MAX_SPEED_ERR);
// integral
rover_guidance.speed_pid.sum_err = 0.f; // nothing for now
// run PID
rover_guidance.cmd.motor_speed = MAX_PPRZ * compute_pid(&rover_guidance.speed_pid);
rover_guidance.cmd.motor_speed = TRIM_PPRZ(rover_guidance.cmd.motor_speed);
// if not close to WP, compute desired heading
rover_guidance.sp.heading = atan2f(pos_err.x, pos_err.y);
*heading_sp = rover_guidance.sp.heading; // update nav sp
}
else {
rover_guidance.cmd.motor_speed = 0.f;
// use nav heading ref and run angular control
rover_guidance.sp.heading = *heading_sp;
}
// angular error
rover_guidance.turn_pid.err = rover_guidance.sp.heading - stateGetNedToBodyEulers_f()->psi;
NormRadAngle(rover_guidance.turn_pid.err);
// turn rate error
rover_guidance.turn_pid.d_err = - stateGetBodyRates_f()->r;
// integral
rover_guidance.turn_pid.sum_err = 0.f; // nothing for now
// run PID
rover_guidance.cmd.motor_turn = MAX_PPRZ * compute_pid(&rover_guidance.turn_pid);
rover_guidance.cmd.motor_turn = TRIM_PPRZ(rover_guidance.cmd.motor_turn);
}
void rover_guidance_enter(void)
{
ClearBit(rover_guidance.sp.mask, 5);
ClearBit(rover_guidance.sp.mask, 7);
/* horizontal position setpoint from navigation/flightplan */
//INT32_VECT2_NED_OF_ENU(rover_guidance.sp.pos, navigation_carrot); TODO move to AP xml
//nav_heading = stateGetNedToBodyEulers_i()->psi; TODO move to AP xml
rover_guidance.sp.heading = stateGetNedToBodyEulers_f()->psi;
// TODO reset integral part ?
}
// TODO from AP xml
//void rover_guidance_from_nav(bool in_flight)
//{
// if (!in_flight) {
// rover_guidance_nav_enter();
// }
//
// if (horizontal_mode == HORIZONTAL_MODE_MANUAL) {
// stabilization_cmd[COMMAND_ROLL] = nav_cmd_roll;
// stabilization_cmd[COMMAND_PITCH] = nav_cmd_pitch;
// stabilization_cmd[COMMAND_YAW] = nav_cmd_yaw;
// } else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
// struct Int32Eulers sp_cmd_i;
// sp_cmd_i.phi = nav_roll;
// sp_cmd_i.theta = nav_pitch;
// sp_cmd_i.psi = nav_heading;
// stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
// stabilization_attitude_run(in_flight);
// } else {
// INT32_VECT2_NED_OF_ENU(rover_guidance.sp.pos, navigation_carrot);
//
// /* set psi command */
// rover_guidance.sp.heading = ANGLE_FLOAT_OF_BFP(nav_heading);
// FLOAT_ANGLE_NORMALIZE(rover_guidance.sp.heading);
//
// /* compute x,y earth commands */
// rover_guidance_traj_run(in_flight);
// /* set final attitude setpoint */
// int32_t heading_sp_i = ANGLE_BFP_OF_REAL(rover_guidance.sp.heading);
// stabilization_attitude_set_earth_cmd_i(&rover_guidance_cmd_earth,
// heading_sp_i);
// stabilization_attitude_run(in_flight);
// }
//}
void rover_guidance_set_speed_igain(uint32_t igain)
{
rover_guidance.speed_pid.i = igain;
rover_guidance.speed_pid.sum_err = 0.f;
}
void rover_guidance_set_turn_igain(uint32_t igain)
{
rover_guidance.turn_pid.i = igain;
rover_guidance.turn_pid.sum_err = 0.f;
}
#ifdef ROVER_GUIDANCE_MODE_GUIDED
void rover_guidance_guided_run(bool in_flight)
{
/* rover_guidance.sp.pos and rover_guidance.sp.heading need to be set from external source */
if (!in_flight) {
rover_guidance_hover_enter();
}
/* compute x,y earth commands */
rover_guidance_traj_run(in_flight);
}
bool rover_guidance_set_guided_pos(float x, float y)
{
if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
ClearBit(rover_guidance.sp.mask, 5);
rover_guidance.sp.pos.x = x;
rover_guidance.sp.pos.y = y;
return true;
}
return false;
}
bool rover_guidance_set_guided_heading(float heading)
{
if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
ClearBit(rover_guidance.sp.mask, 7);
rover_guidance.sp.heading = heading;
FLOAT_ANGLE_NORMALIZE(rover_guidance.sp.heading);
return true;
}
return false;
}
bool rover_guidance_set_guided_body_vel(float vx, float vy)
{
float psi = stateGetNedToBodyEulers_f()->psi;
float newvx = cosf(-psi) * vx + sinf(-psi) * vy;
float newvy = -sinf(-psi) * vx + cosf(-psi) * vy;
return rover_guidance_set_guided_vel(newvx, newvy);
}
bool rover_guidance_set_guided_vel(float vx, float vy)
{
if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
SetBit(rover_guidance.sp.mask, 5);
rover_guidance.sp.speed.x = vx;
rover_guidance.sp.speed.y = vy;
return true;
}
return false;
}
bool rover_guidance_set_guided_heading_rate(float rate)
{
if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
SetBit(rover_guidance.sp.mask, 7);
rover_guidance.sp.heading_rate = rate;
return true;
}
return false;
}
#endif
sw/airborne/firmwares/rover/guidance/rover_guidance.h
+136
View File
@@ -0,0 +1,136 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/** @file firmwares/rover/guidance/rover_guidance.h
* Basic guidance for rover.
* Implement standard PID control loop to track a navigation target.
* Guidance "modes" are using the autopilot generation with the "guidance"
* state machine.
*/
#ifndef ROVER_GUIDANCE_H
#define ROVER_GUIDANCE_H
#include "math/pprz_algebra_float.h"
#include "generated/airframe.h"
#include "std.h"
struct RoverGuidanceSetpoint {
/** position setpoint in NED.
*/
struct FloatVect2 pos; ///< position setpoint
struct FloatVect2 speed; ///< speed setpoint
float heading; ///< heading setpoint
uint8_t mask; ///< bit 5: vx & vy, bit 6: vz, bit 7: vyaw
};
// TODO write a generic PID somewhere else ?
struct RoverGuidancePID {
float p;
float i;
float d;
float err;
float d_err;
float sum_err;
};
struct RoverGuidanceControl {
float motor_speed;
float motor_turn;
};
struct RoverGuidance {
struct RoverGuidanceSetpoint sp; ///< setpoints
struct RoverGuidanceControl cmd; ///< commands
struct RoverGuidancePID speed_pid; ///< motor speed controller
struct RoverGuidancePID turn_pid; ///< turn rate controller
};
extern struct RoverGuidance rover_guidance;
extern void rover_guidance_init(void);
extern void rover_guidance_periodic(void); // call state machine
extern void rover_guidance_run(float *heading_sp); // run control loop
extern void rover_guidance_enter(void);
extern void rover_guidance_set_speed_igain(uint32_t igain);
extern void rover_guidance_set_turn_igain(uint32_t igain);
#ifdef GUIDANCE_MODE_GUIDED
/** Run GUIDED mode control
*/
extern void rover_guidance_guided_run(bool in_flight);
/** Set horizontal position setpoint in GUIDED mode.
* @param x North position (local NED frame) in meters.
* @param y East position (local NED frame) in meters.
* @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
*/
extern bool rover_guidance_set_guided_pos(float x, float y);
/** Set heading setpoint in GUIDED mode.
* @param heading Setpoint in radians.
* @return TRUE if setpoint was set (currently in GUIDANCE_H_MODE_GUIDED)
*/
extern bool rover_guidance_set_guided_heading(float heading);
/** Set body relative horizontal velocity setpoint in GUIDED mode.
* @param vx forward velocity (body frame) in meters/sec.
* @param vy right velocity (body frame) in meters/sec.
* @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
*/
extern bool rover_guidance_set_guided_body_vel(float vx, float vy);
/** Set horizontal velocity setpoint in GUIDED mode.
* @param vx North velocity (local NED frame) in meters/sec.
* @param vy East velocity (local NED frame) in meters/sec.
* @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
*/
extern bool rover_guidance_set_guided_vel(float vx, float vy);
/** Set heading rate setpoint in GUIDED mode.
* @param rate Heading rate in radians.
* @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
*/
extern bool rover_guidance_set_guided_heading_rate(float rate);
#endif
//static inline void rover_guidance_SetMaxSpeed(float speed)
//{
// gh_set_max_speed(speed);
//}
//
//static inline void rover_guidance_SetOmega(float omega)
//{
// gh_set_omega(omega);
//}
//
//static inline void rover_guidance_SetZeta(float zeta)
//{
// gh_set_zeta(zeta);
//}
//
//static inline void rover_guidance_SetTau(float tau)
//{
// gh_set_tau(tau);
//}
#endif /* ROVER_GUIDANCE_H */
sw/airborne/firmwares/rover/main_ap.c
+289
View File
@@ -0,0 +1,289 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*
*/
/**
* @file firmwares/rover/main_ap.c
*
* Rover main loop.
*/
#define MODULES_C
#define ABI_C
#include <inttypes.h>
#include "mcu.h"
#include "mcu_periph/sys_time.h"
#include "led.h"
#include "subsystems/datalink/telemetry.h"
#include "subsystems/datalink/datalink.h"
#include "subsystems/datalink/downlink.h"
#include "subsystems/settings.h"
#include "subsystems/commands.h"
#include "subsystems/actuators.h"
#if USE_IMU
#include "subsystems/imu.h"
#endif
#if USE_GPS
#include "subsystems/gps.h"
#endif
#if USE_BARO_BOARD
#include "subsystems/sensors/baro.h"
PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD)
#endif
#include "subsystems/electrical.h"
#include "autopilot.h"
#include "subsystems/radio_control.h"
#include "subsystems/ahrs.h"
#include "state.h"
#include "firmwares/rover/main_ap.h"
#include "generated/modules.h"
#include "subsystems/abi.h"
// needed for stop-gap measure waypoints_localize_all()
#include "subsystems/navigation/waypoints.h"
/* if PRINT_CONFIG is defined, print some config options */
PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
/* SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY should be an integer, otherwise the timer will not be correct */
#if !(SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY*PERIODIC_FREQUENCY == SYS_TIME_FREQUENCY)
#warning "The SYS_TIME_FREQUENCY can not be divided by PERIODIC_FREQUENCY. Make sure this is the case for correct timing."
#endif
/* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
* defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
*/
PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
/* MODULES_FREQUENCY is defined in generated/modules.h
* according to main_freq parameter set for modules in airframe file
*/
PRINT_CONFIG_VAR(MODULES_FREQUENCY)
#ifndef BARO_PERIODIC_FREQUENCY
#define BARO_PERIODIC_FREQUENCY 50
#endif
PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
#if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
#if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
#warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY)
#endif
#endif
tid_t main_periodic_tid; ///< id for main_periodic() timer
tid_t modules_tid; ///< id for modules_periodic_task() timer
tid_t failsafe_tid; ///< id for failsafe_check() timer
tid_t radio_control_tid; ///< id for radio_control_periodic_task() timer
tid_t electrical_tid; ///< id for electrical_periodic() timer
tid_t telemetry_tid; ///< id for telemetry_periodic() timer
#if USE_BARO_BOARD
tid_t baro_tid; ///< id for baro_periodic() timer
#endif
void main_init(void)
{
mcu_init();
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
pprz_trig_int_init();
#endif
electrical_init();
stateInit();
actuators_init();
#if USE_MOTOR_MIXING
motor_mixing_init();
#endif
radio_control_init();
#if USE_BARO_BOARD
baro_init();
#endif
#if USE_AHRS
ahrs_init();
#endif
autopilot_init();
modules_init();
// call autopilot implementation init after guidance modules init
// it will set startup mode
autopilot_generated_init();
/* temporary hack:
* Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
* This led to the problem that global waypoints were not "localized",
* so as a stop-gap measure we localize them all (again) here..
*/
//waypoints_localize_all();
settings_init();
mcu_int_enable();
#if DOWNLINK
downlink_init();
#endif
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
#if PERIODIC_FREQUENCY != MODULES_FREQUENCY
modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
#endif
radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
failsafe_tid = sys_time_register_timer(0.05, NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif
#if USE_IMU
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif
// Do a failsafe check first
failsafe_check();
}
void handle_periodic_tasks(void)
{
if (sys_time_check_and_ack_timer(main_periodic_tid)) {
main_periodic();
#if PERIODIC_FREQUENCY == MODULES_FREQUENCY
/* Use the main periodc freq timer for modules if the freqs are the same
* This is mainly useful for logging each step.
*/
modules_periodic_task();
#else
}
/* separate timer for modules, since it has a different freq than main */
if (sys_time_check_and_ack_timer(modules_tid)) {
modules_periodic_task();
#endif
}
if (sys_time_check_and_ack_timer(radio_control_tid)) {
radio_control_periodic_task();
}
if (sys_time_check_and_ack_timer(failsafe_tid)) {
failsafe_check();
}
if (sys_time_check_and_ack_timer(electrical_tid)) {
electrical_periodic();
}
if (sys_time_check_and_ack_timer(telemetry_tid)) {
telemetry_periodic();
}
#if USE_BARO_BOARD
if (sys_time_check_and_ack_timer(baro_tid)) {
baro_periodic();
}
#endif
}
void main_periodic(void)
{
/* run control loops */
autopilot_periodic();
/* set actuators */
//actuators_set(autopilot_get_motors_on());
SetActuatorsFromCommands(commands, autopilot_get_mode());
if (autopilot_in_flight()) {
RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++);
}
#if defined DATALINK || defined SITL
RunOnceEvery(PERIODIC_FREQUENCY, datalink_time++);
#endif
RunOnceEvery(10, LED_PERIODIC());
}
void telemetry_periodic(void)
{
static uint8_t boot = true;
/* initialisation phase during boot */
if (boot) {
#if DOWNLINK
autopilot_send_version();
#endif
boot = false;
}
/* then report periodicly */
else {
#if PERIODIC_TELEMETRY
periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
#endif
}
}
/** mode to enter when RC is lost while using a mode with RC input (not AP_MODE_NAV) */
#ifndef RC_LOST_MODE
#define RC_LOST_MODE AP_MODE_FAILSAFE
#endif
void failsafe_check(void)
{
autopilot_check_in_flight(autopilot_get_motors_on());
}
void main_event(void)
{
/* event functions for mcu peripherals: i2c, usb_serial.. */
mcu_event();
if (autopilot.use_rc) {
RadioControlEvent(autopilot_on_rc_frame);
}
#if USE_BARO_BOARD
BaroEvent();
#endif
autopilot_event();
modules_event_task();
}
sw/airborne/firmwares/rover/main_ap.h
+40
View File
@@ -0,0 +1,40 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/rover/main_ap.h
*
* Rover main loop.
*/
#ifndef MAIN_H
#define MAIN_H
extern void main_init(void);
extern void main_event(void);
extern void handle_periodic_tasks(void);
extern void main_periodic(void);
extern void telemetry_periodic(void);
extern void failsafe_check(void);
#endif /* MAIN_H */
sw/airborne/firmwares/rover/main_chibios.c
+102
View File
@@ -0,0 +1,102 @@
/*
* Copyright (C) 2018 Gautier Hattenberger, Alexandre Bustico
*
* 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/rover/main_chibios.c
*/
#include "mcu_periph/sys_time.h"
#include "mcu.h"
#include <ch.h>
#ifndef SYS_TIME_FREQUENCY
#error SYS_TIME_FREQUENCY should be defined in Makefile.chibios or airframe.xml and be equal to CH_CFG_ST_FREQUENCY
#elif SYS_TIME_FREQUENCY != CH_CFG_ST_FREQUENCY
#error SYS_TIME_FREQUENCY should be equal to CH_CFG_ST_FREQUENCY
#elif CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY)
#error CH_CFG_ST_FREQUENCY and SYS_TIME_FREQUENCY should be >= 2 x PERIODIC_FREQUENCY
#endif
#include "firmwares/rotorcraft/main_ap.h"
#ifndef THD_WORKING_AREA_MAIN
#define THD_WORKING_AREA_MAIN 8192
#endif
/*
* PPRZ/AP thread
* Also include FBW on single MCU
*/
static void thd_ap(void *arg);
THD_WORKING_AREA(wa_thd_ap, THD_WORKING_AREA_MAIN);
static thread_t *apThdPtr = NULL;
/**
* Main function
*/
int main(void)
{
// Init
main_init();
chThdSleepMilliseconds(100);
// Create threads
apThdPtr = chThdCreateStatic(wa_thd_ap, sizeof(wa_thd_ap), NORMALPRIO, thd_ap, NULL);
// Main loop, do nothing
while (TRUE) {
chThdSleepMilliseconds(1000);
}
return 0;
}
/*
* PPRZ/AP thread
*
* Call PPRZ AP periodic and event functions
*/
static void thd_ap(void *arg)
{
(void) arg;
chRegSetThreadName("AP");
while (!chThdShouldTerminateX()) {
handle_periodic_tasks();
main_event();
chThdSleepMicroseconds(500);
}
chThdExit(0);
}
/*
* Terminate autopilot threads
* Wait until proper stop
*/
void pprz_terminate_autopilot_threads(void)
{
if (apThdPtr != NULL) {
chThdTerminate(apThdPtr);
chThdWait(apThdPtr);
apThdPtr = NULL;
}
}
sw/airborne/firmwares/rover/main_chibios.h
+35
View File
@@ -0,0 +1,35 @@
/*
* Copyright (C) 2018 Gautier Hattenberger, Alexandre Bustico
*
* 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/rover/main_chibios.h
*/
#ifndef MAIN_CHIBIOS_H
#define MAIN_CHIBIOS_H
#include <ch.h>
/** Terminate all autopilot threads
* Wait until proper stop
*/
extern void pprz_terminate_autopilot_threads(void);
#endif /* MAIN_CHIBIOS_H */
sw/airborne/firmwares/rover/navigation.c
+308
View File
@@ -0,0 +1,308 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/rover/navigation.c
*
* Rover navigation functions.
*/
#define NAV_C
#include "firmwares/rover/navigation.h"
#include "pprz_debug.h"
#include "subsystems/gps.h" // needed by auto_nav from the flight plan
#include "subsystems/ins.h"
#include "state.h"
#include "autopilot.h"
#include "generated/modules.h"
#include "generated/flight_plan.h"
#include "math/pprz_algebra_int.h"
#include "subsystems/datalink/downlink.h"
#include "pprzlink/messages.h"
#include "mcu_periph/uart.h"
struct RoverNavigation nav;
uint8_t last_wp __attribute__((unused));
const float max_dist_from_home = MAX_DIST_FROM_HOME;
const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE;
void set_exception_flag(uint8_t flag_num)
{
nav.exception_flag[flag_num] = true;
}
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
{
static uint8_t i;
i++;
if (i >= nb_waypoint) { i = 0; }
pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
&i,
&(waypoints[i].enu_i.x),
&(waypoints[i].enu_i.y),
&(waypoints[i].enu_i.z));
}
#endif
void nav_init(void)
{
waypoints_init();
nav_block = 0;
nav_stage = 0;
nav.mode = NAV_MODE_WAYPOINT;
VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
VECT3_COPY(nav.carrot, waypoints[WP_HOME].enu_f);
nav.heading = 0.f;
nav.radius = DEFAULT_CIRCLE_RADIUS;
nav.speed = 0.f;
nav.turn = 0.f;
nav.shift = 0.f;
nav.too_far_from_home = false;
nav.dist2_to_home = 0.f;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WP_MOVED, send_wp_moved);
#endif
// generated init function
auto_nav_init();
}
void nav_run(void)
{
VECT2_COPY(nav.carrot, nav.target);
}
bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time)
{
(void) wp;
(void) stay_time;
return true;
// uint16_t time_at_wp;
// float dist_to_point;
// static uint16_t wp_entry_time = 0;
// static bool wp_reached = false;
// static struct EnuCoor_i wp_last = { 0, 0, 0 };
// struct Int32Vect2 diff;
//
// if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
// wp_reached = false;
// wp_last = *wp;
// }
//
// VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
// struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
// dist_to_point = float_vect2_norm(&diff_f);
// if (dist_to_point < ARRIVED_AT_WAYPOINT) {
// if (!wp_reached) {
// wp_reached = true;
// wp_entry_time = autopilot.flight_time;
// time_at_wp = 0;
// } else {
// time_at_wp = autopilot.flight_time - wp_entry_time;
// }
// } else {
// time_at_wp = 0;
// wp_reached = false;
// }
// if (time_at_wp > stay_time) {
// INT_VECT3_ZERO(wp_last);
// return true;
// }
// return false;
}
/** Reset the geographic reference to the current GPS fix */
void nav_reset_reference(void)
{
ins_reset_local_origin();
/* update local ENU coordinates of global waypoints */
waypoints_localize_all();
}
void nav_reset_alt(void)
{
ins_reset_altitude_ref();
waypoints_localize_all();
}
void nav_init_stage(void)
{
VECT3_COPY(nav.last_pos, *stateGetPositionEnu_f());
stage_time = 0;
//nav_circle_radians = 0; FIXME
}
void nav_periodic_task(void)
{
RunOnceEvery(NAV_FREQ, { stage_time++; block_time++; });
//nav.dist2_to_wp = 0; FIXME
/* from flight_plan.h */
auto_nav();
/* run carrot loop */
nav_run();
}
bool nav_is_in_flight(void)
{
return autopilot_in_flight();
}
/** Home mode navigation */
void nav_home(void)
{
nav.mode = NAV_MODE_WAYPOINT;
VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
//nav.dist2_to_wp = nav.dist2_to_home; FIXME
/* run carrot loop */
nav_run();
}
/** Returns squared horizontal distance to given point */
float get_dist2_to_point(struct EnuCoor_f *p)
{
struct EnuCoor_f *pos = stateGetPositionEnu_f();
struct FloatVect2 pos_diff = {
.x = p->x - pos->x,
.y = p->y - pos->y
};
return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
}
/** Returns squared horizontal distance to given waypoint */
float get_dist2_to_waypoint(uint8_t wp_id)
{
return get_dist2_to_point(&waypoints[wp_id].enu_f);
}
/** Computes squared distance to the HOME waypoint potentially sets
* #too_far_from_home
*/
void compute_dist2_to_home(void)
{
nav.dist2_to_home = get_dist2_to_waypoint(WP_HOME);
nav.too_far_from_home = nav.dist2_to_home > max_dist2_from_home;
#ifdef InGeofenceSector
struct EnuCoor_f *pos = stateGetPositionEnu_f();
nav.too_far_from_home = nav.too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
#endif
}
/** Set nav_heading in radians. */
void nav_set_heading_rad(float rad)
{
nav.heading = rad;
NormCourseRad(nav.heading);
}
/** Set nav_heading in degrees. */
void nav_set_heading_deg(float deg)
{
nav_set_heading_rad(RadOfDeg(deg));
}
/** Set heading to point towards x,y position in local coordinates */
void nav_set_heading_towards(float x, float y)
{
struct FloatVect2 target = {x, y};
struct FloatVect2 pos_diff;
VECT2_DIFF(pos_diff, target, *stateGetPositionEnu_f());
// don't change heading if closer than 0.5m to target
if (VECT2_NORM2(pos_diff) > 0.25f) {
nav.heading = atan2f(pos_diff.x, pos_diff.y);
}
}
/** Set heading in the direction of a waypoint */
void nav_set_heading_towards_waypoint(uint8_t wp)
{
nav_set_heading_towards(WaypointX(wp), WaypointY(wp));
}
/** Set heading in the direction of the target*/
void nav_set_heading_towards_target(void)
{
nav_set_heading_towards(nav.target.x, nav.target.y);
}
/** Set heading to the current yaw angle */
void nav_set_heading_current(void)
{
nav.heading = stateGetNedToBodyEulers_f()->psi;
}
void nav_set_failsafe(void)
{
#ifdef AP_MODE_FAILSAFE
autopilot_set_mode(AP_MODE_FAILSAFE);
#endif
}
/** Register functions
*/
void nav_register_goto_wp(nav_rover_goto nav_goto, nav_rover_route nav_route, nav_rover_approaching nav_approaching)
{
nav.nav_goto = nav_goto;
nav.nav_route = nav_route;
nav.nav_approaching = nav_approaching;
}
void nav_register_circle(nav_rover_circle nav_circle)
{
nav.nav_circle = nav_circle;
}
void nav_register_oval(nav_rover_oval_init nav_oval_init, nav_rover_oval nav_oval)
{
nav.nav_oval_init = nav_oval_init;
nav.nav_oval = nav_oval;
}
sw/airborne/firmwares/rover/navigation.h
+318
View File
@@ -0,0 +1,318 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/rover/navigation.h
*
* Rover navigation functions.
*/
#ifndef NAVIGATION_H
#define NAVIGATION_H
#include "std.h"
#include "math/pprz_geodetic_float.h"
#include "state.h"
#include "subsystems/navigation/waypoints.h"
#include "subsystems/navigation/common_flight_plan.h"
#include "autopilot.h"
/** default approaching_time for a wp */ // FIXME
#ifndef CARROT
#define CARROT 0.f
#endif
#ifndef CARROT_DIST
#define CARROT_DIST 2.f
#endif
#ifndef NAV_FREQ
#define NAV_FREQ 16
#endif
/** default nav_circle_radius in meters */
#ifndef DEFAULT_CIRCLE_RADIUS
#define DEFAULT_CIRCLE_RADIUS 6.0f
#endif
/** minimum horizontal distance to waypoint to mark as arrived */
#ifndef ARRIVED_AT_WAYPOINT
#define ARRIVED_AT_WAYPOINT 3.0f
#endif
/** Maximum distance from HOME waypoint before going into failsafe mode */
#ifndef FAILSAFE_MODE_DISTANCE
#define FAILSAFE_MODE_DISTANCE (1.2*MAX_DIST_FROM_HOME)
#endif
/** Nav modes */
#define NAV_MODE_WAYPOINT 0
#define NAV_MODE_ROUTE 1
#define NAV_MODE_CIRCLE 2
#define NAV_MODE_HEADING 3
#define NAV_MODE_MANUAL 4
typedef void (*nav_rover_goto)(struct EnuCoor_f *wp);
typedef void (*nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end);
typedef bool (*nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time);
typedef void (*nav_rover_circle)(struct EnuCoor_f *wp_center, float radius);
typedef void (*nav_rover_oval_init)(void);
typedef void (*nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius);
/** General Navigation structure
*/
struct RoverNavigation {
// mode
uint8_t mode; // nav mode
// commands
struct EnuCoor_f target; ///< final target
struct EnuCoor_f carrot; ///< carrot position
float heading; ///< heading setpoint (in radians)
float radius; ///< radius setpoint
float speed; ///< speed setpoint
float turn; ///< turn rate setpoint
float shift; ///< lateral shift (in meters)
// misc
float dist2_to_home; ///< squared distance to home waypoint
bool too_far_from_home; ///< too_far flag
float failsafe_mode_dist2; ///< maximum squared distance to home wp before going to failsafe mode
bool exception_flag[10]; ///< array of flags that might be used in flight plans
struct EnuCoor_f last_pos; ///< last stage position
// pointers to basic nav functions
nav_rover_goto nav_goto;
nav_rover_route nav_route;
nav_rover_approaching nav_approaching;
nav_rover_circle nav_circle;
nav_rover_oval_init nav_oval_init;
nav_rover_oval nav_oval;
};
extern struct RoverNavigation nav;
/** Registering functions
*/
extern void nav_register_goto_wp(nav_rover_goto nav_goto,
nav_rover_route nav_route,
nav_rover_approaching nav_approaching);
extern void nav_register_circle(nav_rover_circle nav_circle);
extern void nav_register_oval(nav_rover_oval_init nav_oval_init, nav_rover_oval nav_oval);
// TODO: eight, survey
/*****************************************************************
* macros to ensure compatibility between fixedwing and rotorcraft
*****************************************************************/
/// Get current x (east) position in local coordinates
#define GetPosX() (stateGetPositionEnu_f()->x)
/// Get current y (north) position in local coordinates
#define GetPosY() (stateGetPositionEnu_f()->y)
/// Get current altitude above MSL
#define GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
/**
* Get current altitude reference for local coordinates.
* This is the ground_alt from the flight plan at first,
* but might be updated later through a call to NavSetGroundReferenceHere() or
* NavSetAltitudeReferenceHere(), e.g. in the GeoInit flight plan block.
*/
#define GetAltRef() (state.ned_origin_f.hmsl)
/** Normalize a degree angle between 0 and 359 */
#define NormCourse(x) { \
while (x < 0) x += 360; \
while (x >= 360) x -= 360; \
}
/** Normalize a rad angle between 0 and 2*PI */
#define NormCourseRad(x) { \
while (x < 0) x += 2*M_PI; \
while (x >= 2*M_PI) x -= 2*M_PI; \
}
extern uint8_t last_wp __attribute__((unused));
extern void nav_init(void);
extern void nav_run(void);
extern void set_exception_flag(uint8_t flag_num);
extern float get_dist2_to_waypoint(uint8_t wp_id);
extern float get_dist2_to_point(struct EnuCoor_f *p);
extern void compute_dist2_to_home(void);
extern void nav_home(void);
extern void nav_set_manual(float speed, float turn);
extern void nav_reset_reference(void) __attribute__((unused));
extern void nav_periodic_task(void);
extern bool nav_is_in_flight(void);
/** heading utility functions */
extern void nav_set_heading_rad(float rad);
extern void nav_set_heading_deg(float deg);
extern void nav_set_heading_towards(float x, float y);
extern void nav_set_heading_towards_waypoint(uint8_t wp);
extern void nav_set_heading_towards_target(void);
extern void nav_set_heading_current(void);
extern void nav_set_failsafe(void);
/* switching motors on/off */
static inline void NavKillThrottle(void)
{
if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); }
}
static inline void NavResurrect(void)
{
if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); }
}
#define NavSetManual(_roll, _pitch, _yaw) _Pragma("GCC error \"Manual mode in flight plan for fixedwing is not available\"")
#define NavSetFailsafe nav_set_failsafe
#define NavSetGroundReferenceHere nav_reset_reference
#define NavSetAltitudeReferenceHere nav_reset_alt
#define NavSetWaypointHere waypoint_set_here_2d
#define NavCopyWaypoint waypoint_copy
#define NavCopyWaypointPositionOnly waypoint_position_copy
/** Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp */
bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time);
#define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_f, time)
/***********************************************************
* macros used by flight plan to set different modes
**********************************************************/
// command direct turn rate from FP roll [-1.; 1.]
#define NavAttitude(_roll) { \
nav.mode = NAV_MODE_MANUAL; \
nav.turn = _roll; \
BoundAbs(nav.turn, 1.f); \
}
// command direct motor speed from FP throttle [-1.; 1.]
#define NavVerticalThrottleMode(_speed) { \
nav.speed = _speed; \
BoundAbs(nav.speed, 1.f); \
}
/** Set the heading of the rover, nothing else */
#define NavHeading nav_set_heading_rad
/***********************************************************
* built in navigation routines
**********************************************************/
/*********** Navigation to waypoint *************************************/
static inline void NavGotoWaypoint(uint8_t wp)
{
nav.mode = NAV_MODE_WAYPOINT;
VECT3_COPY(nav.target, waypoints[wp].enu_f);
//nav.dist2_to_wp = get_dist2_to_waypoint(wp); FIXME
}
/*********** Navigation along a line *************************************/
static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
{
nav.mode = NAV_MODE_ROUTE;
if (nav.nav_route) {
nav.nav_route(&waypoints[wp_start].enu_f, &waypoints[wp_end].enu_f);
}
}
static inline bool NavApproaching(uint8_t wp, float approaching_time)
{
if (nav.nav_approaching) {
return nav.nav_approaching(&waypoints[wp].enu_f, NULL, approaching_time);
}
else {
return true;
}
}
static inline bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
{
if (nav.nav_approaching) {
return nav.nav_approaching(&waypoints[to].enu_f, &waypoints[from].enu_f, approaching_time);
}
else {
return true;
}
}
/*********** Navigation on a circle **************************************/
static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
{
nav.mode = NAV_MODE_CIRCLE;
if (nav.nav_circle) {
nav.nav_circle(&waypoints[wp_center].enu_f, radius);
}
}
/*********** Navigation along an oval *************************************/
static inline void nav_oval_init(void)
{
if (nav.nav_oval_init) {
nav.nav_oval_init();
}
}
static inline void Oval(uint8_t wp1, uint8_t wp2, float radius)
{
if (nav.nav_oval) {
nav.nav_oval(&waypoints[wp1].enu_f, &waypoints[wp2].enu_f, radius);
}
}
/** Settings handlers
*/
#define navigation_IncreaseShift(x) { if (x==0) nav.shift = 0; else nav.shift += x; }
#define navigation_SetNavRadius(x) { if (x==1) nav.radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav.radius = -DEFAULT_CIRCLE_RADIUS; else nav.radius = x; }
/* follow another aircraft TODO */
#define NavFollow nav_follow(_id, _dist, _height) {}
/** Unused compat macros
*/
#define NavGlide(_start_wp, _wp) {}
#define NavVerticalAutoThrottleMode(_pitch) {}
#define NavVerticalAutoPitchMode(_throttle) {}
#define NavVerticalAltitudeMode(_alt, _pre_climb) {}
#define NavVerticalClimbMode(_climb) {}
#endif /* NAVIGATION_H */
sw/airborne/firmwares/rover/rover_datalink.c
+74
View File
@@ -0,0 +1,74 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file firmwares/rover/rover_datalink.c
* Handling of messages coming from ground and other A/Cs.
*
*/
#include "subsystems/datalink/datalink.h"
#include "pprzlink/dl_protocol.h"
#ifdef USE_NAVIGATION
#include "firmwares/rover/navigation.h"
#include "math/pprz_geodetic_int.h"
#endif
#include "autopilot.h"
void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
{
uint8_t msg_id = IdOfPprzMsg(buf);
/* parse telemetry messages coming from ground station */
switch (msg_id) {
#ifdef USE_NAVIGATION
case DL_BLOCK : {
if (DL_BLOCK_ac_id(buf) != AC_ID) { break; }
nav_goto_block(DL_BLOCK_block_id(buf));
}
break;
case DL_MOVE_WP : {
uint8_t ac_id = DL_MOVE_WP_ac_id(buf);
if (ac_id != AC_ID) { break; }
if (stateIsLocalCoordinateValid()) {
uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
struct LlaCoor_i lla;
lla.lat = DL_MOVE_WP_lat(buf);
lla.lon = DL_MOVE_WP_lon(buf);
/* WP_alt from message is alt above MSL in mm
* lla.alt is above ellipsoid in mm
*/
lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl +
state.ned_origin_i.lla.alt;
waypoint_move_lla(wp_id, &lla);
}
}
break;
#endif /* USE_NAVIGATION */
default:
break;
}
}
sw/airborne/firmwares/rover/rover_telemetry.c
+27
View File
@@ -0,0 +1,27 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/* PERIODIC_C_MAIN is defined before generated/periodic_telemetry.h
* in order to implement telemetry_mode_Main_*
*/
#define PERIODIC_C_MAIN
#include "generated/periodic_telemetry.h"
sw/airborne/modules/nav/nav_rover_base.c
+304
View File
@@ -0,0 +1,304 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/nav/nav_rover_base.c"
* @author 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
* Basic navigation functions for Rovers
*/
#include "modules/nav/nav_rover_base.h"
struct RoverNavBase nav_rover_base;
/** Implement basic nav function
*/
static void nav_goto(struct EnuCoor_f *wp)
{
nav_rover_base.goto_wp.to = *wp;
nav_rover_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp);
nav.mode = NAV_MODE_WAYPOINT;
}
static void nav_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
{
struct FloatVect2 wp_diff, pos_diff;
VECT2_DIFF(wp_diff, *wp_end, *wp_start);
VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *wp_start);
// leg length
float leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 0.1f);
nav_rover_base.goto_wp.leg_length = sqrtf(leg_length2);
// leg progress
nav_rover_base.goto_wp.leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_rover_base.goto_wp.leg_length;
nav_rover_base.goto_wp.leg_progress += Max(CARROT_DIST, 0.f);
// next pos on leg
struct FloatVect2 progress_pos;
VECT2_SMUL(progress_pos, wp_diff, nav_rover_base.goto_wp.leg_progress / nav_rover_base.goto_wp.leg_length);
VECT2_SUM(nav.target, *wp_start, progress_pos);
nav_rover_base.goto_wp.from = *wp_start;
nav_rover_base.goto_wp.to = *wp_end;
nav_rover_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp_end);
nav.mode = NAV_MODE_ROUTE;
}
static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
{
float dist_to_point;
struct FloatVect2 diff;
struct EnuCoor_f *pos = stateGetPositionEnu_f();
/* if an approaching_time is given, estimate diff after approching_time secs */
if (approaching_time > 0.f) {
struct FloatVect2 estimated_pos;
struct FloatVect2 estimated_progress;
struct EnuCoor_f *speed = stateGetSpeedEnu_f();
VECT2_SMUL(estimated_progress, *speed, approaching_time);
VECT2_SUM(estimated_pos, *pos, estimated_progress);
VECT2_DIFF(diff, *wp, estimated_pos);
}
/* else use current position */
else {
VECT2_DIFF(diff, *wp, *pos);
}
/* compute distance of estimated/current pos to target wp
*/
//nav.dist_to_point = float_vect2_norm(&diff); FIXME
dist_to_point = float_vect2_norm(&diff);
/* return TRUE if we have arrived */
if (dist_to_point < ARRIVED_AT_WAYPOINT) {
return true;
}
/* if coming from a valid waypoint */
if (from != NULL) {
/* return TRUE if normal line at the end of the segment is crossed */
struct FloatVect2 from_diff;
VECT2_DIFF(from_diff, *wp, *from);
return (diff.x * from_diff.x + diff.y * from_diff.y < 0.f);
}
return false;
}
static void nav_circle(struct EnuCoor_f *wp_center, float radius)
{
struct FloatVect2 pos_diff;
VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *wp_center);
// store last qdr
float last_qdr = nav_rover_base.circle.qdr;
// compute qdr
nav_rover_base.circle.qdr = atan2f(pos_diff.y, pos_diff.x);
// increment circle radians
float trigo_diff = nav_rover_base.circle.qdr - last_qdr;
NormRadAngle(trigo_diff);
nav_rover_base.circle.radians += trigo_diff;
// direction of rotation
float sign_radius = radius > 0.f ? 1.f : -1.f;
// absolute radius
float abs_radius = fabsf(radius);
if (abs_radius > 0.1f) {
// carrot_angle
float carrot_angle = CARROT_DIST / abs_radius;
carrot_angle = Min(carrot_angle, M_PI / 4);
carrot_angle = Max(carrot_angle, M_PI / 16);
carrot_angle = nav_rover_base.circle.qdr - sign_radius * carrot_angle;
// compute setpoint
VECT2_ASSIGN(pos_diff, abs_radius * cosf(carrot_angle), abs_radius * sinf(carrot_angle));
VECT2_SUM(nav.target, *wp_center, pos_diff);
}
else {
// radius is too small, direct to center
VECT2_COPY(nav.target, *wp_center);
}
nav_rover_base.circle.center = *wp_center;
nav_rover_base.circle.radius = radius;
nav.mode = NAV_MODE_CIRCLE;
}
/**
* Navigation along a figure O. One side leg is defined by waypoints [p1] and [p2].
* The navigation goes through 4 states: OC1 (half circle next to [p1]),
* OR21 (route [p2] to [p1], OC2 (half circle next to [p2]) and OR12 (opposite leg).
* Initial state is the route along the desired segment (OC2).
*/
#ifndef LINE_START_FUNCTION
#define LINE_START_FUNCTION {}
#endif
#ifndef LINE_STOP_FUNCTION
#define LINE_STOP_FUNCTION {}
#endif
static void _nav_oval_init(void)
{
nav_rover_base.oval.status = OC2;
nav_rover_base.oval.count = 0;
}
static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
{
(void) wp1;
(void) wp2;
(void) radius;
#if 0
radius = - radius; /* Historical error ? */
int32_t alt = waypoints[p1].enu_i.z;
waypoints[p2].enu_i.z = alt;
float p2_p1_x = waypoints[p1].enu_f.x - waypoints[p2].enu_f.x;
float p2_p1_y = waypoints[p1].enu_f.y - waypoints[p2].enu_f.y;
float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
/* Unit vector from p1 to p2 */
int32_t u_x = POS_BFP_OF_REAL(p2_p1_x / d);
int32_t u_y = POS_BFP_OF_REAL(p2_p1_y / d);
/* The half circle centers and the other leg */
struct EnuCoor_i p1_center = { waypoints[p1].enu_i.x + radius * -u_y,
waypoints[p1].enu_i.y + radius * u_x,
alt
};
struct EnuCoor_i p1_out = { waypoints[p1].enu_i.x + 2 * radius * -u_y,
waypoints[p1].enu_i.y + 2 * radius * u_x,
alt
};
struct EnuCoor_i p2_in = { waypoints[p2].enu_i.x + 2 * radius * -u_y,
waypoints[p2].enu_i.y + 2 * radius * u_x,
alt
};
struct EnuCoor_i p2_center = { waypoints[p2].enu_i.x + radius * -u_y,
waypoints[p2].enu_i.y + radius * u_x,
alt
};
int32_t qdr_out_2 = INT32_ANGLE_PI - int32_atan2_2(u_y, u_x);
int32_t qdr_out_1 = qdr_out_2 + INT32_ANGLE_PI;
if (radius < 0) {
qdr_out_2 += INT32_ANGLE_PI;
qdr_out_1 += INT32_ANGLE_PI;
}
int32_t qdr_anticipation = ANGLE_BFP_OF_REAL(radius > 0 ? -15 : 15);
switch (oval_status) {
case OC1 :
nav_circle(&p1_center, POS_BFP_OF_REAL(-radius));
if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_1) - qdr_anticipation)) {
oval_status = OR12;
InitStage();
LINE_START_FUNCTION;
}
return;
case OR12:
nav_route(&p1_out, &p2_in);
if (nav_approaching_from(&p2_in, &p1_out, CARROT)) {
oval_status = OC2;
nav_oval_count++;
InitStage();
LINE_STOP_FUNCTION;
}
return;
case OC2 :
nav_circle(&p2_center, POS_BFP_OF_REAL(-radius));
if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_2) - qdr_anticipation)) {
oval_status = OR21;
InitStage();
LINE_START_FUNCTION;
}
return;
case OR21:
nav_route(&waypoints[p2].enu_i, &waypoints[p1].enu_i);
if (nav_approaching_from(&waypoints[p1].enu_i, &waypoints[p2].enu_i, CARROT)) {
oval_status = OC1;
InitStage();
LINE_STOP_FUNCTION;
}
return;
default: /* Should not occur !!! Doing nothing */
return;
}
#endif
}
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_segment(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_SEGMENT(trans, dev, AC_ID,
&nav_rover_base.goto_wp.from.x,
&nav_rover_base.goto_wp.from.y,
&nav_rover_base.goto_wp.to.x,
&nav_rover_base.goto_wp.to.y);
}
static void send_circle(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_CIRCLE(trans, dev, AC_ID,
&nav_rover_base.circle.center.x,
&nav_rover_base.circle.center.y,
&nav_rover_base.circle.radius);
}
static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
{
float dist_home = sqrtf(nav.dist2_to_home);
float dist_wp = sqrtf(nav_rover_base.goto_wp.dist2_to_wp);
pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
&block_time, &stage_time,
&dist_home, &dist_wp,
&nav_block, &nav_stage,
&nav.mode);
if (nav.mode == NAV_MODE_ROUTE) {
send_segment(trans, dev);
} else if (nav.mode == NAV_MODE_CIRCLE) {
send_circle(trans, dev);
}
}
#endif
/** Init and register nav functions
*/
void nav_rover_init(void)
{
nav_rover_base.circle.radius = DEFAULT_CIRCLE_RADIUS;
nav_rover_base.goto_wp.leg_progress = 0.f;
nav_rover_base.goto_wp.leg_length = 1.f;
// register nav functions
nav_register_goto_wp(nav_goto, nav_route, nav_approaching);
nav_register_circle(nav_circle);
nav_register_oval(_nav_oval_init, nav_oval);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
#endif
}
sw/airborne/modules/nav/nav_rover_base.h
+87
View File
@@ -0,0 +1,87 @@
/*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/nav/nav_rover_base.h"
* @author 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
* Basic navigation functions for Rovers
*/
#ifndef NAV_ROVER_BASE_H
#define NAV_ROVER_BASE_H
#include "firmwares/rover/navigation.h"
/** Waypoint and route pattern
*/
struct RoverNavGoto {
struct EnuCoor_f from; ///< start WP position
struct EnuCoor_f to; ///< end WP position
float dist2_to_wp; ///< squared distance to next waypoint
float leg_progress; ///< progress over leg
float leg_length; ///< leg length
};
/** Circle pattern
*/
struct RoverNavCircle {
struct EnuCoor_f center; ///< center WP position
float radius; ///< radius in meters
float qdr; ///< qdr in radians
float radians; ///< incremental angular distance
};
/** Oval pattern
*/
enum oval_status { OR12, OC2, OR21, OC1 };
struct RoverNavOval {
enum oval_status status; ///< oval status
uint8_t count; ///< number of laps
};
/** Basic Nav struct
*/
struct RoverNavBase {
struct RoverNavGoto goto_wp;
struct RoverNavCircle circle;
struct RoverNavOval oval;
};
extern struct RoverNavBase nav_rover_base;
extern void nav_rover_init(void);
/** Macros for circle nav
*/
#define NavCircleCount() (fabsf(nav_rover_base.circle.radians) / (2*M_PI))
#define NavCircleQdr() ({ float qdr = DegOfRad(M_PI_2 - nav_rover_base.circle.qdr); NormCourse(qdr); qdr; })
#define CloseDegAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff || _diff < 10; })
#define CloseRadAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormRadAngle(_diff); fabsf(_diff) < 0.0177; })
/** True if x (in degrees) is close to the current QDR (less than 10 degrees)
*/
#define NavQdrCloseTo(x) CloseDegAngles(x, NavCircleQdr())
#define NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(stateGetHorizontalSpeedDir_f()))
#endif
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: af581eec19...07bc6cfc72