Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 04:46:51 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Fix nav rework (#3004)

Browse Source 
* fix guidance_v in flight plan and remove old settings

* nav radius, descend and climb

* [stab] make a attitute set quat function

* fix function name

* fix parsing of variables in proc, don't use = in fp

* fix nav heading in modules

* fix some more errors

* fix var name
This commit is contained in:
Gautier Hattenberger
2023-03-06 10:45:44 +01:00
committed by GitHub
parent 28ef30ed80
commit b152b927b3
37 changed files with 106 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/BR/conf.xml
-1
View File
@@ -75,7 +75,6 @@
settings="settings/fixedwing_basic.xml" settings="settings/fixedwing_basic.xml"
settings_modules="modules/ahrs_float_dcm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_full_pid_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/stabilization_adaptive_fw.xml" settings_modules="modules/ahrs_float_dcm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_full_pid_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/stabilization_adaptive_fw.xml"
gui_color="blue" gui_color="blue"
release="3c20b30a61bf15a55fdf6d909c82615364e75751"
/> />
<aircraft <aircraft
name="DreamCatcher" name="DreamCatcher"
conf/airframes/CDW/cdw_conf.xml
-1
View File
@@ -9,6 +9,5 @@
settings="settings/rotorcraft_basic.xml settings/modules/config_asctec_v2.xml" settings="settings/rotorcraft_basic.xml settings/modules/config_asctec_v2.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_quality_assessment.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_euler.xml" settings_modules="modules/ahrs_int_cmpl_quat.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_quality_assessment.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_euler.xml"
gui_color="white" gui_color="white"
release="33e02c9f1e831cb30ba9dc442e055b6e28c72a52"
/> />
</conf> </conf>
conf/flight_plans/ENAC/crazyflie_multi_ranger_test.xml
+2 -2
View File
@@ -106,8 +106,8 @@ static inline void obstacle_detection_cb(uint8_t sender_id __attribute__((unused
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/> <stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block> </block>
<block name="Ramp down"> <block name="Ramp down">
<exception cond="guidance_v_delta_t @LT 0.1*9600." deroute="Kill Engine"/> <exception cond="guidance_v.delta_t @LT 0.1*9600." deroute="Kill Engine"/>
<set var="fp_throttle" value="guidance_v_delta_t/9600."/> <set var="fp_throttle" value="guidance_v.delta_t/9600."/>
<stay throttle="fp_throttle-0.1*stage_time" vmode="throttle" wp="TD"/> <stay throttle="fp_throttle-0.1*stage_time" vmode="throttle" wp="TD"/>
</block> </block>
<block name="Kill Engine"> <block name="Kill Engine">
conf/flight_plans/ENAC/demo_aruco_drop_indoor.xml
+2 -2
View File
@@ -170,8 +170,8 @@
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/> <stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block> </block>
<block name="Ramp down"> <block name="Ramp down">
<exception cond="guidance_v_delta_t @LT 0.05*9600." deroute="Kill landed"/> <exception cond="guidance_v.delta_t @LT 0.05*9600." deroute="Kill landed"/>
<set var="fp_throttle" value="guidance_v_delta_t/9600."/> <set var="fp_throttle" value="guidance_v.delta_t/9600."/>
<stay throttle="fp_throttle-0.02*stage_time" vmode="throttle" wp="TAG"/> <stay throttle="fp_throttle-0.02*stage_time" vmode="throttle" wp="TAG"/>
</block> </block>
<block name="Kill landed"> <block name="Kill landed">
conf/flight_plans/ENAC/fish_outdoor.xml
+2 -2
View File
@@ -99,8 +99,8 @@
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/> <stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block> </block>
<block name="Ramp down"> <block name="Ramp down">
<exception cond="guidance_v_delta_t @LT 0.1*9600." deroute="Landed"/> <exception cond="guidance_v.delta_t @LT 0.1*9600." deroute="Landed"/>
<set value="guidance_v_delta_t/9600." var="fp_throttle"/> <set value="guidance_v.delta_t/9600." var="fp_throttle"/>
<stay throttle="fp_throttle-0.1*stage_time" vmode="throttle" wp="TD"/> <stay throttle="fp_throttle-0.1*stage_time" vmode="throttle" wp="TD"/>
</block> </block>
<block name="Landed"> <block name="Landed">
conf/flight_plans/ENAC/fish_voliere.xml
+2 -2
View File
@@ -83,8 +83,8 @@
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/> <stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block> </block>
<block name="Ramp down"> <block name="Ramp down">
<exception cond="guidance_v_delta_t @LT 0.1*9600." deroute="Landed"/> <exception cond="guidance_v.delta_t @LT 0.1*9600." deroute="Landed"/>
<set var="fp_throttle" value="guidance_v_delta_t/9600."/> <set var="fp_throttle" value="guidance_v.delta_t/9600."/>
<stay throttle="fp_throttle-0.1*stage_time" vmode="throttle" wp="TD"/> <stay throttle="fp_throttle-0.1*stage_time" vmode="throttle" wp="TD"/>
</block> </block>
<block name="Landed"> <block name="Landed">
conf/flight_plans/HOOPERFLY/hooperfly_gsa_one.xml
+1 -1
View File
@@ -110,7 +110,7 @@
<deroute block="stay_p1"/> <deroute block="stay_p1"/>
</block> </block>
<block name="circle CAM" pre_call="nav_set_heading_towards_waypoint(WP_CAM)"> <block name="circle CAM" pre_call="nav_set_heading_towards_waypoint(WP_CAM)">
<circle radius="nav_radius" wp="CAM"/> <circle radius="nav.radius" wp="CAM"/>
</block> </block>
<block group="land_pattern" name="land here" strip_button="Land Here" strip_icon="land_here.png"> <block group="land_pattern" name="land here" strip_button="Land Here" strip_icon="land_here.png">
<call_once fun="NavSetWaypointHere(WP_TD)"/> <call_once fun="NavSetWaypointHere(WP_TD)"/>
conf/flight_plans/HOOPERFLY/hooperfly_rotorcraft_multiflight.xml
+1 -1
View File
@@ -165,7 +165,7 @@
<call fun="nav_survey_rectangle_rotorcraft_run(WP_S1, WP_S2)"/> <call fun="nav_survey_rectangle_rotorcraft_run(WP_S1, WP_S2)"/>
</block> </block>
<block name="circle CAM" pre_call="nav_set_heading_towards_waypoint(WP_CAM)"> <block name="circle CAM" pre_call="nav_set_heading_towards_waypoint(WP_CAM)">
<circle radius="nav_radius" wp="CAM"/> <circle radius="nav.radius" wp="CAM"/>
</block> </block>
<block group="land_pattern" name="land here" strip_button="Land Here" strip_icon="land_here.png"> <block group="land_pattern" name="land here" strip_button="Land Here" strip_icon="land_here.png">
<call_once fun="NavSetWaypointHere(WP_TD)"/> <call_once fun="NavSetWaypointHere(WP_TD)"/>
conf/flight_plans/rotorcraft_basic_safety.xml
+1 -1
View File
@@ -87,7 +87,7 @@
<deroute block="stay_p1"/> <deroute block="stay_p1"/>
</block> </block>
<block name="circle"> <block name="circle">
<circle radius="nav_radius" wp="p1"/> <circle radius="nav.radius" wp="p1"/>
</block> </block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png"> <block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<call_once fun="NavSetWaypointHere(WP_TD)"/> <call_once fun="NavSetWaypointHere(WP_TD)"/>
conf/flight_plans/tudelft/include_rotorcraft_survey_mission.xml
+3 -2
View File
@@ -22,6 +22,7 @@
#define StopVision() FALSE #define StopVision() FALSE
#endif #endif
#define WpAlt(X) (30) #define WpAlt(X) (30)
#define SetValue(_a, _b) { _a = _b; }
</header> </header>
<variables> <variables>
<variable var="exception_flag_0" type="bool" init="false"/> <variable var="exception_flag_0" type="bool" init="false"/>
@@ -29,8 +30,8 @@
</variables> </variables>
<exceptions> <exceptions>
<exception cond="electrical.bat_low @AND (exception_flag_0 == false)" deroute="ComeBackAndLand" exec="exception_flag_0 = true"/> <exception cond="electrical.bat_low @AND (exception_flag_0 == false)" deroute="ComeBackAndLand" exec="SetValue(exception_flag_0, true)"/>
<exception cond="electrical.bat_critical @AND (exception_flag_1 == false)" deroute="land_here" exec="exception_flag_1 = true"/> <exception cond="electrical.bat_critical @AND (exception_flag_1 == false)" deroute="land_here" exec="SetValue(exception_flag_1, true)"/>
<exception cond="(!InsideRED(GetPosX(), GetPosY()) @AND !(nav_block == IndexOfBlock('flare')) @AND !(nav_block == IndexOfBlock('landed')) @AND !(nav_block == IndexOfBlock('WaitGPS')) @AND !(nav_block == IndexOfBlock('GeoInit')))" deroute="land_emergency"/> <exception cond="(!InsideRED(GetPosX(), GetPosY()) @AND !(nav_block == IndexOfBlock('flare')) @AND !(nav_block == IndexOfBlock('landed')) @AND !(nav_block == IndexOfBlock('WaitGPS')) @AND !(nav_block == IndexOfBlock('GeoInit')))" deroute="land_emergency"/>
</exceptions> </exceptions>
conf/flight_plans/tudelft/nederdrone_terschelling.xml
+3 -3
View File
@@ -77,7 +77,7 @@
<exception cond="GetPosHeight() @GT 40.0" deroute="Standby"/> <exception cond="GetPosHeight() @GT 40.0" deroute="Standby"/>
<call_once fun="nav_set_heading_current()"/> <call_once fun="nav_set_heading_current()"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/> <call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/> <stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block> </block>
<block name="Standby" strip_button="Standby" strip_icon="home.png"> <block name="Standby" strip_button="Standby" strip_icon="home.png">
<set value="FALSE" var="force_forward"/> <set value="FALSE" var="force_forward"/>
@@ -129,7 +129,7 @@
</block> </block>
<block name="Oval"> <block name="Oval">
<set value="TRUE" var="force_forward"/> <set value="TRUE" var="force_forward"/>
<oval p1="p1" p2="p2" radius="nav_radius"/> <oval p1="p1" p2="p2" radius="nav.radius"/>
</block> </block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png"> <block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<set value="FALSE" var="force_forward"/> <set value="FALSE" var="force_forward"/>
@@ -142,7 +142,7 @@
<block name="descend"> <block name="descend">
<set value="FALSE" var="force_forward"/> <set value="FALSE" var="force_forward"/>
<exception cond="LessThan(GetPosHeight(), 15.0)" deroute="flare"/> <exception cond="LessThan(GetPosHeight(), 15.0)" deroute="flare"/>
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/> <stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block> </block>
<block name="flare"> <block name="flare">
<set value="FALSE" var="force_forward"/> <set value="FALSE" var="force_forward"/>
conf/flight_plans/tudelft/nederdrone_troia.xml
+3 -3
View File
@@ -147,7 +147,7 @@
<exception cond="GetPosHeight() @GT 70.0" deroute="GO NORTH"/> <exception cond="GetPosHeight() @GT 70.0" deroute="GO NORTH"/>
<call_once fun="nav_set_heading_current()"/> <call_once fun="nav_set_heading_current()"/>
<call_once fun="waypoint_set_xy_i(WP_CLIMB,POS_BFP_OF_REAL(GetPosX()+20.0*sinf(stateGetNedToBodyEulers_f()->psi)),POS_BFP_OF_REAL(GetPosY()+20.0*cosf(stateGetNedToBodyEulers_f()->psi)))"/> <call_once fun="waypoint_set_xy_i(WP_CLIMB,POS_BFP_OF_REAL(GetPosX()+20.0*sinf(stateGetNedToBodyEulers_f()->psi)),POS_BFP_OF_REAL(GetPosY()+20.0*cosf(stateGetNedToBodyEulers_f()->psi)))"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/> <stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block> </block>
<block name="GO SOUTH"> <block name="GO SOUTH">
<set value="TRUE" var="force_forward"/> <set value="TRUE" var="force_forward"/>
@@ -194,7 +194,7 @@
</block> </block>
<block name="Oval"> <block name="Oval">
<set value="TRUE" var="force_forward"/> <set value="TRUE" var="force_forward"/>
<oval p1="p1" p2="p2" radius="nav_radius"/> <oval p1="p1" p2="p2" radius="nav.radius"/>
</block> </block>
<block name="land here"> <block name="land here">
<set value="FALSE" var="force_forward"/> <set value="FALSE" var="force_forward"/>
@@ -207,7 +207,7 @@
<block name="descend"> <block name="descend">
<set value="FALSE" var="force_forward"/> <set value="FALSE" var="force_forward"/>
<exception cond="LessThan(GetPosHeight(), 15.0)" deroute="flare"/> <exception cond="LessThan(GetPosHeight(), 15.0)" deroute="flare"/>
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/> <stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block> </block>
<block name="flare"> <block name="flare">
<set value="FALSE" var="force_forward"/> <set value="FALSE" var="force_forward"/>
conf/settings/control/rotorcraft_guidance.xml
-40
View File
@@ -1,40 +0,0 @@
<!DOCTYPE settings SYSTEM "../settings.dtd">
<settings target="ap|nps">
<dl_settings>
<dl_settings NAME="Vert Loop">
<dl_setting var="guidance_v_kp" min="0" step="1" max="600" module="guidance/guidance_v" shortname="kp" param="GUIDANCE_V_HOVER_KP" persistent="true"/>
<dl_setting var="guidance_v_kd" min="0" step="1" max="600" module="guidance/guidance_v" shortname="kd" param="GUIDANCE_V_HOVER_KD" persistent="true"/>
<dl_setting var="guidance_v_ki" min="0" step="1" max="300" module="guidance/guidance_v" shortname="ki" handler="SetKi" param="GUIDANCE_V_HOVER_KI" persistent="true"/>
<dl_setting var="guidance_v_nominal_throttle" min="0.2" step="0.01" max="0.8" module="guidance/guidance_v" shortname="nominal_throttle" param="GUIDANCE_V_NOMINAL_HOVER_THROTTLE" persistent="true"/>
<dl_setting var="guidance_v_adapt_throttle_enabled" min="0" step="1" max="1" module="guidance/guidance_v" shortname="adapt_throttle" param="GUIDANCE_V_ADAPT_THROTTLE_ENABLED" values="FALSE|TRUE" persistent="true"/>
<dl_setting var="guidance_v_z_sp" min="-5" step="0.5" max="3" module="guidance/guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
</dl_settings>
<dl_settings NAME="Horiz Loop">
<dl_setting var="guidance_h.use_ref" min="0" step="1" max="1" module="guidance/guidance_h" shortname="use_ref" values="FALSE|TRUE" handler="SetUseRef" param="GUIDANCE_H_USE_REF" persistent="true"/>
<dl_setting var="gh_ref.max_speed" min="0.1" step="0.1" max="15.0" module="guidance/guidance_h" shortname="max_speed" handler="SetMaxSpeed" param="GUIDANCE_H_REF_MAX_SPEED" type="float" persistent="true"/>
<dl_setting var="guidance_h.approx_force_by_thrust" min="0" step="1" max="1" module="guidance/guidance_h" shortname="approx_force" values="FALSE|TRUE" param="GUIDANCE_H_APPROX_FORCE_BY_THRUST" type="uint8" persistent="true"/>
<dl_setting var="gh_ref.tau" min="0.1" step="0.1" max="1.0" module="guidance/guidance_h" shortname="tau" handler="SetTau" param="GUIDANCE_H_REF_TAU" type="float" persistent="true"/>
<dl_setting var="gh_ref.omega" min="0.1" step="0.1" max="3.0" module="guidance/guidance_h" shortname="omega" handler="SetOmega" param="GUIDANCE_H_REF_OMEGA" type="float" persistent="true"/>
<dl_setting var="gh_ref.zeta" min="0.7" step="0.05" max="1.0" module="guidance/guidance_h" shortname="zeta" handler="SetZeta" param="GUIDANCE_H_REF_ZETA" type="float" persistent="true"/>
<dl_setting var="guidance_h.gains.p" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kp" param="GUIDANCE_H_PGAIN" type="int32" persistent="true"/>
<dl_setting var="guidance_h.gains.d" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kd" param="GUIDANCE_H_DGAIN" type="int32" persistent="true"/>
<dl_setting var="guidance_h.gains.i" min="0" step="1" max="400" module="guidance/guidance_h" shortname="ki" handler="set_igain" param="GUIDANCE_H_IGAIN" type="int32" persistent="true"/>
<dl_setting var="guidance_h.gains.v" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kv" param="GUIDANCE_H_VGAIN" type="int32" persistent="true"/>
<dl_setting var="guidance_h.gains.a" min="0" step="1" max="400" module="guidance/guidance_h" shortname="ka" param="GUIDANCE_H_AGAIN" type="int32" persistent="true"/>
<dl_setting var="guidance_h.sp.pos.x" MIN="-10" MAX="10" STEP="1" module="guidance/guidance_h" shortname="sp_x_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="guidance_h.sp.pos.y" MIN="-10" MAX="10" STEP="1" module="guidance/guidance_h" shortname="sp_y_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
</dl_settings>
<dl_settings NAME="NAV">
<dl_setting var="flight_altitude" MIN="0" STEP="0.1" MAX="400" module="navigation" unit="m" handler="SetFlightAltitude"/>
<dl_setting var="nav.heading" MIN="0" STEP="0.1" MAX="360" module="navigation" unit="rad" alt_unit="deg"/>
<dl_setting var="nav.radius" MIN="-50" STEP="0.1" MAX="50" module="navigation" unit="m"/>
<dl_setting var="nav.climb_vspeed" MIN="0" STEP="0.1" MAX="10.0" module="navigation" unit="m/s" param="NAV_CLIMB_VSPEED"/>
<dl_setting var="nav.descend_vspeed" MIN="-10.0" STEP="0.1" MAX="0.0" module="navigation" unit="m/s" param="NAV_DESCEND_VSPEED"/>
</dl_settings>
</dl_settings>
</settings>
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+3 -1
View File
@@ -429,7 +429,9 @@ void guidance_h_from_nav(bool in_flight)
} else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_ATTITUDE) { } else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_ATTITUDE) {
if (nav.setpoint_mode == NAV_SETPOINT_MODE_QUAT) { if (nav.setpoint_mode == NAV_SETPOINT_MODE_QUAT) {
// directly apply quat setpoint // directly apply quat setpoint
QUAT_BFP_OF_REAL(stab_att_sp_quat, nav.quat); struct Int32Quat quat_i;
QUAT_BFP_OF_REAL(quat_i, nav.quat);
stabilization_attitude_set_quat_setpoint_i(&quat_i);
stabilization_attitude_run(in_flight); stabilization_attitude_run(in_flight);
} }
else { else {
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
+1
View File
@@ -40,6 +40,7 @@ extern void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, boo
extern void stabilization_attitude_enter(void); extern void stabilization_attitude_enter(void);
extern void stabilization_attitude_set_failsafe_setpoint(void); extern void stabilization_attitude_set_failsafe_setpoint(void);
extern void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy); extern void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy);
extern void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat);
extern void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading); extern void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
extern void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp); extern void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp);
extern void stabilization_attitude_run(bool in_flight); extern void stabilization_attitude_run(bool in_flight);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+7
View File
@@ -156,6 +156,13 @@ void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy); EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy);
} }
void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
{
struct FloatQuat quat_f,
QUAT_FLOAT_OF_BFP(quat_f, *quat);
float_eulers_of_quat(&stab_att_sp_euler, &quat_f);
}
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
{ {
struct FloatVect2 cmd_f; struct FloatVect2 cmd_f;
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+5
View File
@@ -192,6 +192,11 @@ void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
stab_att_sp_euler = *rpy; stab_att_sp_euler = *rpy;
} }
void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
{
int32_eulers_of_quat(&stab_att_sp_euler, quat);
}
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
{ {
/* Rotate horizontal commands to body frame by psi */ /* Rotate horizontal commands to body frame by psi */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.c
+6
View File
@@ -586,6 +586,12 @@ void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler); int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
} }
void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
{
stab_att_sp_quat = *quat;
int32_eulers_of_quat(&stab_att_sp_euler, quat);
}
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
{ {
// stab_att_sp_euler.psi still used in ref.. // stab_att_sp_euler.psi still used in ref..
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
+5
View File
@@ -88,6 +88,11 @@ void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
stab_att_sp_euler = *rpy; stab_att_sp_euler = *rpy;
} }
void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
{
int32_eulers_of_quat(&stab_att_sp_euler, quat);
}
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
{ {
/* Rotate horizontal commands to body frame by psi */ /* Rotate horizontal commands to body frame by psi */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+6
View File
@@ -234,6 +234,12 @@ void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
float_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler); float_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
} }
void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
{
QUAT_FLOAT_OF_BFP(stab_att_sp_quat, *quat);
float_eulers_of_quat(&stab_att_sp_euler, &stab_att_sp_quat);
}
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
{ {
struct FloatVect2 cmd_f; struct FloatVect2 cmd_f;
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c
+5
View File
@@ -55,6 +55,11 @@ void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
stabilization_indi_set_rpy_setpoint_i(rpy); stabilization_indi_set_rpy_setpoint_i(rpy);
} }
void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
{
stabilization_indi_set_quat_setpoint_i(quat);
}
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
{ {
stabilization_indi_set_earth_cmd_i(cmd, heading); stabilization_indi_set_earth_cmd_i(cmd, heading);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+6
View File
@@ -191,6 +191,12 @@ void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler); int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
} }
void stabilization_attitude_set_quat_setpoint_i(struct Int32Quat *quat)
{
stab_att_sp_quat = *quat;
int32_eulers_of_quat(&stab_att_sp_euler, quat);
}
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
{ {
// stab_att_sp_euler.psi still used in ref.. // stab_att_sp_euler.psi still used in ref..
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+9
View File
@@ -342,6 +342,15 @@ void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler); int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
} }
/**
* @param quat quaternion setpoint
*/
void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat)
{
stab_att_sp_quat = *quat;
int32_eulers_of_quat(&stab_att_sp_euler, quat);
}
/** /**
* @param cmd 2D command in North East axes * @param cmd 2D command in North East axes
* @param heading Heading of the setpoint * @param heading Heading of the setpoint
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
+1
View File
@@ -50,6 +50,7 @@ extern void stabilization_indi_init(void);
extern void stabilization_indi_enter(void); extern void stabilization_indi_enter(void);
extern void stabilization_indi_set_failsafe_setpoint(void); extern void stabilization_indi_set_failsafe_setpoint(void);
extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy); extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy);
extern void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat);
extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading); extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
extern void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp); extern void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp);
extern void stabilization_indi_rate_run(struct FloatRates rate_ref, bool in_flight); extern void stabilization_indi_rate_run(struct FloatRates rate_ref, bool in_flight);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
+9
View File
@@ -256,6 +256,15 @@ void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler); int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
} }
/**
* @param quat quaternion setpoint
*/
void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat)
{
stab_att_sp_quat = *quat;
int32_eulers_of_quat(&stab_att_sp_euler, quat);
}
/** /**
* @brief Set attitude setpoint from command in earth axes * @brief Set attitude setpoint from command in earth axes
* *
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h
+1
View File
@@ -85,6 +85,7 @@ extern void stabilization_indi_init(void);
extern void stabilization_indi_enter(void); extern void stabilization_indi_enter(void);
extern void stabilization_indi_set_failsafe_setpoint(void); extern void stabilization_indi_set_failsafe_setpoint(void);
extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy); extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy);
extern void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat);
extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading); extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
extern void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp); extern void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp);
extern void stabilization_indi_rate_run(struct FloatRates rates_sp, bool in_flight); extern void stabilization_indi_rate_run(struct FloatRates rates_sp, bool in_flight);
sw/airborne/modules/cam_control/rotorcraft_cam.c
+2 -2
View File
@@ -147,7 +147,7 @@ void rotorcraft_cam_periodic(void)
#endif #endif
#if ROTORCRAFT_CAM_USE_PAN #if ROTORCRAFT_CAM_USE_PAN
INT32_COURSE_NORMALIZE(rotorcraft_cam_pan); INT32_COURSE_NORMALIZE(rotorcraft_cam_pan);
nav_heading = rotorcraft_cam_pan; nav.heading = ANGLE_FLOAT_OF_BFP(rotorcraft_cam_pan);
#endif #endif
break; break;
case ROTORCRAFT_CAM_MODE_WP: case ROTORCRAFT_CAM_MODE_WP:
@@ -157,7 +157,7 @@ void rotorcraft_cam_periodic(void)
VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i()); VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i());
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC); INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC);
rotorcraft_cam_pan = int32_atan2(diff.x, diff.y); rotorcraft_cam_pan = int32_atan2(diff.x, diff.y);
nav_heading = rotorcraft_cam_pan; nav.heading = ANGLE_FLOAT_OF_BFP(rotorcraft_cam_pan);
#if ROTORCRAFT_CAM_USE_TILT_ANGLES #if ROTORCRAFT_CAM_USE_TILT_ANGLES
int32_t dist, height; int32_t dist, height;
dist = INT32_VECT2_NORM(diff); dist = INT32_VECT2_NORM(diff);
sw/airborne/modules/ctrl/approach_moving_target.c
+2 -2
View File
@@ -166,7 +166,7 @@ void follow_diagonal_approach(void) {
if(stateGetAirspeed_f() > 13.0) { if(stateGetAirspeed_f() > 13.0) {
Bound(des_vel.z, -4.0, 5.0); Bound(des_vel.z, -4.0, 5.0);
} else { } else {
Bound(des_vel.z, -nav_climb_vspeed, -nav_descend_vspeed); Bound(des_vel.z, -nav.climb_vspeed, -nav.descend_vspeed);
} }
AbiSendMsgVEL_SP(VEL_SP_FCR_ID, &des_vel); AbiSendMsgVEL_SP(VEL_SP_FCR_ID, &des_vel);
@@ -177,7 +177,7 @@ void follow_diagonal_approach(void) {
float min_speed; float min_speed;
float sin_gamma_ref = sinf(gamma_ref); float sin_gamma_ref = sinf(gamma_ref);
if (sin_gamma_ref > 0.05) { if (sin_gamma_ref > 0.05) {
min_speed = (nav_descend_vspeed+0.1) / sin_gamma_ref; min_speed = (nav.descend_vspeed+0.1) / sin_gamma_ref;
} else { } else {
min_speed = -5.0; // prevent dividing by zero min_speed = -5.0; // prevent dividing by zero
} }
sw/airborne/modules/ctrl/object_tracking.c
+1 -1
View File
@@ -108,7 +108,7 @@ void object_tracking_run(void)
if (!bit_is_set(object_frame, 0)) { if (!bit_is_set(object_frame, 0)) {
target_heading += stateGetNedToBodyEulers_f()->psi; // relative frame target_heading += stateGetNedToBodyEulers_f()->psi; // relative frame
} }
float diff = target_heading - ANGLE_FLOAT_OF_BFP(nav_heading); float diff = target_heading - nav.heading;
FLOAT_ANGLE_NORMALIZE(diff); FLOAT_ANGLE_NORMALIZE(diff);
BoundAbs(diff, object_tracking_rate * nav_dt) BoundAbs(diff, object_tracking_rate * nav_dt)
nav_heading += ANGLE_BFP_OF_REAL(diff); nav_heading += ANGLE_BFP_OF_REAL(diff);
sw/airborne/modules/guidance/gvf/gvf_low_level_control.h
+2 -2
View File
@@ -33,8 +33,8 @@
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#define gvf_setNavMode(_navMode) (nav.horizontal_mode = _navMode) #define gvf_setNavMode(_navMode) (nav.horizontal_mode = _navMode)
#define GVF_MODE_ROUTE NAV_HORIZONTAL_MODE_ROUTE #define GVF_MODE_ROUTE NAV_HORIZONTAL_MODE_ROUTE
#define GVF_MODE_WAYPOINT NAV_HORIZONTAL_MODE_WAYPOINT #define GVF_MODE_WAYPOINT HORIZONTAL_MODE_WAYPOINT
#define GVF_MODE_CIRCLE NAV_HORIZONTAL_MODE_CIRCLE #define GVF_MODE_CIRCLE HORIZONTAL_MODE_CIRCLE
#elif defined(ROVER_FIRMWARE) #elif defined(ROVER_FIRMWARE)
#include "state.h" #include "state.h"
sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp
+1 -1
View File
@@ -355,7 +355,7 @@ bool gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, f
bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha) bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha)
{ {
nav.horizontal_mode = NAV_HORIZONTAL_MODE_CIRCLE; // Circle for the 2D GCS nav_horizontal_mode = HORIZONTAL_MODE_CIRCLE; // Circle for the 2D GCS
// Safety first! If the asked altitude is low // Safety first! If the asked altitude is low
if (zl > zh) { if (zl > zh) {
sw/airborne/modules/mav_course_exercise/mav_exercise.c
+1 -1
View File
@@ -140,7 +140,7 @@ uint8_t increase_nav_heading(float incrementDegrees) {
FLOAT_ANGLE_NORMALIZE(new_heading); FLOAT_ANGLE_NORMALIZE(new_heading);
// set heading // set heading
nav_heading = ANGLE_BFP_OF_REAL(new_heading); nav.heading = new_heading;
return false; return false;
} }
sw/airborne/modules/orange_avoider/orange_avoider.c
+1 -2
View File
@@ -185,8 +185,7 @@ uint8_t increase_nav_heading(float incrementDegrees)
FLOAT_ANGLE_NORMALIZE(new_heading); FLOAT_ANGLE_NORMALIZE(new_heading);
// set heading, declared in firmwares/rotorcraft/navigation.h // set heading, declared in firmwares/rotorcraft/navigation.h
// for performance reasons the navigation variables are stored and processed in Binary Fixed-Point format nav.heading = new_heading;
nav_heading = ANGLE_BFP_OF_REAL(new_heading);
VERBOSE_PRINT("Increasing heading to %f\n", DegOfRad(new_heading)); VERBOSE_PRINT("Increasing heading to %f\n", DegOfRad(new_heading));
return false; return false;
sw/airborne/modules/stereocam/droplet/stereocam_droplet.c
+2 -2
View File
@@ -124,8 +124,8 @@ void stereocam_droplet_periodic(void)
enu.x = waypoint_get_x(WP_GOAL); enu.x = waypoint_get_x(WP_GOAL);
enu.y = waypoint_get_y(WP_GOAL); enu.y = waypoint_get_y(WP_GOAL);
enu.z = waypoint_get_alt(WP_GOAL); enu.z = waypoint_get_alt(WP_GOAL);
float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading)); float sin_heading = sinf(nav.heading);
float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading)); float cos_heading = cosf(nav.heading);
enu.x += (sin_heading * 1.3 / 20); enu.x += (sin_heading * 1.3 / 20);
enu.y += (cos_heading * 1.3 / 20); enu.y += (cos_heading * 1.3 / 20);
waypoint_set_enu(WP_GOAL, &enu); waypoint_set_enu(WP_GOAL, &enu);
sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.c
+2 -2
View File
@@ -91,8 +91,8 @@ void run_avoid_navigation_onvision(void)
//Stop and put waypoint 2.5 m ahead //Stop and put waypoint 2.5 m ahead
struct EnuCoor_i new_coor; struct EnuCoor_i new_coor;
struct EnuCoor_i *pos = stateGetPositionEnu_i(); struct EnuCoor_i *pos = stateGetPositionEnu_i();
float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading)); float sin_heading = sinf(nav.heading);
float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading)); float cos_heading = cosf(nav.heading);
new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH)); new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH));
new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH)); new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH));
new_coor.z = pos->z; new_coor.z = pos->z;
sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.c
+5 -5
View File
@@ -23,7 +23,7 @@
#define STEREOCAM_FOLLOW_ME_USE_OPTITRACK FALSE #define STEREOCAM_FOLLOW_ME_USE_OPTITRACK FALSE
#endif #endif
#define HEADING_CHANGE_PER_MEASUREMENT 260 #define HEADING_CHANGE_PER_MEASUREMENT 0.063f
#define CENTER_IMAGE_HOR 65 #define CENTER_IMAGE_HOR 65
#define MAXIMUM_ALTITUDE_FOLLOWING 3.0 #define MAXIMUM_ALTITUDE_FOLLOWING 3.0
#define MINIMUM_ALTITUDE_FOLLOWING 1.0 #define MINIMUM_ALTITUDE_FOLLOWING 1.0
@@ -54,7 +54,7 @@ static void changeRollYawPhase(int *phaseCounterArg, int *isRollPhaseArg, int *i
} }
} }
static void increase_nav_heading(int32_t *heading, int32_t increment) static void increase_nav_heading(float *heading, float increment)
{ {
*heading = *heading + increment; *heading = *heading + increment;
} }
@@ -70,17 +70,17 @@ void follow_me(uint8_t headingToFollow, uint8_t heightObject, uint8_t distanceTo
// Change our heading if the user is starting to get out of sight. // Change our heading if the user is starting to get out of sight.
float heading_change = 0.0; float heading_change = 0.0;
int headingChangePerIt = HEADING_CHANGE_PER_MEASUREMENT; float headingChangePerIt = HEADING_CHANGE_PER_MEASUREMENT;
if (abs(headingToFollow - CENTER_IMAGE_HOR) > 10) { if (abs(headingToFollow - CENTER_IMAGE_HOR) > 10) {
if (headingToFollow > CENTER_IMAGE_HOR) { if (headingToFollow > CENTER_IMAGE_HOR) {
heading_change = 0.25; heading_change = 0.25;
if (isYawPhase || STEREOCAM_FOLLOW_ME_USE_OPTITRACK) { if (isYawPhase || STEREOCAM_FOLLOW_ME_USE_OPTITRACK) {
increase_nav_heading(&nav_heading, headingChangePerIt); increase_nav_heading(&nav.heading, headingChangePerIt);
} }
} else if (headingToFollow < CENTER_IMAGE_HOR) { } else if (headingToFollow < CENTER_IMAGE_HOR) {
heading_change = -0.25; heading_change = -0.25;
if (isYawPhase || STEREOCAM_FOLLOW_ME_USE_OPTITRACK) { if (isYawPhase || STEREOCAM_FOLLOW_ME_USE_OPTITRACK) {
increase_nav_heading(&nav_heading, -1 * headingChangePerIt); increase_nav_heading(&nav.heading, -1 * headingChangePerIt);
} }
} else { } else {
heading_change = 0.0; heading_change = 0.0;
sw/lib/ocaml/fp_proc.ml
+3 -1
View File
@@ -235,7 +235,8 @@ let parse_include = fun dir flight_plan include_xml ->
and modules = get_children "modules" proc and modules = get_children "modules" proc
and blocks = get_children "blocks" proc and blocks = get_children "blocks" proc
and sectors = get_children "sectors" proc and sectors = get_children "sectors" proc
and header = get_pc_data "header" proc in and header = get_pc_data "header" proc
and variables = get_children "variables" proc in
let exceptions = List.map (transform_exception prefix reroutes (proc_name, env)) exceptions let exceptions = List.map (transform_exception prefix reroutes (proc_name, env)) exceptions
and blocks = List.map (transform_block prefix reroutes (proc_name, env)) blocks in and blocks = List.map (transform_block prefix reroutes (proc_name, env)) blocks in
@@ -245,6 +246,7 @@ let parse_include = fun dir flight_plan include_xml ->
["waypoints", waypoints; ["waypoints", waypoints;
"blocks", blocks; "blocks", blocks;
"modules", modules; "modules", modules;
"variables", variables;
"exceptions", exceptions; "exceptions", exceptions;
"sectors", sectors] "sectors", sectors]
(append_pc_data "header" header flight_plan) (append_pc_data "header" header flight_plan)