Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

en route pour le nouveau FMS

Browse Source
This commit is contained in:
Antoine Drouin
2010-08-24 14:12:51 +00:00
parent 24a95659b4
commit f3b5e82769
7 changed files with 25 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/UofAdelaide/A1000_LISA.xml
+1 -1
View File
@@ -19,7 +19,7 @@
<command_laws>
<set servo="ESC" value="motors_on?ESC_HOVER:ESC_STOPPED"/>
<set servo="FRONT" value=" @PITCH + @YAW + @THRUST"/>
<set servo="FRONT" value=" @PITCH + @YAW + (@THRUST<<5)"/>
<set servo="BACK" value="-@PITCH + @YAW + @THRUST"/>
<set servo="LEFT" value=" @ROLL - @YAW + @THRUST"/>
<set servo="RIGHT" value="-@ROLL - @YAW + @THRUST"/>
conf/flight_plans/basic_booz.xml
+4
View File
@@ -63,11 +63,15 @@
<block name="circle">
<circle radius="nav_radius" wp="p1"/>
</block>
<!--
<block name="Joystick" pre_call="NavUpdateWPFromFms(WP_p1)" strip_button="Joystick" strip_icon="joystick.png">
<set var="fms.enabled" value="TRUE"/>
<call fun="NavSetWaypointHere(WP_p1)"/>
<stay wp="p1"/>
</block>
-->
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<call fun="NavSetWaypointHere(WP_TD)"/>
</block>
conf/telemetry/telemetry_booz2.xml
+1
View File
@@ -19,6 +19,7 @@
</mode>
<mode name="ppm">
<message name="BOOZ2_CMD" period=".05"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="BOOZ2_RADIO_CONTROL" period="0.5"/>
sw/airborne/booz/booz2_autopilot.c
+1 -1
View File
@@ -71,7 +71,7 @@ void booz2_autopilot_init(void) {
void booz2_autopilot_periodic(void) {
RunOnceEvery(BOOZ2_AP_NAV_RATE, nav_periodic_task());
RunOnceEvery(BOOZ2_NAV_PRESCALER, nav_periodic_task());
#ifdef BOOZ_FAILSAFE_GROUND_DETECT
if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE && booz2_autopilot_detect_ground) {
booz2_autopilot_set_mode(BOOZ2_AP_MODE_KILL);
sw/airborne/booz/booz2_autopilot.h
-1
View File
@@ -46,7 +46,6 @@
#define BOOZ2_AP_MODE_HOVER_Z_HOLD 11
#define BOOZ2_AP_MODE_NAV 12
#define BOOZ2_AP_NAV_RATE 32
extern uint8_t booz2_autopilot_mode;
extern uint8_t booz2_autopilot_mode_auto2;
sw/airborne/booz/booz2_navigation.c
+14 -25
View File
@@ -25,6 +25,7 @@
#include "booz2_navigation.h"
#include "booz/booz2_debug.h"
#include "booz_gps.h"
#include "booz2_ins.h"
@@ -32,10 +33,6 @@
#include "modules.h"
#include "flight_plan.h"
#ifdef USE_FMS
#include "booz_fms.h"
#endif
#include "math/pprz_algebra_int.h"
const uint8_t nb_waypoint = NB_WAYPOINT;
@@ -317,28 +314,20 @@ void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) {
}
}
#ifdef USE_FMS
void nav_update_wp_from_fms(uint8_t _wp) {
if (fms.enabled && _wp < nb_waypoint) {
int32_t s_heading, c_heading;
PPRZ_ITRIG_SIN(s_heading, nav_heading);
PPRZ_ITRIG_COS(c_heading, nav_heading);
struct Int32Vect3 dpos;
dpos.x = (NavFmsMaxHSpeed * fms.input.h_sp.speed.x >> (INT32_SPEED_FRAC - INT32_POS_FRAC)) / (BOOZ2_AP_NAV_RATE * 128);
dpos.y = (NavFmsMaxHSpeed * fms.input.h_sp.speed.y >> (INT32_SPEED_FRAC - INT32_POS_FRAC)) / (BOOZ2_AP_NAV_RATE * 128);
dpos.z = (NavFmsMaxVSpeed * fms.input.v_sp.climb >> (INT32_SPEED_FRAC - INT32_POS_FRAC)) / (BOOZ2_AP_NAV_RATE * 128);
waypoints[_wp].x += (s_heading * dpos.x + c_heading * dpos.y) >> INT32_TRIG_FRAC;
waypoints[_wp].y += (c_heading * dpos.x - s_heading * dpos.y) >> INT32_TRIG_FRAC;
waypoints[_wp].z += dpos.z;
int32_t dheading = (NavFmsMaxHeadingRate * fms.input.h_sp.speed.z >> (INT32_RATE_FRAC - INT32_ANGLE_FRAC)) / (BOOZ2_AP_NAV_RATE * 128);
if (dheading != 0) {
nav_heading += dheading;
INT32_COURSE_NORMALIZE(nav_heading);
}
RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &_wp, &(waypoints[_wp].x), &(waypoints[_wp].y), &(waypoints[_wp].z)));
}
void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp ) {
MY_ASSERT(_wp < nb_waypoint);
int32_t s_heading, c_heading;
PPRZ_ITRIG_SIN(s_heading, nav_heading);
PPRZ_ITRIG_COS(c_heading, nav_heading);
struct Int32Vect3 delta_pos;
VECT3_SDIV(delta_pos, speed_sp,BOOZ2_NAV_FREQ); /* fixme :make sure the division is really a >> */
waypoints[_wp].x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC;
waypoints[_wp].y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC;
waypoints[_wp].z += delta_pos.z;
nav_heading += heading_rate_sp / BOOZ2_NAV_FREQ;
INT32_COURSE_NORMALIZE(nav_heading);
RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &_wp, &(waypoints[_wp].x), &(waypoints[_wp].y), &(waypoints[_wp].z)));
}
#endif /* USE_FMS */
bool_t nav_detect_ground(void) {
if (!booz2_autopilot_detect_ground) return FALSE;
sw/airborne/booz/booz2_navigation.h
+4 -9
View File
@@ -28,6 +28,9 @@
#include "math/pprz_geodetic_int.h"
#include "math/pprz_geodetic_float.h"
#define BOOZ2_NAV_FREQ 16
#define BOOZ2_NAV_PRESCALER (PERIODIC_FREQ/BOOZ2_NAV_FREQ)
extern struct EnuCoor_i booz2_navigation_target;
extern struct EnuCoor_i booz2_navigation_carrot;
@@ -212,15 +215,7 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
#define GetPosY() POS_FLOAT_OF_BFP(booz_ins_enu_pos.y)
#define GetPosAlt() (POS_FLOAT_OF_BFP(booz_ins_enu_pos.z+ground_alt))
#ifdef USE_FMS
#define NavFmsMaxHSpeed ((int32_t)SPEED_BFP_OF_REAL(4.))
#define NavFmsMaxVSpeed ((int32_t)SPEED_BFP_OF_REAL(2.))
#define NavFmsMaxHeadingRate ((int32_t)RATE_BFP_OF_REAL(RadOfDeg(60.)))
extern void nav_update_wp_from_fms(uint8_t _wp);
#define NavUpdateWPFromFms(_wp) { \
nav_update_wp_from_fms(_wp); \
}
#endif /* USE_FMS */
extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp );
#endif /* BOOZ2_NAVIGATION_H */