Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 12:57:27 +08:00
Code Issues Actions 28 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>
<init fun="servo_switch_init()"/>
<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
Servo value in usec
sw/airborne/arch/sim/jsbsim_transport.c
+12 -7
View File
@@ -2,6 +2,8 @@
#include "jsbsim_hw.h"
#include "math/pprz_geodetic_float.h"
#include <stdlib.h>
#define MOfCm(_x) (((float)(_x))/100.)
@@ -47,15 +49,18 @@ void parse_dl_move_wp(char* argv[]) {
float a = MOfCm(atoi(argv[5]));
/* Computes from (lat, long) in the referenced UTM zone */
float lat = RadOfDeg((float)(atoi(argv[3]) / 1e7));
float lon = RadOfDeg((float)(atoi(argv[4]) / 1e7));
latlong_utm_of(lat, lon, nav_utm_zone0);
nav_move_waypoint(wp_id, latlong_utm_x, latlong_utm_y, a);
struct LlaCoor_f lla;
lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7));
lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7));
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
coordinates */
latlong_utm_x = waypoints[wp_id].x + nav_utm_east0;
latlong_utm_y = waypoints[wp_id].y + nav_utm_north0;
DOWNLINK_SEND_WP_MOVED(DefaultChannel,&wp_id, &latlong_utm_x, &latlong_utm_y, &a, &nav_utm_zone0);
utm.east = waypoints[wp_id].x + nav_utm_east0;
utm.north = waypoints[wp_id].y + nav_utm_north0;
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 "estimator.h"
#include "subsystems/nav.h"
#include "gps.h"
#include "subsystems/gps.h"
#include "baro_MS5534A.h"
bool_t alt_baro_enabled;
@@ -31,7 +31,7 @@ void baro_MS5534A_send(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_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!
*/
#include "spiral.h"
#include "subsystems/navigation/spiral.h"
#include "subsystems/nav.h"
#include "estimator.h"
@@ -89,6 +89,8 @@ bool_t SpiralNav(void)
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
bool_t InCircle = TRUE;
float DistanceStartEstim;
float CircleAlpha;
if(DistanceFromCenter > Spiralradius)
InCircle = FALSE;
@@ -126,9 +128,9 @@ bool_t SpiralNav(void)
// if alphamax already reached, increase radius.
//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)));
float CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad)));
CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad)));
if (CircleAlpha >= Alphalimit) {
LastCircleX = estimator_x;
LastCircleY = estimator_y;