mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-25 14:35:51 +08:00
Remove the dependencies of 'nav_hybrid' from 'guidance_indi_hybrid'. (#3160)
* remove nav_hybrid dependency from guidance_indi_hybrid * removed NAV_MAX_SPEED definition from the testing airframe * removed dependency from max bank angle of guidance --------- Co-authored-by: Christophe De Wagter <dewagter@gmail.com>
This commit is contained in:
Alessandro Mancinelli
committed by
GitHub
GitHub
parent
0ec27e76cf
commit
5aff1f7894
@@ -0,0 +1,246 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
<airframe name="hybrid_nav_test">
|
||||
<description>
|
||||
Test for using the hybrid_nav module without dependecies from the guidance_indi module.
|
||||
</description>
|
||||
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
|
||||
<target name="ap" board="px4fmu_5.0_chibios">
|
||||
<configure name="FLASH_MODE" value="SWD"/>
|
||||
<configure name="PERIODIC_FREQUENCY" value="500"/>
|
||||
|
||||
<module name="imu" type="mpu6000"/>
|
||||
|
||||
<module name="mag" type="rm3100">
|
||||
<configure name="MAG_RM3100_I2C_DEV" value="i2c2"/>
|
||||
<define name="RM3100_CHAN_X_SIGN" value="+"/>
|
||||
<define name="RM3100_CHAN_Y_SIGN" value="-"/>
|
||||
<define name="RM3100_CHAN_Z_SIGN" value="-"/>
|
||||
<define name="RM3100_CHAN_X" value="0"/>
|
||||
<define name="RM3100_CHAN_Y" value="1"/>
|
||||
<define name="RM3100_CHAN_Z" value="2"/>
|
||||
<define name="MODULE_RM3100_SYNC_SEND" value="FALSE"/>
|
||||
<define name="MODULE_RM3100_UPDATE_AHRS" value="TRUE"/>
|
||||
</module>
|
||||
|
||||
<module name="actuators" type="pwm">
|
||||
</module>
|
||||
|
||||
<module name="gps" type="ublox">
|
||||
<configure name="GPS_BAUD" value="B460800"/>
|
||||
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
|
||||
</module>
|
||||
|
||||
<module name="radio_control" type="sbus">
|
||||
<configure name="SBUS_PORT" value="UART3"/>
|
||||
</module>
|
||||
|
||||
<module name="telemetry" type="transparent">
|
||||
<configure name="MODEM_BAUD" value="B460800"/>
|
||||
</module>
|
||||
|
||||
<!-- Enable for onboard logging -->
|
||||
<module name="tlsf"/>
|
||||
<module name="pprzlog"/>
|
||||
<module name="logger" type="sd_chibios"/>
|
||||
<module name="flight_recorder"/>
|
||||
|
||||
</target>
|
||||
|
||||
<define name="USE_AIRSPEED" value="TRUE"/>
|
||||
|
||||
<!-- <module name="guidance" type="indi_hybrid">
|
||||
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
|
||||
<define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
|
||||
<define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>
|
||||
<define name="GUIDANCE_INDI_POS_GAINZ" value="0.3"/>
|
||||
<define name="GUIDANCE_INDI_SPEED_GAINZ" value="0.5"/>
|
||||
<define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
|
||||
<define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
|
||||
<define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/>
|
||||
<define name="GUIDANCE_INDI_MAX_AIRSPEED" value="17.0"/>
|
||||
<define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/>
|
||||
<define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="5"/>
|
||||
<define name="GUIDANCE_INDI_LINE_GAIN" value="0.2"/>
|
||||
<define name="GUIDANCE_INDI_MIN_THROTTLE" value="1500"/>
|
||||
<define name="GUIDANCE_INDI_MIN_THROTTLE_FWD" value="1500"/>
|
||||
<define name="GUIDANCE_INDI_LIFTD_P50" value="6.0"/>
|
||||
<define name="GUIDANCE_INDI_LIFTD_P80" value="10.0"/>
|
||||
<define name="GUIDANCE_INDI_LIFTD_ASQ" value="0.15"/>
|
||||
<define name="TRANSITION_MAX_OFFSET" value="10"/>
|
||||
</module> -->
|
||||
|
||||
<module name="nav_hybrid">
|
||||
<configure name="NAV_HYBRID_POS_GAIN" value="1"/>
|
||||
</module>
|
||||
|
||||
<module name="stabilization" type="indi_simple"/>
|
||||
|
||||
<module name="ins" type="ekf2">
|
||||
</module>
|
||||
|
||||
<module name="geo_mag"/>
|
||||
<module name="air_data"/>
|
||||
|
||||
</firmware>
|
||||
|
||||
<servos>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="YAW" failsafe_value="0"/>
|
||||
<axis name="THRUST" failsafe_value="0"/>
|
||||
<axis name="RADIO_TH_HOLD" failsafe_value="-9600"/>
|
||||
</commands>
|
||||
|
||||
<rc_commands>
|
||||
<set command="THRUST" value="@THROTTLE"/>
|
||||
<set command="ROLL" value="@YAW"/>
|
||||
<set command="PITCH" value="@PITCH/2"/>
|
||||
<set command="YAW" value="-@ROLL/4"/>
|
||||
</rc_commands>
|
||||
|
||||
|
||||
<command_laws>
|
||||
</command_laws>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
|
||||
|
||||
<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true},.neutral={-9,-6,-4}, .scale={{38883,41480,36505},{3968,4241,3726}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-33,-9,11}, .scale={{36893,53565,3365},{7544,10964,689}}}}"/>
|
||||
|
||||
<!-- Calibrated at Valkenburg the 20-07-2023 -->
|
||||
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={9,-131,25}, .scale={{20269,20901,3230},{35294,37646,5847}}}}"/>
|
||||
|
||||
</section>
|
||||
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
|
||||
<!-- Delft -->
|
||||
<define name="H_X" value="0.3892503"/>
|
||||
<define name="H_Y" value="0.0017972"/>
|
||||
<define name="H_Z" value="0.9211303"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<define name="SP_MAX_PHI" value="40." unit="deg"/>
|
||||
<define name="SP_MAX_THETA" value="40." unit="deg"/>
|
||||
<define name="SP_MAX_R" value="150." unit="deg/s"/>
|
||||
<define name="DEADBAND_R" value="200"/>
|
||||
<define name="SP_PSI_DELTA_LIMIT" value="45" unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
|
||||
<!-- control effectiveness (hover) [conventional + yaw tilting] -->
|
||||
<define name="G1_P" value="0.004"/>
|
||||
<define name="G1_Q" value="0.01"/>
|
||||
<define name="G1_R" value="0.001"/>
|
||||
<define name="G2_R" value="0.00015"/>
|
||||
|
||||
<!-- control effectiveness (forward) -->
|
||||
<define name="FORWARD_G1_P" value="0.0020"/>
|
||||
<define name="FORWARD_G1_Q" value="0.0077"/>
|
||||
<define name="FORWARD_G1_R" value="0.004"/>
|
||||
<!-- reference acceleration for attitude control -->
|
||||
<define name="REF_ERR_P" value="60.0"/>
|
||||
<define name="REF_ERR_Q" value="60.0"/>
|
||||
<define name="REF_ERR_R" value="44.0"/>
|
||||
<define name="REF_RATE_P" value="6.5"/>
|
||||
<define name="REF_RATE_Q" value="6.5"/>
|
||||
<define name="REF_RATE_R" value="5.4"/>
|
||||
|
||||
<!--Maxium yaw rate, to avoid instability-->
|
||||
<define name="MAX_R" value="120.0" unit="deg/s"/>
|
||||
|
||||
<!-- Maximum rate setpoint in rate control mode -->
|
||||
<define name="MAX_RATE" value="3.0" unit="rad/s"/>
|
||||
|
||||
<!-- second order filter parameters -->
|
||||
<define name="FILT_CUTOFF" value="1.5"/>
|
||||
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
|
||||
<define name="ESTIMATION_FILT_CUTOFF" value="5.0"/>
|
||||
<define name="FILT_CUTOFF_R" value="4.0"/>
|
||||
|
||||
<!-- first order actuator dynamics -->
|
||||
<define name="ACT_DYN_P" value="0.047"/>
|
||||
<define name="ACT_DYN_Q" value="0.047"/>
|
||||
<define name="ACT_DYN_R" value="0.047"/>
|
||||
<define name="ACT_DYN_B" value="0.047"/>
|
||||
<define name="ACT_DYN_P" value="0.047"/>
|
||||
|
||||
<!-- Adaptive Learning Rate -->
|
||||
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="310"/>
|
||||
<define name="HOVER_KD" value="130"/>
|
||||
<define name="HOVER_KI" value="10"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="MAX_BANK" value="30" unit="deg"/>
|
||||
<define name="USE_SPEED_REF" value="TRUE"/>
|
||||
<define name="PGAIN" value="60"/>
|
||||
<define name="DGAIN" value="100"/>
|
||||
<define name="AGAIN" value="0"/>
|
||||
<define name="IGAIN" value="20"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_MANUAL" value="AP_MODE_RC_DIRECT"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_HOVER_DIRECT"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
||||
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 0.76* adc)"/>
|
||||
<define name="BAT_NB_CELLS" value="4"/>
|
||||
<define name="MAX_BAT_LEVEL" value="16.8" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="14.5" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="14.0" unit="V"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="12.0" unit="V"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
|
||||
<define name="NAV_CLIMB_VSPEED" value="3.5"/>
|
||||
<define name="NAV_DESCEND_VSPEED" value="-0.5"/>
|
||||
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
|
||||
|
||||
|
||||
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
|
||||
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
|
||||
|
||||
<define name="UNLOCKED_HOME_MODE" value="TRUE"/>
|
||||
<define name="RC_LOST_MODE" value="AP_MODE_NAV"/>
|
||||
|
||||
<define name="FAILSAFE_HOME_RADIUS" value="600" unit="m"/>
|
||||
|
||||
</section>
|
||||
|
||||
<section name="AIR_DATA" prefix="AIR_DATA_">
|
||||
<define name="CALC_AIRSPEED" value="TRUE"/>
|
||||
<define name="CALC_TAS_FACTOR" value="FALSE"/>
|
||||
<define name="CALC_AMSL_BARO" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="GCS">
|
||||
<define name="SPEECH_NAME" value="OverDone"/>
|
||||
<define name="AC_ICON" value="flyingwing"/>
|
||||
<define name="ALT_SHIFT_PLUS_PLUS" value="2"/>
|
||||
<define name="ALT_SHIFT_PLUS" value="1"/>
|
||||
<define name="ALT_SHIFT_MINUS" value="-1"/>
|
||||
</section>
|
||||
|
||||
</airframe>
|
||||
@@ -20,13 +20,15 @@
|
||||
<dl_settings>
|
||||
<dl_settings NAME="nav_hybrid">
|
||||
<dl_setting var="nav_max_speed" min="1.0" step="1.0" max="50.0"/>
|
||||
<dl_setting var="nav_goto_speed" min="0.1" step="0.1" max="10.0"/>
|
||||
<dl_setting var="nav_goto_max_speed" min="0.1" step="0.1" max="50.0"/>
|
||||
<dl_setting var="nav_max_deceleration_sp" min="0.5" step="0.1" max="10.0" shortname="max_deceleration" param="NAV_HYBRID_MAX_DECELERATION"/>
|
||||
<dl_setting var="nav_hybrid_line_gain" min="0.1" step="0.1" max="3" shortname="nav_hybrid_line_gain" param="NAV_HYBRID_LINE_GAIN"/>
|
||||
<dl_setting var="nav_hybrid_pos_gain" min="0.1" step="0.1" max="10" shortname="nav_hybrid_pos_gain" param="NAV_HYBRID_POS_GAIN"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<dep>
|
||||
<depends>nav_basic_rotorcraft,guidance_indi_hybrid</depends>
|
||||
<depends>nav_basic_rotorcraft</depends>
|
||||
</dep>
|
||||
<header>
|
||||
<file name="nav_rotorcraft_hybrid.h"/>
|
||||
@@ -35,7 +37,6 @@
|
||||
<makefile target="ap|nps" firmware="rotorcraft">
|
||||
<file name="nav_rotorcraft_hybrid.c"/>
|
||||
<test firmware="rotorcraft">
|
||||
<define name="GUIDANCE_INDI_MAX_AIRSPEED" value="15."/>
|
||||
<configure name="SRC_FIRMWARE" value="firmwares/rotorcraft"/>
|
||||
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
|
||||
<define name="DOWNLINK_DEVICE" value="uart0"/>
|
||||
|
||||
@@ -593,4 +593,15 @@
|
||||
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/ctrl_eff_sched_rot_wing.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml"
|
||||
gui_color="red"
|
||||
/>
|
||||
<aircraft
|
||||
name="nav_hybrid_test"
|
||||
ac_id="23"
|
||||
airframe="airframes/tudelft/hybrid_nav_test.xml"
|
||||
radio="radios/crossfire_sbus.xml"
|
||||
telemetry="telemetry/default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/tudelft/delft_basic.xml"
|
||||
settings=""
|
||||
settings_modules="modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
|
||||
gui_color="red"
|
||||
/>
|
||||
</conf>
|
||||
|
||||
@@ -25,22 +25,31 @@
|
||||
|
||||
#include "modules/nav/nav_rotorcraft_hybrid.h"
|
||||
#include "firmwares/rotorcraft/navigation.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h" // strong dependency for now
|
||||
#include "math/pprz_isa.h"
|
||||
#include "generated/flight_plan.h"
|
||||
|
||||
// if NAV_HYBRID_MAX_BANK is not defined, set it to 30 degrees.
|
||||
#ifndef NAV_HYBRID_MAX_BANK
|
||||
#define NAV_HYBRID_MAX_BANK 0.52f
|
||||
#endif
|
||||
|
||||
// Max ground speed that will be commanded
|
||||
#ifndef GUIDANCE_INDI_NAV_SPEED_MARGIN
|
||||
#define GUIDANCE_INDI_NAV_SPEED_MARGIN 10.0f
|
||||
#ifndef NAV_HYBRID_MAX_AIRSPEED
|
||||
#define NAV_HYBRID_MAX_AIRSPEED 15.0f
|
||||
#endif
|
||||
#define NAV_MAX_SPEED (GUIDANCE_INDI_MAX_AIRSPEED + GUIDANCE_INDI_NAV_SPEED_MARGIN)
|
||||
|
||||
#ifndef NAV_HYBRID_SPEED_MARGIN
|
||||
#define NAV_HYBRID_SPEED_MARGIN 10.0f
|
||||
#endif
|
||||
|
||||
#define NAV_MAX_SPEED (NAV_HYBRID_MAX_AIRSPEED + NAV_HYBRID_SPEED_MARGIN)
|
||||
float nav_max_speed = NAV_MAX_SPEED;
|
||||
|
||||
// Max ground speed in with goto/stay instruction
|
||||
// by default, same as route speed
|
||||
#ifndef GUIDANCE_INDI_GOTO_SPEED
|
||||
#define GUIDANCE_INDI_GOTO_SPEED NAV_MAX_SPEED
|
||||
#ifndef NAV_HYBRID_GOTO_MAX_SPEED
|
||||
#define NAV_HYBRID_GOTO_MAX_SPEED NAV_MAX_SPEED
|
||||
#endif
|
||||
float nav_goto_speed = GUIDANCE_INDI_GOTO_SPEED;
|
||||
|
||||
float nav_goto_max_speed = NAV_HYBRID_GOTO_MAX_SPEED;
|
||||
|
||||
#ifndef NAV_HYBRID_MAX_DECELERATION
|
||||
#define NAV_HYBRID_MAX_DECELERATION 1.0
|
||||
@@ -48,18 +57,30 @@ float nav_goto_speed = GUIDANCE_INDI_GOTO_SPEED;
|
||||
|
||||
float nav_max_deceleration_sp = NAV_HYBRID_MAX_DECELERATION;
|
||||
|
||||
#ifdef GUIDANCE_INDI_LINE_GAIN
|
||||
static float guidance_indi_line_gain = GUIDANCE_INDI_LINE_GAIN;
|
||||
#ifdef NAV_HYBRID_LINE_GAIN
|
||||
float nav_hybrid_line_gain = NAV_HYBRID_LINE_GAIN;
|
||||
#else
|
||||
static float guidance_indi_line_gain = 1.0f;
|
||||
float nav_hybrid_line_gain = 1.0f;
|
||||
#endif
|
||||
|
||||
#ifndef GUIDANCE_INDI_NAV_LINE_DIST
|
||||
#define GUIDANCE_INDI_NAV_LINE_DIST 50.f
|
||||
#ifndef NAV_HYBRID_NAV_LINE_DIST
|
||||
#define NAV_HYBRID_NAV_LINE_DIST 50.f
|
||||
#endif
|
||||
|
||||
#ifndef GUIDANCE_INDI_NAV_CIRCLE_DIST
|
||||
#define GUIDANCE_INDI_NAV_CIRCLE_DIST 40.f
|
||||
#ifndef NAV_HYBRID_NAV_CIRCLE_DIST
|
||||
#define NAV_HYBRID_NAV_CIRCLE_DIST 40.f
|
||||
#endif
|
||||
|
||||
#ifdef NAV_HYBRID_POS_GAIN
|
||||
float nav_hybrid_pos_gain = NAV_HYBRID_POS_GAIN;
|
||||
#else
|
||||
float nav_hybrid_pos_gain = 1.0;
|
||||
#endif
|
||||
|
||||
#ifndef USE_NPS
|
||||
#ifndef GUIDANCE_INDI_HYBRID
|
||||
bool force_forward = 0;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
/** Implement basic nav function for the hybrid case
|
||||
@@ -77,12 +98,12 @@ static void nav_hybrid_goto(struct EnuCoor_f *wp)
|
||||
VECT2_DIFF(pos_error, nav.target, *pos);
|
||||
|
||||
struct FloatVect2 speed_sp;
|
||||
VECT2_SMUL(speed_sp, pos_error, gih_params.pos_gain);
|
||||
VECT2_SMUL(speed_sp, pos_error, nav_hybrid_pos_gain);
|
||||
|
||||
// Bound the setpoint velocity vector
|
||||
float max_h_speed = nav_max_speed;
|
||||
if (!force_forward) {
|
||||
// If not in force_forward, compute speed based on decceleration and nav_goto_speed
|
||||
// If not in force_forward, compute speed based on decceleration and nav_goto_max_speed
|
||||
// Calculate distance to waypoint
|
||||
float dist_to_wp = float_vect2_norm(&pos_error);
|
||||
// Calculate max speed when decelerating at MAX capacity a_max
|
||||
@@ -93,7 +114,7 @@ static void nav_hybrid_goto(struct EnuCoor_f *wp)
|
||||
float max_speed_decel2 = fabsf(2.f * dist_to_wp * nav_max_deceleration_sp); // dist_to_wp can only be positive, but just in case
|
||||
float max_speed_decel = sqrtf(max_speed_decel2);
|
||||
// Bound the setpoint velocity vector
|
||||
max_h_speed = Min(nav_goto_speed, max_speed_decel); // use hover max speed
|
||||
max_h_speed = Min(nav_goto_max_speed, max_speed_decel); // use hover max speed
|
||||
}
|
||||
float_vect2_bound_in_2d(&speed_sp, max_h_speed);
|
||||
|
||||
@@ -114,7 +135,7 @@ static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_en
|
||||
if (force_forward) {
|
||||
desired_speed = nav_max_speed;
|
||||
} else {
|
||||
desired_speed = dist_to_target * gih_params.pos_gain;
|
||||
desired_speed = dist_to_target * nav_hybrid_pos_gain;
|
||||
Bound(desired_speed, 0.0f, nav_max_speed);
|
||||
}
|
||||
|
||||
@@ -129,11 +150,11 @@ static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_en
|
||||
float dist_to_line = (pos_diff.x * normalv.x + pos_diff.y * normalv.y) / length_normalv;
|
||||
// Normal vector scaled to be the distance to the line
|
||||
struct FloatVect2 v_to_line, v_along_line;
|
||||
v_to_line.x = dist_to_line * normalv.x / length_normalv * guidance_indi_line_gain;
|
||||
v_to_line.y = dist_to_line * normalv.y / length_normalv * guidance_indi_line_gain;
|
||||
v_to_line.x = dist_to_line * normalv.x / length_normalv * nav_hybrid_line_gain;
|
||||
v_to_line.y = dist_to_line * normalv.y / length_normalv * nav_hybrid_line_gain;
|
||||
// The distance that needs to be traveled along the line
|
||||
v_along_line.x = wp_diff.x / length_line * GUIDANCE_INDI_NAV_LINE_DIST;
|
||||
v_along_line.y = wp_diff.y / length_line * GUIDANCE_INDI_NAV_LINE_DIST;
|
||||
v_along_line.x = wp_diff.x / length_line * NAV_HYBRID_NAV_LINE_DIST;
|
||||
v_along_line.y = wp_diff.y / length_line * NAV_HYBRID_NAV_LINE_DIST;
|
||||
// Calculate the desired direction to converge to the line
|
||||
struct FloatVect2 direction;
|
||||
VECT2_SMUL(direction, v_along_line, (1.f / (1.f + fabsf(dist_to_line) * 0.05f)));
|
||||
@@ -211,7 +232,7 @@ static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
|
||||
NormRadAngle(trigo_diff);
|
||||
nav_rotorcraft_base.circle.radians += trigo_diff;
|
||||
// progress angle
|
||||
float progress_angle = GUIDANCE_INDI_NAV_CIRCLE_DIST / abs_radius;
|
||||
float progress_angle = NAV_HYBRID_NAV_CIRCLE_DIST / abs_radius;
|
||||
Bound(progress_angle, M_PI / 16.f, M_PI / 4.f);
|
||||
float alpha = nav_rotorcraft_base.circle.qdr - sign_radius * progress_angle;
|
||||
// final target position, should be on the circle, for display
|
||||
@@ -227,13 +248,13 @@ static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
|
||||
desired_speed = nav_max_speed;
|
||||
} else {
|
||||
float radius_diff = fabsf(float_vect2_norm(&pos_diff) - abs_radius);
|
||||
if (radius_diff > GUIDANCE_INDI_NAV_CIRCLE_DIST) {
|
||||
if (radius_diff > NAV_HYBRID_NAV_CIRCLE_DIST) {
|
||||
// far from circle, speed proportional to diff
|
||||
desired_speed = radius_diff * gih_params.pos_gain;
|
||||
desired_speed = radius_diff * nav_hybrid_pos_gain;
|
||||
} else {
|
||||
// close to circle, speed function of radius for a feasible turn
|
||||
// 0.8 * MAX_BANK gives some margins for the turns
|
||||
desired_speed = sqrtf(PPRZ_ISA_GRAVITY * abs_radius * tanf(0.8f * GUIDANCE_H_MAX_BANK));
|
||||
desired_speed = sqrtf(PPRZ_ISA_GRAVITY * abs_radius * tanf(0.8f * NAV_HYBRID_MAX_BANK));
|
||||
}
|
||||
Bound(desired_speed, 0.0f, nav_max_speed);
|
||||
}
|
||||
|
||||
@@ -33,7 +33,16 @@
|
||||
|
||||
// settings
|
||||
extern float nav_max_speed; // max speed in route mode
|
||||
extern float nav_goto_speed; // max speed in goto/stay mode
|
||||
extern float nav_goto_max_speed; // max speed in goto/stay mode
|
||||
extern float nav_max_deceleration_sp;
|
||||
extern float nav_hybrid_line_gain;
|
||||
extern float nav_hybrid_pos_gain;
|
||||
|
||||
#ifndef GUIDANCE_INDI_HYBRID
|
||||
extern bool force_forward;
|
||||
#endif
|
||||
|
||||
|
||||
extern float nav_max_deceleration_sp;
|
||||
|
||||
extern void nav_rotorcraft_hybrid_init(void);
|
||||
|
||||
Reference in New Issue
Block a user