Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 16:30:07 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2008-11-24 02:59:53 +00:00
parent f5e6e1afe1
commit 65896a3be1
5 changed files with 207 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/in_progress/fdm/Makefile
+8 -2
View File
@@ -28,6 +28,12 @@ all: fdm_step
fdm_step: fdm_step.c
gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy
clean:
rm -f *~ core *.o *.bak .depend fdm_step
fms_steps_attitude: fms_steps_attitude.c
gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy
clean:
rm -f *~ core *.o *.bak .depend fdm_step fms_steps_attitude
sw/in_progress/fdm/booz2_fms_extern.h
+18
View File
@@ -0,0 +1,18 @@
#ifndef BOOZ2_FMS_EXTERN_H
#define BOOZ2_FMS_EXTERN_H
#define FMS_H_MODE_RATE 0
#define FMS_H_MODE_ATTITUDE 1
#define FMS_H_MODE_SPEED 2
#define FMS_H_MODE_POS 3
#define FMS_ATTITUDE_OF_DEG(_d) ((int32_t)((_d)*0.0000546))
#define FMS_V_MODE_DIRECT 0
#define FMS_V_MODE_CLIMB 1
#define FMS_V_MODE_HEIGHT 2
//#define BOOZ2_SEND_IVY_FMS_COMMAND
#endif /* BOOZ2_FMS_EXTERN_H */
sw/in_progress/fdm/fdm_step.c
-4
View File
@@ -1,10 +1,6 @@
/*
* fdm_step
*
* send joystick control to paparazzi through ivy
*
* based on Force Feedback: Constant Force Stress Test
* Copyright (C) 2001 Oliver Hamann
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
sw/in_progress/fdm/fms_steps_attitude.c
+92
View File
@@ -0,0 +1,92 @@
/*
* fms_steps_attitude
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <glib.h>
#include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h>
#include <inttypes.h>
#include "booz2_fms_extern.h"
#define TIMEOUT_PERIOD 100
#define DEFAULT_AC_ID 150
#define STEP_VAL 2
#define STEP_PERIOD 3
static int aircraft_id = DEFAULT_AC_ID;
static int counter;
static gboolean periodic(gpointer data __attribute__ ((unused)));
void parse_args(int argc, char * argv[])
{
int i;
for (i = 1; i < argc; i++) {
if (!strcmp(argv[i],"-a") && i<argc-1) aircraft_id = atoi(argv[++i]);
else if (!strcmp(argv[i],"-h")) goto l_help;
}
return;
l_help:
printf("Usage:\n");
printf(" %s <option> [<option>...]\n",argv[0]);
printf("Options:\n");
printf(" -a <int> aircraft id (default: %d)\n",DEFAULT_AC_ID);
printf(" -h display this help\n");
exit(1);
}
static gboolean periodic(gpointer data __attribute__ ((unused))) {
counter++;
uint8_t h_mode = FMS_H_MODE_ATTITUDE;
uint8_t v_mode = FMS_V_MODE_DIRECT;
int32_t pitch = FMS_ATTITUDE_OF_DEG(0);
int32_t roll = FMS_ATTITUDE_OF_DEG(STEP_VAL);
int32_t yaw = FMS_ATTITUDE_OF_DEG(0);
int32_t unused = 0;
if ((counter%(2*STEP_PERIOD)) >= STEP_PERIOD) roll = -roll;
IvySendMsg("dl BOOZ2_FMS_COMMAND %d %d %d %d,%d,%d %d", aircraft_id, h_mode, v_mode, roll, pitch, yaw, unused);
return TRUE;
}
int main ( int argc, char** argv) {
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
parse_args(argc, argv);
IvyInit ("IvyBooz2FmsAttStep", "IvyBooz2FmsAttStep READY", NULL, NULL, NULL, NULL);
IvyStart("127.255.255.255");
g_timeout_add(TIMEOUT_PERIOD, periodic, NULL);
g_main_loop_run(ml);
return 0;
}
sw/in_progress/fdm/fms_steps_position.c
+89
View File
@@ -0,0 +1,89 @@
/*
* fdm_step_position
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <glib.h>
#include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h>
#define TIMEOUT_PERIOD 100
#define DEFAULT_AC_ID 150
static int aircraft_id = DEFAULT_AC_ID;
static int counter;
static gboolean periodic(gpointer data __attribute__ ((unused)));
static void on_FMS_info(IvyClientPtr app, void *user_data, int argc, char *argv[]);
void parse_args(int argc, char * argv[])
{
int i;
for (i = 1; i < argc; i++) {
if (!strcmp(argv[i],"-a") && i<argc-1) aircraft_id = atoi(argv[++i]);
else if (!strcmp(argv[i],"-h")) goto l_help;
}
return;
l_help:
printf("Usage:\n");
printf(" %s <option> [<option>...]\n",argv[0]);
printf("Options:\n");
printf(" -a <int> aircraft id (default: %d)\n",DEFAULT_AC_ID);
printf(" -h display this help\n");
exit(1);
}
static void on_FMS_state(IvyClientPtr app, void *user_data, int argc, char *argv[]){
// guint ac_id = atoi(argv[0]);
// float estimator_phi = atof(argv[1]);
}
#define STEP_VAL 32
static gboolean periodic(gpointer data __attribute__ ((unused))) {
int roll, pitch;
counter++;
pitch = 0;
roll = STEP_VAL;
if ((counter%6) >=3) roll = -roll;
IvySendMsg("dl COMMANDS_RAW %d %d,%d", aircraft_id, roll, pitch);
return TRUE;
}
int main ( int argc, char** argv) {
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
parse_args(argc, argv);
IvyInit ("IvyFdmStep", "IvyFdmStep READY", NULL, NULL, NULL, NULL);
IvyStart("127.255.255.255");
g_timeout_add(TIMEOUT_PERIOD, periodic, NULL);
g_main_loop_run(ml);
return 0;
}