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

some fixes for jsbsim sim

Browse Source
This commit is contained in:
Felix Ruess
2011-02-27 13:30:21 +01:00
parent 5cf52e44e6
commit ac7deea506
4 changed files with 20 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/servo_switch.xml
+1 -1
View File
@@ -6,7 +6,7 @@
</header> </header>
<init fun="servo_switch_init()"/> <init fun="servo_switch_init()"/>
<periodic fun="servo_switch_periodic()" freq="10."/> <periodic fun="servo_switch_periodic()" freq="10."/>
<makefile> <makefile target="ap|sim|jsbsim">
<!-- these parameters should be set for that module in the airframe file unless you want the defaults <!-- these parameters should be set for that module in the airframe file unless you want the defaults
Servo value in usec Servo value in usec
sw/airborne/arch/sim/jsbsim_transport.c
+12 -7
View File
@@ -2,6 +2,8 @@
#include "jsbsim_hw.h" #include "jsbsim_hw.h"
#include "math/pprz_geodetic_float.h"
#include <stdlib.h> #include <stdlib.h>
#define MOfCm(_x) (((float)(_x))/100.) #define MOfCm(_x) (((float)(_x))/100.)
@@ -47,15 +49,18 @@ void parse_dl_move_wp(char* argv[]) {
float a = MOfCm(atoi(argv[5])); float a = MOfCm(atoi(argv[5]));
/* Computes from (lat, long) in the referenced UTM zone */ /* Computes from (lat, long) in the referenced UTM zone */
float lat = RadOfDeg((float)(atoi(argv[3]) / 1e7)); struct LlaCoor_f lla;
float lon = RadOfDeg((float)(atoi(argv[4]) / 1e7)); lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7));
latlong_utm_of(lat, lon, nav_utm_zone0); lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7));
nav_move_waypoint(wp_id, latlong_utm_x, latlong_utm_y, a); struct UtmCoor_f utm;
utm.zone = nav_utm_zone0;
utm_of_lla_f(&utm, &lla);
nav_move_waypoint(wp_id, utm.east, utm.north, a);
/* Waypoint range is limited. Computes the UTM pos back from the relative /* Waypoint range is limited. Computes the UTM pos back from the relative
coordinates */ coordinates */
latlong_utm_x = waypoints[wp_id].x + nav_utm_east0; utm.east = waypoints[wp_id].x + nav_utm_east0;
latlong_utm_y = waypoints[wp_id].y + nav_utm_north0; utm.north = waypoints[wp_id].y + nav_utm_north0;
DOWNLINK_SEND_WP_MOVED(DefaultChannel,&wp_id, &latlong_utm_x, &latlong_utm_y, &a, &nav_utm_zone0); DOWNLINK_SEND_WP_MOVED(DefaultChannel, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0);
} }
sw/airborne/arch/sim/sim_baro.c
+2 -2
View File
@@ -1,7 +1,7 @@
#include <stdlib.h> #include <stdlib.h>
#include "estimator.h" #include "estimator.h"
#include "subsystems/nav.h" #include "subsystems/nav.h"
#include "gps.h" #include "subsystems/gps.h"
#include "baro_MS5534A.h" #include "baro_MS5534A.h"
bool_t alt_baro_enabled; bool_t alt_baro_enabled;
@@ -31,7 +31,7 @@ void baro_MS5534A_send(void) {
} }
void baro_MS5534A_event_task( void ) { void baro_MS5534A_event_task( void ) {
baro_MS5534A_pressure = baro_MS5534A_ground_pressure - (gps_alt/100.-ground_alt) / 0.08 + ((10.*random()) / RAND_MAX); baro_MS5534A_pressure = baro_MS5534A_ground_pressure - (gps.hmsl/1000.-ground_alt) / 0.08 + ((10.*random()) / RAND_MAX);
baro_MS5534A_temp = 10 + estimator_z; baro_MS5534A_temp = 10 + estimator_z;
baro_MS5534A_available = TRUE; baro_MS5534A_available = TRUE;
} }
sw/airborne/subsystems/navigation/spiral.c
+5 -3
View File
@@ -6,7 +6,7 @@
IMPORTANT: numer of segments has to be larger than 2! IMPORTANT: numer of segments has to be larger than 2!
*/ */
#include "spiral.h" #include "subsystems/navigation/spiral.h"
#include "subsystems/nav.h" #include "subsystems/nav.h"
#include "estimator.h" #include "estimator.h"
@@ -89,6 +89,8 @@ bool_t SpiralNav(void)
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
bool_t InCircle = TRUE; bool_t InCircle = TRUE;
float DistanceStartEstim;
float CircleAlpha;
if(DistanceFromCenter > Spiralradius) if(DistanceFromCenter > Spiralradius)
InCircle = FALSE; InCircle = FALSE;
@@ -126,9 +128,9 @@ bool_t SpiralNav(void)
// if alphamax already reached, increase radius. // if alphamax already reached, increase radius.
//DistanceStartEstim = |Starting position angular - current positon| //DistanceStartEstim = |Starting position angular - current positon|
float DistanceStartEstim = sqrt (((LastCircleX-estimator_x)*(LastCircleX-estimator_x)) DistanceStartEstim = sqrt (((LastCircleX-estimator_x)*(LastCircleX-estimator_x))
+ ((LastCircleY-estimator_y)*(LastCircleY-estimator_y))); + ((LastCircleY-estimator_y)*(LastCircleY-estimator_y)));
float CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad))); CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad)));
if (CircleAlpha >= Alphalimit) { if (CircleAlpha >= Alphalimit) {
LastCircleX = estimator_x; LastCircleX = estimator_x;
LastCircleY = estimator_y; LastCircleY = estimator_y;