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

*** empty log message ***

Browse Source
This commit is contained in:
Pascal Brisset
2007-01-31 15:52:54 +00:00
parent cf2b718d1d
commit c8ee71ef90
15 changed files with 204 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/microjet7.xml
+12 -7
View File
@@ -5,8 +5,12 @@
<!-- commands section -->
<servos>
<servo name="GAZ" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="AILEVON_LEFT" no="3" min="1075" neutral="1516" max="2025"/>
<servo name="AILEVON_RIGHT" no="6" min="1975" neutral="1468" max="975"/>
<!--
<servo name="AILEVON_LEFT" no="3" min="1075" neutral="1487" max="2025"/>
<servo name="AILEVON_RIGHT" no="6" min="1975" neutral="1433" max="975"/>
-->
<servo name="AILEVON_LEFT" no="6" min="1075" neutral="1480" max="2025"/>
<servo name="AILEVON_RIGHT" no="3" min="1975" neutral="1515" max="975"/>
<servo name="HATCH" no="2" max="2000" neutral="1000" min="1000"/>
</servos>
@@ -102,8 +106,8 @@
<define name="CARROT" value="5." unit="s"/>
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
<define name="CONTROL_RATE" value="60" unit="Hz"/>
<!-- <define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/> -->
<define name="NO_XBEE_API_INIT" value="TRUE"/>
<define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/>
<!-- <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
<define name="TRIGGER_DELAY" value="1."/>
@@ -249,9 +253,10 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
ap.CFLAGS += -DGYRO -DADXRS150
ap.srcs += gyro.c
ap.srcs += gyro.c nav_line.c chemotaxis.c anemotaxis.c discsurvey.c
ap.srcs += traffic_info.c
# ap.srcs += bomb.c
ap.srcs += bomb.c
# Hack to use the same tuning file than slayer1
ap.CFLAGS += -DUSE_GPIO
@@ -267,7 +272,7 @@ ap.srcs += $(SRC_ARCH)/gpio.c
include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
sim.CFLAGS += -DCONFIG=\"tiny.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM -DALT_KALMAN -DIR_360
sim.srcs += traffic_info.c
sim.srcs += anemotaxis.c chemotaxis.c discsurvey.c nav_line.c
sim.srcs += bomb.c nav_line.c chemotaxis.c anemotaxis.c discsurvey.c
conf/airframes/minimag1.xml
+9 -9
View File
@@ -4,7 +4,7 @@
<!-- commands section -->
<servos>
<servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="THROTTLE" no="0" min="1200" neutral="1200" max="2000"/>
<!-- Servo aileron gauche = couleur blanche -->
<!-- Aileron gauche oppose de l'aileron droit -->
<servo name="AILERON_LEFT" no="1" min="1675" neutral="1500" max="1150"/>
@@ -12,7 +12,7 @@
<servo name="AILERON_RIGHT" no="3" min="1775" neutral="1475" max="1325"/>
<!-- Rudder = Couleur rouge droite gauche -->
<servo name="RUDDER" no="4" min="1875" neutral="1500" max="1250"/>
<servo name="ELEVATOR" no="5" min="1850" neutral="1500" max="1150"/>
<servo name="ELEVATOR" no="5" min="1850" neutral="1586" max="1150"/>
</servos>
@@ -59,18 +59,18 @@
</section>
<section name="INFRARED" prefix="IR_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="ROLL_NEUTRAL_DEFAULT" value="6" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="DEFAULT_CONTRAST" value="200"/>
<define name="RAD_OF_IR_CONTRAST" value="0.75"/>
<linear name="RollOfIrs" arity="2" coeff1="-0.7" coeff2="0.7"/>
<linear name="PitchOfIrs" arity="2" coeff1="-0.7" coeff2="-0.7"/>
<linear name="RollOfIrs" arity="2" coeff1="0.7" coeff2="0.7"/>
<linear name="PitchOfIrs" arity="2" coeff1="-0.7" coeff2="0.7"/>
<linear name="TopOfIr" arity="1" coeff1="1"/>
<define name="RAD_OF_IR_MAX_VALUE" value="0.0045"/>
<define name="RAD_OF_IR_MIN_VALUE" value="0.00075"/>
<define name="ADC_ROLL_NEUTRAL" value="0"/>
<define name="ADC_PITCH_NEUTRAL" value="-716"/>
<define name="ADC_ROLL_NEUTRAL" value="716"/>
<define name="ADC_PITCH_NEUTRAL" value="0"/>
<define name="ADC_TOP_NEUTRAL" value="512"/>
@@ -117,7 +117,7 @@
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
<!-- auto throttle inner loop -->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.35"/>
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.45"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.30"/>
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.80"/>
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
@@ -144,7 +144,7 @@
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
<define name="ROLL_PGAIN" value="5000."/>
<define name="ROLL_PGAIN" value="12000."/>
<define name="AILERON_OF_THROTTLE" value="0.0"/>
<define name="PITCH_PGAIN" value="-9000."/>
<define name="PITCH_DGAIN" value="1.5"/>
conf/flight_plans/IS.xml
+53 -12
View File
@@ -1,22 +1,29 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan ground_alt="200" alt="250" lat0="43.4624" lon0="1.2727" max_dist_from_home="1500" name="Intelligent Surveillance" qfu="270" security_height="25">
<flight_plan ground_alt="185" alt="250" lat0="43.4624" lon0="1.2727" max_dist_from_home="500" name="Intelligent Surveillance" qfu="270" security_height="25">
<header>
#include "anemotaxis.h"
#include "chemotaxis.h"
#include "discsurvey.h"
#include "nav_line.h"
#include "bomb.h"
</header>
<waypoints>
<waypoint name="HOME" x="0" y="0"/>
<waypoint name="1" x="200" y="0"/>
<waypoint name="2" x="0" y="200"/>
<waypoint name="1" x="-75" y="75"/>
<waypoint name="2" x="75" y="75"/>
<waypoint name="MOB" x="200" y="200"/>
<waypoint name="S1" x="-200" y="-200"/>
<waypoint name="S2" x="300" y="400"/>
<waypoint name="C" x="-250" y="-200"/>
<waypoint name="C1" x="-200" y="-200"/>
<waypoint name="C2" x="300" y="400"/>
<waypoint name="PLUME" x="400" y="400"/>
<waypoint name="PLUME" x="400" y="300"/>
<waypoint name="BASELEG" x="400" y="300"/>
<waypoint name="RELEASE" x="400" y="300"/>
<waypoint name="CLIMB" x="400" y="300"/>
<waypoint name="TARGET" x="0" y="30" alt="185"/>
<waypoint name="START" x="200" y="30"/>
</waypoints>
<blocks>
<!--
@@ -27,11 +34,11 @@
<block name="init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call fun="NavSetGroundReferenceHere()"/>
<deroute block="circle 1"/>
<deroute block="wait"/>
</block>
-->
<block name="circle 1">
<block name="wait">
<circle radius="75" wp="1"/>
</block>
@@ -39,6 +46,12 @@
<eight radius="75" center="1" turn_around="2"/>
</block>
<block name="Line" strip_button="Line">
<call fun="nav_line_init()"/>
<call fun="nav_line(WP_1, WP_2, 75)"/>
</block>
<block name="oval" strip_button="Oval">
<oval p1="1" p2="2" radius="80"/>
</block>
@@ -48,21 +61,28 @@
<circle wp="MOB" radius="100"/>
</block>
<block name="Anemotaxis" strip_button="Anemotaxis">
<call fun="nav_anemotaxis_downwind(WP_C)"/>
<block name="Anemotaxis" strip_button="Anem">
<call fun="nav_anemotaxis_downwind(WP_C, 400)"/>
<go wp="C" hmode="route" approaching_time="0"/>
<call fun="nav_anemotaxis_init(WP_C)"/>
<call fun="nav_anemotaxis(WP_C, WP_C1, WP_C2, WP_PLUME)"/>
</block>
<block name="Chemotaxis" strip_button="Chemotaxis">
<call fun="nav_chemotaxis_init()"/>
<block name="Chemotaxis" strip_button="Chem">
<call fun="nav_chemotaxis_init(WP_C, WP_PLUME)"/>
<call fun="nav_chemotaxis(WP_C, WP_PLUME)"/>
</block>
<block name="ChemotaxisHere">
<set var="waypoints[WP_C].x" value="estimator_x"/>
<set var="waypoints[WP_C].y" value="estimator_y"/>
<deroute block="Chemotaxis"/>
</block>
<block name="Disc survey" strip_button="DS">
<exception cond="chemo_sensor > 0" deroute="ChemotaxisHere"/>
<call fun="disc_survey_init()"/>
<call fun="disc_survey(WP_HOME, 500)"/>
<call fun="disc_survey(WP_HOME, 350, 75)"/>
</block>
<block name="circle 1 auto pitch">
@@ -83,7 +103,7 @@
<block name="descent 0" strip_button="Descent">
<circle radius="50+(estimator_z-ground_alt)/2" wp="1" throttle="0.0" pitch="-0.3" vmode="throttle" until="ground_alt+50 > estimator_z"/>
<deroute block="circle 1"/>
<deroute block="wait"/>
</block>
<block name="route12">
@@ -113,5 +133,26 @@
<block name="follow">
<follow ac_id="5" distance="25" height="10"/>
</block>
<block name="bomb" strip_button="Bomb">
<set value="BombComputeApproach()" var="unit"/>
<circle radius="BOMB_RADIUS" until="NavQdrCloseTo(DegOfRad(bomb_start_qdr)-10)" wp="BASELEG"/>
</block>
<block name="align">
<exception cond="BombUpdateRelease()" deroute="wait"/>
<go approaching_time="bomb_trigger_delay" from="START" hmode="route" wp="RELEASE"/>
</block>
<block name="shoot">
<set value="BombShoot()" var="unit"/>
<go approaching_time="0" from="RELEASE" hmode="route" wp="CLIMB"/>
<set value="BombCloseHatch()" var="unit"/>
<deroute block="wait"/>
</block>
<block name="close">
<set value="BombCloseHatch()" var="unit"/>
<deroute block="wait"/>
</block>
</blocks>
</flight_plan>
conf/flight_plans/versatile.xml
+7 -1
View File
@@ -1,6 +1,9 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan ground_alt="200" alt="250" lat0="43.4624" lon0="1.2727" max_dist_from_home="1000" name="Versatile" qfu="270" security_height="25">
<flight_plan ground_alt="200" alt="250" lat0="43.46223" lon0="1.27289" max_dist_from_home="1000" name="Versatile" qfu="270" security_height="25">
<header>
#include "bomb.h"
</header>
<waypoints>
<waypoint name="HOME" x="0" y="0"/>
<waypoint name="1" x="200" y="0"/>
@@ -82,8 +85,11 @@
<survey_rectangle grid="150" wp1="S1" wp2="S2"/>
</block>
<!--
<block name="follow">
<follow ac_id="5" distance="25" height="10"/>
</block>
-->
</blocks>
</flight_plan>
sw/airborne/anemotaxis.c
+8 -8
View File
@@ -17,10 +17,10 @@ static void last_plume_was_here( void ) {
last_plume.y = estimator_y;
}
bool_t nav_anemotaxis_downwind( uint8_t c ) {
bool_t nav_anemotaxis_downwind( uint8_t c, float radius ) {
float wind_dir = atan2(wind_north, wind_east);
waypoints[c].x = waypoints[WP_HOME].x + MAX_DIST_FROM_HOME*0.9*cos(wind_dir);
waypoints[c].y = waypoints[WP_HOME].y + MAX_DIST_FROM_HOME*0.9*sin(wind_dir);
waypoints[c].x = waypoints[WP_HOME].x + radius*cos(wind_dir);
waypoints[c].y = waypoints[WP_HOME].y + radius*sin(wind_dir);
return FALSE;
}
@@ -39,7 +39,7 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) {
last_plume_was_here();
waypoints[plume].x = estimator_x;
waypoints[plume].y = estimator_y;
DownlinkSendWp(plume);
// DownlinkSendWp(plume);
}
float wind_dir = atan2(wind_north, wind_east) + M_PI;
@@ -62,8 +62,8 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) {
waypoints[c2].x = waypoints[c1].x - width*crosswind_x*sign;
waypoints[c2].y = waypoints[c1].y - width*crosswind_y*sign;
DownlinkSendWp(c1);
DownlinkSendWp(c2);
// DownlinkSendWp(c1);
// DownlinkSendWp(c2);
status = CROSSWIND;
nav_init_stage();
@@ -76,7 +76,7 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) {
waypoints[c].x = waypoints[c2].x + MIN_CIRCLE_RADIUS*upwind_x;
waypoints[c].y = waypoints[c2].y + MIN_CIRCLE_RADIUS*upwind_y;
DownlinkSendWp(c);
// DownlinkSendWp(c);
sign = -sign;
status = UTURN;
@@ -87,7 +87,7 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) {
waypoints[c].x = estimator_x + MIN_CIRCLE_RADIUS*upwind_x;
waypoints[c].y = estimator_y + MIN_CIRCLE_RADIUS*upwind_y;
DownlinkSendWp(c);
// DownlinkSendWp(c);
sign = -sign;
status = UTURN;
sw/airborne/anemotaxis.h
+1 -1
View File
@@ -1,7 +1,7 @@
#include "std.h"
extern uint8_t chemo_sensor;
extern bool_t nav_anemotaxis_downwind(uint8_t c);
extern bool_t nav_anemotaxis_downwind(uint8_t c, float radius);
extern bool_t nav_anemotaxis_init(uint8_t c);
extern bool_t nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume);
sw/airborne/chemotaxis.c
+22 -14
View File
@@ -7,40 +7,48 @@
uint8_t chemo_sensor;
#define MAX_RADIUS 500
#define MAX_RADIUS 250
#define ALPHA 0.5
static uint8_t last_plume_value;
#define MAX_CHEMO 255
static float radius;
static int8_t sign;
bool_t nav_chemotaxis_init( void ) {
bool_t nav_chemotaxis_init( uint8_t c, uint8_t plume ) {
radius = MAX_RADIUS;
last_plume_value = 0;
sign = 1;
waypoints[plume].x = waypoints[c].x;
waypoints[plume].y = waypoints[c].y;
chemo_sensor = 0;
return FALSE;
}
bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) {
if (chemo_sensor > last_plume_value) {
/* Move the circle in this direction */
float x = estimator_x - waypoints[plume].x;
float y = estimator_y - waypoints[plume].y;
waypoints[c].x = waypoints[plume].x + ALPHA * x;
waypoints[c].y = waypoints[plume].y + ALPHA * y;
// DownlinkSendWp(c);
/* Turn in the right direction */
float dir_x = cos(M_PI_2 - estimator_hspeed_dir);
float dir_y = sin(M_PI_2 - estimator_hspeed_dir);
float pvect = dir_x*y - dir_y*x;
sign = (pvect > 0 ? -1 : 1);
/* Reduce the radius */
radius = sign * (MIN_CIRCLE_RADIUS+(MAX_CHEMO-chemo_sensor)/(float)MAX_CHEMO*(MAX_RADIUS-MIN_CIRCLE_RADIUS));
/* Store this plume */
waypoints[plume].x = estimator_x;
waypoints[plume].y = estimator_y;
DownlinkSendWp(plume);
// DownlinkSendWp(plume);
last_plume_value = chemo_sensor;
/* Reduce the radius */
sign = - sign;
radius = sign * (MIN_CIRCLE_RADIUS+(255-chemo_sensor)/256.*(MAX_RADIUS-MIN_CIRCLE_RADIUS));
/* Move the circle in this direction */
float x = waypoints[c].x - waypoints[plume].x;
float y = waypoints[c].y - waypoints[plume].y;
waypoints[c].x = waypoints[plume].x + ALPHA * x;
waypoints[c].y = waypoints[plume].y + ALPHA * y;
DownlinkSendWp(c);
}
NavCircleWaypoint(c, radius);
sw/airborne/chemotaxis.h
+1 -1
View File
@@ -1,4 +1,4 @@
#include "std.h"
extern bool_t nav_chemotaxis_init( void );
extern bool_t nav_chemotaxis_init( uint8_t c, uint8_t plume );
extern bool_t nav_chemotaxis( uint8_t c, uint8_t plume );
sw/airborne/discsurvey.c
+32 -20
View File
@@ -5,24 +5,22 @@
#include "flight_plan.h"
#include "ap_downlink.h"
enum status { UTURN, CROSSWIND };
enum status { UTURN, SEGMENT, DOWNWIND };
static enum status status;
static int8_t sign;
static int8_t upwind;
static struct point c;
static struct point c1;
static struct point c2;
bool_t disc_survey_init( void ) {
status = UTURN;
status = DOWNWIND;
sign = 1;
c.x = estimator_x;
c.y = estimator_y;
upwind = 1;
c1.x = estimator_x;
c1.y = estimator_y;
return FALSE;
}
bool_t disc_survey( uint8_t center, float radius ) {
bool_t disc_survey( uint8_t center, float radius, float grid ) {
float wind_dir = atan2(wind_north, wind_east) + M_PI;
/** Not null even if wind_east=wind_north=0 */
@@ -31,30 +29,40 @@ bool_t disc_survey( uint8_t center, float radius ) {
switch (status) {
case UTURN:
nav_circle_XY(c.x, c.y, MIN_CIRCLE_RADIUS*sign);
nav_circle_XY(c.x, c.y, grid*sign);
if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
c1.x = estimator_x;
c1.y = estimator_y;
float d = ScalarProduct(upwind_x, upwind_y, estimator_x-waypoints[center].x, estimator_y-waypoints[center].y);
float w = sqrt(radius*radius - d*d) - 1.5*MIN_CIRCLE_RADIUS;
float crosswind_x = - upwind_y;
float crosswind_y = upwind_x;
c2.x = waypoints[center].x+d*upwind_x-w*sign*crosswind_x;
c2.y = waypoints[center].y+d*upwind_y-w*sign*crosswind_y;
status = CROSSWIND;
if (d > radius) {
status = DOWNWIND;
} else {
float w = sqrt(radius*radius - d*d) - 1.5*grid;
float crosswind_x = - upwind_y;
float crosswind_y = upwind_x;
c2.x = waypoints[center].x+d*upwind_x-w*sign*crosswind_x;
c2.y = waypoints[center].y+d*upwind_y-w*sign*crosswind_y;
status = SEGMENT;
}
nav_init_stage();
}
break;
case CROSSWIND:
case DOWNWIND:
c2.x = waypoints[center].x - upwind_x * radius;
c2.y = waypoints[center].y - upwind_y * radius;
status = SEGMENT;
/* No break; */
case SEGMENT:
nav_route_xy(c1.x, c1.y, c2.x, c2.y);
if (nav_approaching_xy(c2.x, c2.y, CARROT)) {
c.x = c2.x + MIN_CIRCLE_RADIUS*upwind_x;
c.y = c2.y + MIN_CIRCLE_RADIUS*upwind_y;
c.x = c2.x + grid*upwind_x;
c.y = c2.y + grid*upwind_y;
sign = -sign;
status = UTURN;
@@ -62,5 +70,9 @@ bool_t disc_survey( uint8_t center, float radius ) {
}
break;
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(center), 0.); /* No preclimb */
return TRUE;
}
sw/airborne/discsurvey.h
+1 -1
View File
@@ -1,5 +1,5 @@
#include "std.h"
extern bool_t disc_survey_init( void );
extern bool_t disc_survey(uint8_t c, float radius);
extern bool_t disc_survey(uint8_t c, float radius, float grid);
sw/airborne/nav.c
+1 -35
View File
@@ -158,40 +158,6 @@ void nav_circle_XY(float x, float y, float radius) {
#define NavSurveyRectangleInit(_wp1, _wp2, _grid) nav_survey_rectangle_init(_wp1, _wp2, _grid)
#define NavSurveyRectangle(_wp1, _wp2) nav_survey_rectangle(_wp1, _wp2)
/** Set the climb control to auto-throttle with the specified pitch
pre-command */
#define NavVerticalAutoThrottleMode(_pitch) { \
v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; \
nav_pitch = _pitch; \
}
/** Set the climb control to auto-pitch with the specified throttle
pre-command */
#define NavVerticalAutoPitchMode(_throttle) { \
v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_PITCH; \
nav_throttle_setpoint = _throttle; \
}
/** Set the vertical mode to altitude control with the specified altitude
setpoint and climb pre-command. */
#define NavVerticalAltitudeMode(_alt, _pre_climb) { \
v_ctl_mode = V_CTL_MODE_AUTO_ALT; \
nav_altitude = _alt; \
v_ctl_altitude_pre_climb = _pre_climb; \
}
/** Set the vertical mode to climb control with the specified climb setpoint */
#define NavVerticalClimbMode(_climb) { \
v_ctl_mode = V_CTL_MODE_AUTO_CLIMB; \
v_ctl_climb_setpoint = _climb; \
}
/** Set the vertical mode to fixed throttle with the specified setpoint */
#define NavVerticalThrottleMode(_throttle) { \
v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE; \
nav_throttle_setpoint = _throttle; \
}
#define NavGlide(_last_wp, _wp) { \
float start_alt = waypoints[_last_wp].a; \
float diff_alt = waypoints[_wp].a - start_alt; \
@@ -431,7 +397,7 @@ static inline void fly_to_xy(float x, float y) {
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
float leg_x = wp_x - last_wp_x;
float leg_y = wp_y - last_wp_y;
float leg2 = leg_x * leg_x + leg_y * leg_y;
float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.);
nav_leg_progress = ((estimator_x - last_wp_x) * leg_x + (estimator_y - last_wp_y) * leg_y) / leg2;
nav_leg_progress = Max(nav_leg_progress, 0.);
nav_leg_length = sqrt(leg2);
sw/airborne/nav.h
+37
View File
@@ -35,6 +35,7 @@
#include "std.h"
#include "paparazzi.h"
#include "airframe.h"
#include "fw_v_ctl.h"
#define G 9.806
#define Square(_x) ((_x)*(_x))
@@ -151,4 +152,40 @@ extern void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_
bool_t nav_approaching_xy(float x, float y, float approaching_time);
#define NavApproaching(wp, time) nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, time)
/** Set the climb control to auto-throttle with the specified pitch
pre-command */
#define NavVerticalAutoThrottleMode(_pitch) { \
v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; \
nav_pitch = _pitch; \
}
/** Set the climb control to auto-pitch with the specified throttle
pre-command */
#define NavVerticalAutoPitchMode(_throttle) { \
v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_PITCH; \
nav_throttle_setpoint = _throttle; \
}
/** Set the vertical mode to altitude control with the specified altitude
setpoint and climb pre-command. */
#define NavVerticalAltitudeMode(_alt, _pre_climb) { \
v_ctl_mode = V_CTL_MODE_AUTO_ALT; \
nav_altitude = _alt; \
v_ctl_altitude_pre_climb = _pre_climb; \
}
/** Set the vertical mode to climb control with the specified climb setpoint */
#define NavVerticalClimbMode(_climb) { \
v_ctl_mode = V_CTL_MODE_AUTO_CLIMB; \
v_ctl_climb_setpoint = _climb; \
}
/** Set the vertical mode to fixed throttle with the specified setpoint */
#define NavVerticalThrottleMode(_throttle) { \
v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE; \
nav_throttle_setpoint = _throttle; \
}
#endif /* NAV_H */
sw/ground_segment/tmtc/link.ml
+16 -5
View File
@@ -70,21 +70,32 @@ type status = {
mutable rx_byte : int;
mutable rx_msg : int;
mutable rx_err : int;
mutable ms_since_last_msg : int
}
let statuss = Hashtbl.create 3
let dead_aircraft_time_ms = 5000
let update_status = fun ac_id buf ->
let status =
try Hashtbl.find statuss ac_id with Not_found ->
let s = { last_rx_byte = 0; last_rx_msg = 0; rx_byte = 0; rx_msg = 0; rx_err = 0 } in
let s = { last_rx_byte = 0; last_rx_msg = 0; rx_byte = 0; rx_msg = 0; rx_err = 0; ms_since_last_msg = dead_aircraft_time_ms } in
Hashtbl.add statuss ac_id s;
s in
status.rx_byte <- status.rx_byte + String.length buf;
status.rx_msg <- status.rx_msg + 1;
status.rx_err <- !PprzTransport.nb_err
status.rx_err <- !PprzTransport.nb_err;
status.ms_since_last_msg <- 0
let status_msg_period = 1000 (** ms *)
let live_aircraft = fun ac_id ->
try
let s = Hashtbl.find statuss ac_id in
s.ms_since_last_msg < dead_aircraft_time_ms
with
Not_found -> false
let send_status_msg =
let start = Unix.gettimeofday () in
fun () ->
@@ -95,6 +106,7 @@ let send_status_msg =
and msg_rate = float (status.rx_msg - status.last_rx_msg) /. dt in
status.last_rx_msg <- status.rx_msg;
status.last_rx_byte <- status.rx_byte;
status.ms_since_last_msg <- status.ms_since_last_msg + status_msg_period;
let vs = ["run_time", Pprz.Int t;
"rx_bytes_rate", Pprz.Float byte_rate;
"rx_msgs_rate", Pprz.Float msg_rate;
@@ -117,7 +129,6 @@ let airframes =
let device = get_define dls "DEVICE_TYPE"
and addr = get_define dls "DEVICE_ADDRESS" in
let dl = airborne_device device addr in
printf "%s %b\n%!" (ExtXml.attrib a "ac_id") (dl = Uart);
(ios (ExtXml.attrib a "ac_id"), dl)::r
with
Not_found -> r
@@ -383,7 +394,7 @@ let get_fp = fun device _sender vs ->
let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
List.iter
(fun (dest_id, _) ->
if dest_id <> ac_id then (** Do not send to itself *)
if dest_id <> ac_id && live_aircraft dest_id then (** Do not send to itself *)
try
Debug.trace 'b' (sprintf "ACINFO %d for %d" ac_id dest_id);
let ac_device = airborne_device dest_id airframes device.transport in
@@ -613,7 +624,7 @@ let _ =
if !uplink then begin
(** Listening on Ivy (FIXME: remove the ad hoc messages) *)
ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" (get_fp device));
(***) ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" (get_fp device)); (***)
ignore (Ground_Pprz.message_bind "MOVE_WAYPOINT" (move_wp device));
ignore (Ground_Pprz.message_bind "DL_SETTING" (setting device));
ignore (Ground_Pprz.message_bind "JUMP_TO_BLOCK" (jump_block device));
sw/simulator/diffusion.ml
+1 -1
View File
@@ -8,7 +8,7 @@ type plume = { mutable utm_x : float; mutable utm_y : float; mutable value : int
(* NW of Muret ref *)
let muret = utm_of WGS84 {LL.posn_lat=(Deg>>Rad)43.4624; posn_long=(Deg>>Rad)1.2727}
let source = fun () -> { utm_x = muret.LL.utm_x -. 500.; utm_y = muret.LL.utm_y +. 500.; value = 255}
let source = fun () -> { utm_x = muret.LL.utm_x -. 275.; utm_y = muret.LL.utm_y +. 275.; value = 255}
let available_ids = ref []
let gen_id =
sw/tools/gen_flight_plan.ml
+3 -2
View File
@@ -126,7 +126,7 @@ let get_index_waypoint = fun x l ->
try
string_of_int (List.assoc x l)
with
Not_found -> failwith (sprintf "Unknown waypoint: %s\n" x)
Not_found -> failwith (sprintf "Unknown waypoint: %s" x)
let output_cam_mode = fun x index_of_waypoints ->
let m = try Xml.attrib x "cam_mode" with _ -> "fix" in
@@ -336,7 +336,7 @@ let rec print_stage = fun index_of_waypoints x ->
try
get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints
with
_ ->
ExtXml.Error _ ->
lprintf "waypoints[0].x = %s;\n" (parsed_attrib x "x");
lprintf "waypoints[0].y = %s;\n" (parsed_attrib x "y");
"0"
@@ -747,4 +747,5 @@ let _ =
| Dtd.Prove_error e -> fprintf stderr "%s: DTD error:%s\n%!" !xml_file (Dtd.prove_error e); exit 1
| Dtd.Check_error e -> fprintf stderr "%s: DTD error:%s\n%!" !xml_file (Dtd.check_error e); exit 1
| Dtd.Parse_error e -> fprintf stderr "%s: DTD error:%s\n%!" !xml_file (Dtd.parse_error e); exit 1
| Failure x -> fprintf stderr "%s: %s\n" !xml_file x; exit 1