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

[nps] reformat files

Browse Source
This commit is contained in:
Felix Ruess
2015-08-06 21:33:06 +02:00
parent 47ce1339ed
commit 0f4a2034c9
24 changed files with 874 additions and 760 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/nps/nps_atmosphere.c
+4 -4
View File
@@ -65,8 +65,8 @@ void nps_atmosphere_set_wind_speed(double speed)
void nps_atmosphere_set_wind_dir(double dir)
{
/* normalize dir to 0-2Pi */
while (dir < 0.0) dir += 2 * M_PI;
while (dir >= 2 * M_PI) dir -= 2 * M_PI;
while (dir < 0.0) { dir += 2 * M_PI; }
while (dir >= 2 * M_PI) { dir -= 2 * M_PI; }
nps_atmosphere.wind_dir = dir;
/* recalc wind in north and east */
@@ -84,8 +84,8 @@ void nps_atmosphere_set_wind_ned(double wind_north, double wind_east, double win
double dir = atan2(-wind_east, -wind_north);
/* normalize dir to 0-2Pi */
while (dir < 0.0) dir += 2 * M_PI;
while (dir >= 2 * M_PI) dir -= 2 * M_PI;
while (dir < 0.0) { dir += 2 * M_PI; }
while (dir >= 2 * M_PI) { dir -= 2 * M_PI; }
nps_atmosphere.wind_dir = dir;
}
sw/simulator/nps/nps_autopilot.h
+1 -1
View File
@@ -37,7 +37,7 @@ extern bool_t nps_bypass_ins;
extern void sim_overwrite_ahrs(void);
extern void sim_overwrite_ins(void);
extern void nps_autopilot_init(enum NpsRadioControlType type, int num_script, char* js_dev);
extern void nps_autopilot_init(enum NpsRadioControlType type, int num_script, char *js_dev);
extern void nps_autopilot_run_step(double time);
extern void nps_autopilot_run_systime_step(void);
sw/simulator/nps/nps_autopilot_fixedwing.c
+28 -21
View File
@@ -72,7 +72,8 @@ bool_t nps_bypass_ins;
#error NPS does not currently support dual processor simulation for FBW and AP on fixedwing!
#endif
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) {
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
{
autopilot.launch = FALSE;
autopilot.datalink_enabled = TRUE;
@@ -88,14 +89,16 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha
}
void nps_autopilot_run_systime_step( void ) {
void nps_autopilot_run_systime_step(void)
{
sys_tick_handler();
}
#include <stdio.h>
#include "subsystems/gps.h"
void nps_autopilot_run_step(double time) {
void nps_autopilot_run_step(double time)
{
nps_electrical_run_step(time);
@@ -116,7 +119,7 @@ void nps_autopilot_run_step(double time) {
imu_feed_mag();
Fbw(event_task);
Ap(event_task);
}
}
if (nps_sensors_baro_available()) {
float pressure = (float) sensors.baro.value;
@@ -144,27 +147,28 @@ void nps_autopilot_run_step(double time) {
/* scale final motor commands to 0-1 for feeding the fdm */
#ifdef NPS_ACTUATOR_NAMES
PRINT_CONFIG_MSG("actuators for JSBSim explicitly set.")
PRINT_CONFIG_VAR(NPS_COMMANDS_NB)
PRINT_CONFIG_MSG("actuators for JSBSim explicitly set.")
PRINT_CONFIG_VAR(NPS_COMMANDS_NB)
//PRINT_CONFIG_VAR(NPS_ACTUATOR_NAMES)
for (uint8_t i=0; i < NPS_COMMANDS_NB; i++)
autopilot.commands[i] = (double)commands[i]/MAX_PPRZ;
for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
autopilot.commands[i] = (double)commands[i] / MAX_PPRZ;
}
// hack: invert pitch to fit most JSBSim models
autopilot.commands[COMMAND_PITCH] = -(double)commands[COMMAND_PITCH]/MAX_PPRZ;
autopilot.commands[COMMAND_PITCH] = -(double)commands[COMMAND_PITCH] / MAX_PPRZ;
#else
PRINT_CONFIG_MSG("Using throttle, roll, pitch, yaw commands instead of explicit actuators.")
PRINT_CONFIG_VAR(COMMAND_THROTTLE)
PRINT_CONFIG_VAR(COMMAND_ROLL)
PRINT_CONFIG_VAR(COMMAND_PITCH)
PRINT_CONFIG_MSG("Using throttle, roll, pitch, yaw commands instead of explicit actuators.")
PRINT_CONFIG_VAR(COMMAND_THROTTLE)
PRINT_CONFIG_VAR(COMMAND_ROLL)
PRINT_CONFIG_VAR(COMMAND_PITCH)
autopilot.commands[COMMAND_THROTTLE] = (double)commands[COMMAND_THROTTLE]/MAX_PPRZ;
autopilot.commands[COMMAND_ROLL] = (double)commands[COMMAND_ROLL]/MAX_PPRZ;
autopilot.commands[COMMAND_THROTTLE] = (double)commands[COMMAND_THROTTLE] / MAX_PPRZ;
autopilot.commands[COMMAND_ROLL] = (double)commands[COMMAND_ROLL] / MAX_PPRZ;
// hack: invert pitch to fit most JSBSim models
autopilot.commands[COMMAND_PITCH] = -(double)commands[COMMAND_PITCH]/MAX_PPRZ;
autopilot.commands[COMMAND_PITCH] = -(double)commands[COMMAND_PITCH] / MAX_PPRZ;
#ifdef COMMAND_YAW
PRINT_CONFIG_VAR(COMMAND_YAW)
autopilot.commands[COMMAND_YAW] = (double)commands[COMMAND_YAW]/MAX_PPRZ;
PRINT_CONFIG_VAR(COMMAND_YAW)
autopilot.commands[COMMAND_YAW] = (double)commands[COMMAND_YAW] / MAX_PPRZ;
#else
autopilot.commands[3] = 0.;
#endif
@@ -172,8 +176,9 @@ PRINT_CONFIG_VAR(COMMAND_YAW)
// do the launch when clicking launch in GCS
autopilot.launch = launch && !kill_throttle;
if (!launch)
if (!launch) {
autopilot.commands[COMMAND_THROTTLE] = 0;
}
// hack to reset datalink_time, since we don't use actual dl_parse_msg
if (autopilot.datalink_enabled) {
@@ -181,7 +186,8 @@ PRINT_CONFIG_VAR(COMMAND_YAW)
}
}
void sim_overwrite_ahrs(void) {
void sim_overwrite_ahrs(void)
{
struct FloatQuat quat_f;
QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
@@ -193,7 +199,8 @@ void sim_overwrite_ahrs(void) {
}
void sim_overwrite_ins(void) {
void sim_overwrite_ins(void)
{
struct NedCoor_f ltp_pos;
VECT3_COPY(ltp_pos, fdm.ltpprz_pos);
sw/simulator/nps/nps_autopilot_rotorcraft.c
+13 -7
View File
@@ -61,7 +61,8 @@ bool_t nps_bypass_ins;
#error "NPS_COMMANDS_NB does not match MOTOR_MIXING_NB_MOTOR!"
#endif
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) {
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
{
autopilot.launch = TRUE;
autopilot.datalink_enabled = TRUE;
@@ -75,14 +76,16 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha
}
void nps_autopilot_run_systime_step( void ) {
void nps_autopilot_run_systime_step(void)
{
sys_tick_handler();
}
#include <stdio.h>
#include "subsystems/gps.h"
void nps_autopilot_run_step(double time) {
void nps_autopilot_run_step(double time)
{
nps_electrical_run_step(time);
@@ -137,8 +140,9 @@ void nps_autopilot_run_step(double time) {
handle_periodic_tasks();
/* scale final motor commands to 0-1 for feeding the fdm */
for (uint8_t i=0; i < NPS_COMMANDS_NB; i++)
autopilot.commands[i] = (double)motor_mixing.commands[i]/MAX_PPRZ;
for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
autopilot.commands[i] = (double)motor_mixing.commands[i] / MAX_PPRZ;
}
// hack to reset datalink_time, since we don't use actual dl_parse_msg
if (autopilot.datalink_enabled) {
@@ -147,7 +151,8 @@ void nps_autopilot_run_step(double time) {
}
void sim_overwrite_ahrs(void) {
void sim_overwrite_ahrs(void)
{
struct FloatQuat quat_f;
QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
@@ -159,7 +164,8 @@ void sim_overwrite_ahrs(void) {
}
void sim_overwrite_ins(void) {
void sim_overwrite_ins(void)
{
struct NedCoor_f ltp_pos;
VECT3_COPY(ltp_pos, fdm.ltpprz_pos);
sw/simulator/nps/nps_electrical.c
+4 -2
View File
@@ -30,7 +30,8 @@
struct NpsElectrical nps_electrical;
void nps_electrical_init(void) {
void nps_electrical_init(void)
{
#ifdef MAX_BAT_LEVEL
nps_electrical.supply_voltage = MAX_BAT_LEVEL;
@@ -40,7 +41,8 @@ void nps_electrical_init(void) {
}
void nps_electrical_run_step(double time __attribute__ ((unused))) {
void nps_electrical_run_step(double time __attribute__((unused)))
{
// todo: auto-decrease bat voltage
electrical.vsupply = nps_electrical.supply_voltage * 10;
}
sw/simulator/nps/nps_fdm.h
+1 -1
View File
@@ -123,7 +123,7 @@ struct NpsFdm {
extern struct NpsFdm fdm;
extern void nps_fdm_init(double dt);
extern void nps_fdm_run_step(bool_t launch, double* commands, int commands_nb);
extern void nps_fdm_run_step(bool_t launch, double *commands, int commands_nb);
extern void nps_fdm_set_wind(double speed, double dir);
extern void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down);
extern void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity);
sw/simulator/nps/nps_fdm_crrcsim.c
+98 -97
View File
@@ -74,7 +74,7 @@
// uNAV packet length definition
#define IMU_PACKET_LENGTH 51
#define GPS_PACKET_LENGTH 86
#define GPS_PACKET_LENGTH 86
#define AHRS_PACKET_LENGTH 93
#define FULL_PACKET_SIZE 93
@@ -85,9 +85,9 @@ struct NpsFdm fdm;
#define INPUT_BUFFER_SIZE (3*FULL_PACKET_SIZE)
// Input buffer
struct inputbuf {
byte buf[INPUT_BUFFER_SIZE];
byte start;
int length;
byte buf[INPUT_BUFFER_SIZE];
byte start;
int length;
};
// Socket structure
@@ -104,18 +104,19 @@ static struct _crrcsim crrcsim;
static struct LtpDef_d ltpdef;
// static functions declaration
static void open_udp(char* host, int port, int blocking);
static void inputbuf_init(struct inputbuf* c);
static void read_into_buffer(struct _crrcsim* io);
static void open_udp(char *host, int port, int blocking);
static void inputbuf_init(struct inputbuf *c);
static void read_into_buffer(struct _crrcsim *io);
static void init_ltp(void);
static int get_msg(struct _crrcsim* io, byte* data_buffer);
static void decode_imupacket(struct NpsFdm * fdm, byte* buffer);
static void decode_gpspacket(struct NpsFdm * fdm, byte* buffer);
static void decode_ahrspacket(struct NpsFdm * fdm, byte* buffer);
static void send_servo_cmd(struct _crrcsim* io, double* commands);
static int get_msg(struct _crrcsim *io, byte *data_buffer);
static void decode_imupacket(struct NpsFdm *fdm, byte *buffer);
static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer);
static void decode_ahrspacket(struct NpsFdm *fdm, byte *buffer);
static void send_servo_cmd(struct _crrcsim *io, double *commands);
// NPS FDM interface
void nps_fdm_init(double dt) {
void nps_fdm_init(double dt)
{
fdm.init_dt = dt;
fdm.curr_dt = dt;
fdm.nan_count = 0;
@@ -126,13 +127,12 @@ void nps_fdm_init(double dt) {
inputbuf_init(&crrcsim.buf);
printf("Starting to connect to CRRCsim server.\n");
open_udp((char*)(NPS_CRRCSIM_HOST_IP), NPS_CRRCSIM_HOST_PORT, UDP_NONBLOCKING);
open_udp((char *)(NPS_CRRCSIM_HOST_IP), NPS_CRRCSIM_HOST_PORT, UDP_NONBLOCKING);
if (crrcsim.socket < 0) {
printf("Connection to CRRCsim failed\n");
exit(0);
}
else {
} else {
printf("Connection to CRRCsim succed\n");
}
@@ -141,7 +141,8 @@ void nps_fdm_init(double dt) {
send_servo_cmd(&crrcsim, zero);
}
void nps_fdm_run_step(bool_t launch __attribute__((unused)), double* commands, int commands_nb) {
void nps_fdm_run_step(bool_t launch __attribute__((unused)), double *commands, int commands_nb)
{
// read state
if (get_msg(&crrcsim, crrcsim.data_buffer) <= 0) {
return; // nothing on the socket
@@ -151,8 +152,7 @@ void nps_fdm_run_step(bool_t launch __attribute__((unused)), double* commands, i
send_servo_cmd(&crrcsim, commands);
//printf("new data %c %c\n", crrcsim.data_buffer[2], crrcsim.data_buffer[33]);
switch (crrcsim.data_buffer[2])
{
switch (crrcsim.data_buffer[2]) {
case 'S': /* IMU packet without GPS */
decode_imupacket(&fdm, crrcsim.data_buffer);
break;
@@ -165,7 +165,7 @@ void nps_fdm_run_step(bool_t launch __attribute__((unused)), double* commands, i
*check GPS data packet
******************************************/
//if(data_buffer[31]==(byte)0x55 && data_buffer[32]==(byte)0x55 && data_buffer[33]=='G')
if(crrcsim.data_buffer[33]=='G') {
if (crrcsim.data_buffer[33] == 'G') {
decode_gpspacket(&fdm, &crrcsim.data_buffer[31]);
}
break;
@@ -177,10 +177,10 @@ void nps_fdm_run_step(bool_t launch __attribute__((unused)), double* commands, i
/******************************************
*check GPS data packet
******************************************/
if(crrcsim.data_buffer[33]=='G') {
if (crrcsim.data_buffer[33] == 'G') {
decode_gpspacket(&fdm, &crrcsim.data_buffer[31]);
}
if(crrcsim.data_buffer[66]=='A') {
if (crrcsim.data_buffer[66] == 'A') {
decode_ahrspacket(&fdm, &crrcsim.data_buffer[66]);
}
break;
@@ -211,7 +211,7 @@ void nps_fdm_set_turbulence(double wind_speed __attribute__((unused)),
** Open and configure UDP connection
****************************************************************************/
static void open_udp(char* host, int port, int blocking)
static void open_udp(char *host, int port, int blocking)
{
int flags;
@@ -225,25 +225,24 @@ static void open_udp(char* host, int port, int blocking)
flags = fcntl(crrcsim.socket, F_GETFL, 0);
fcntl(crrcsim.socket, F_SETFL, flags | O_NONBLOCK);
if (connect(crrcsim.socket,(struct sockaddr*)&crrcsim.addr,sizeof(crrcsim.addr)) < 0) {
if (connect(crrcsim.socket, (struct sockaddr *)&crrcsim.addr, sizeof(crrcsim.addr)) < 0) {
close(crrcsim.socket);
crrcsim.socket = -1;
}
if(crrcsim.socket != -1 && blocking == UDP_BLOCKING)
{
if (crrcsim.socket != -1 && blocking == UDP_BLOCKING) {
//restore
fcntl(crrcsim.socket, F_SETFL, flags);
}
}
static void inputbuf_init(struct inputbuf* c)
static void inputbuf_init(struct inputbuf *c)
{
c->start = 0;
c->length = 0;
}
static void read_into_buffer(struct _crrcsim* io)
static void read_into_buffer(struct _crrcsim *io)
{
struct inputbuf *c = &io->buf;
int res;
@@ -257,13 +256,14 @@ static void read_into_buffer(struct _crrcsim* io)
}
}
static void init_ltp(void) {
static void init_ltp(void)
{
struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = RadOfDeg((double)NAV_LAT0/1e7);
llh_nav0.lon = RadOfDeg((double)NAV_LON0/1e7);
llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7);
llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7);
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = (NAV_ALT0 + NAV_MSL0)/1000.;
llh_nav0.alt = (NAV_ALT0 + NAV_MSL0) / 1000.;
struct EcefCoor_d ecef_nav0;
ecef_of_lla_d(&ecef_nav0, &llh_nav0);
@@ -287,7 +287,7 @@ static void init_ltp(void) {
}
static int get_msg(struct _crrcsim* io, byte* data_buffer)
static int get_msg(struct _crrcsim *io, byte *data_buffer)
{
struct inputbuf *c = &io->buf;
int count = 0;
@@ -300,19 +300,19 @@ static int get_msg(struct _crrcsim* io, byte* data_buffer)
/*********************************************************************
* Find start of packet: the header (2 bytes) starts with 0x5555
*********************************************************************/
while(c->length >= 4 && (c->buf[c->start] != (byte)0x55 || c->buf[(byte)(c->start + 1)] != (byte)0x55)) {
while (c->length >= 4 && (c->buf[c->start] != (byte)0x55 || c->buf[(byte)(c->start + 1)] != (byte)0x55)) {
c->start++;
c->length--;
}
if(c->length < 4)
if (c->length < 4) {
return count;
}
/*********************************************************************
* Read packet contents
*********************************************************************/
packet_len = 0;
switch (c->buf[(byte)(c->start + 2)])
{
switch (c->buf[(byte)(c->start + 2)]) {
case 'S':
packet_len = IMU_PACKET_LENGTH;
break;
@@ -329,33 +329,34 @@ static int get_msg(struct _crrcsim* io, byte* data_buffer)
break;
}
if(packet_len > 0 && c->length < packet_len)
return count; // not enough data
if(packet_len > 0) {
if (packet_len > 0 && c->length < packet_len) {
return count; // not enough data
}
if (packet_len > 0) {
byte ib;
word rcvchecksum = 0;
word sum = 0;
for(i = 2, ib = c->start + (byte)2; i < packet_len - 2; i++, ib++)
for (i = 2, ib = c->start + (byte)2; i < packet_len - 2; i++, ib++) {
sum += c->buf[ib];
}
rcvchecksum = c->buf[ib++] << 8;
rcvchecksum = rcvchecksum | c->buf[ib++];
if(rcvchecksum != sum) {
if (rcvchecksum != sum) {
packet_len = 0;
printf("checksum error\n");
}
}
// fill data buffer or go to next bytes
if(packet_len > 0) {
for(i = 0; i < packet_len; i++) {
if (packet_len > 0) {
for (i = 0; i < packet_len; i++) {
data_buffer[i] = c->buf[c->start];
c->start++;
c->length--;
}
count++;
}
else {
} else {
c->start += 3;
c->length -= 3;
}
@@ -374,21 +375,21 @@ static int get_msg(struct _crrcsim* io, byte* data_buffer)
/***************************************************************************************
*decode the gps data packet
***************************************************************************************/
static void decode_gpspacket(struct NpsFdm * fdm, byte* buffer)
static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer)
{
/* gps velocity (1e2 m/s to m/s */
struct NedCoor_d vel;
vel.x = (double)LongOfBuf(buffer,3)*1.0e-2;
vel.y = (double)LongOfBuf(buffer,7)*1.0e-2;
vel.z = (double)LongOfBuf(buffer,11)*1.0e-2;
vel.x = (double)LongOfBuf(buffer, 3) * 1.0e-2;
vel.y = (double)LongOfBuf(buffer, 7) * 1.0e-2;
vel.z = (double)LongOfBuf(buffer, 11) * 1.0e-2;
fdm->ltp_ecef_vel = vel;
ecef_of_ned_vect_d(&fdm->ecef_ecef_vel, &ltpdef, &vel);
/* gps position (1e7 deg to rad and 1e3 m to m) */
struct LlaCoor_d pos;
pos.lon=(double)LongOfBuf(buffer,15)*1.74533e-9;
pos.lat=(double)LongOfBuf(buffer,19)*1.74533e-9;
pos.alt=(double)LongOfBuf(buffer,23)*1.0e-3;
pos.lon = (double)LongOfBuf(buffer, 15) * 1.74533e-9;
pos.lat = (double)LongOfBuf(buffer, 19) * 1.74533e-9;
pos.alt = (double)LongOfBuf(buffer, 23) * 1.0e-3;
pos.lat += ltpdef.lla.lat;
pos.lon += ltpdef.lla.lon;
@@ -396,10 +397,10 @@ static void decode_gpspacket(struct NpsFdm * fdm, byte* buffer)
fdm->lla_pos = pos;
ecef_of_lla_d(&fdm->ecef_pos, &pos);
fdm->hmsl = pos.alt - NAV_MSL0/1000.;
fdm->hmsl = pos.alt - NAV_MSL0 / 1000.;
/* gps time */
fdm->time = (double)UShortOfBuf(buffer,27);
fdm->time = (double)UShortOfBuf(buffer, 27);
/* in LTP pprz */
ned_of_ecef_point_d(&fdm->ltpprz_pos, &ltpdef, &fdm->ecef_pos);
@@ -408,44 +409,44 @@ static void decode_gpspacket(struct NpsFdm * fdm, byte* buffer)
#if NPS_CRRCSIM_DEBUG
printf("decode gps | pos %f %f %f | vel %f %f %f | time %f\n",
57.3*fdm->lla_pos.lat,
57.3*fdm->lla_pos.lon,
fdm->lla_pos.alt,
fdm->ltp_ecef_vel.x,
fdm->ltp_ecef_vel.y,
fdm->ltp_ecef_vel.z,
fdm->time);
57.3 * fdm->lla_pos.lat,
57.3 * fdm->lla_pos.lon,
fdm->lla_pos.alt,
fdm->ltp_ecef_vel.x,
fdm->ltp_ecef_vel.y,
fdm->ltp_ecef_vel.z,
fdm->time);
#endif
}
/***************************************************************************************
*decode the ahrs data packet
***************************************************************************************/
static void decode_ahrspacket(struct NpsFdm * fdm, byte* buffer)
static void decode_ahrspacket(struct NpsFdm *fdm, byte *buffer)
{
/* euler angles (0.9387340515702713e04 rad to rad) */
fdm->ltp_to_body_eulers.phi = (double)ShortOfBuf(buffer,1)*0.000106526 - NPS_CRRCSIM_ROLL_NEUTRAL;
fdm->ltp_to_body_eulers.theta = (double)ShortOfBuf(buffer,3)*0.000106526 - NPS_CRRCSIM_PITCH_NEUTRAL;
fdm->ltp_to_body_eulers.psi = (double)ShortOfBuf(buffer,5)*0.000106526;
fdm->ltp_to_body_eulers.phi = (double)ShortOfBuf(buffer, 1) * 0.000106526 - NPS_CRRCSIM_ROLL_NEUTRAL;
fdm->ltp_to_body_eulers.theta = (double)ShortOfBuf(buffer, 3) * 0.000106526 - NPS_CRRCSIM_PITCH_NEUTRAL;
fdm->ltp_to_body_eulers.psi = (double)ShortOfBuf(buffer, 5) * 0.000106526;
DOUBLE_QUAT_OF_EULERS(fdm->ltp_to_body_quat, fdm->ltp_to_body_eulers);
#if NPS_CRRCSIM_DEBUG
printf("decode ahrs %f %f %f\n",
fdm->ltp_to_body_eulers.phi*57.3,
fdm->ltp_to_body_eulers.theta*57.3,
fdm->ltp_to_body_eulers.psi*57.3);
fdm->ltp_to_body_eulers.phi * 57.3,
fdm->ltp_to_body_eulers.theta * 57.3,
fdm->ltp_to_body_eulers.psi * 57.3);
#endif
}
/***************************************************************************************
*decode the imu data packet
***************************************************************************************/
void decode_imupacket(struct NpsFdm * fdm, byte* buffer)
void decode_imupacket(struct NpsFdm *fdm, byte *buffer)
{
/* acceleration (0.1670132517315938e04 m/s^2 to m/s^2) */
fdm->body_accel.x = (double)ShortOfBuf(buffer,3)*5.98755e-04;
fdm->body_accel.y = (double)ShortOfBuf(buffer,5)*5.98755e-04;
fdm->body_accel.z = (double)ShortOfBuf(buffer,7)*5.98755e-04;
fdm->body_accel.x = (double)ShortOfBuf(buffer, 3) * 5.98755e-04;
fdm->body_accel.y = (double)ShortOfBuf(buffer, 5) * 5.98755e-04;
fdm->body_accel.z = (double)ShortOfBuf(buffer, 7) * 5.98755e-04;
/* since we don't get acceleration in ecef frame, use ECI for now */
fdm->body_ecef_accel.x = fdm->body_accel.x;
@@ -454,9 +455,9 @@ void decode_imupacket(struct NpsFdm * fdm, byte* buffer)
/* angular rate (0.9387340515702713e4 rad/s to rad/s) */
fdm->body_inertial_rotvel.p = (double)ShortOfBuf(buffer,9)*1.06526e-04;
fdm->body_inertial_rotvel.q = (double)ShortOfBuf(buffer,11)*1.06526e-04;
fdm->body_inertial_rotvel.r = (double)ShortOfBuf(buffer,13)*1.06526e-04;
fdm->body_inertial_rotvel.p = (double)ShortOfBuf(buffer, 9) * 1.06526e-04;
fdm->body_inertial_rotvel.q = (double)ShortOfBuf(buffer, 11) * 1.06526e-04;
fdm->body_inertial_rotvel.r = (double)ShortOfBuf(buffer, 13) * 1.06526e-04;
/* since we don't get angular velocity in ECEF frame, use the rotvel in ECI frame for now */
fdm->body_ecef_rotvel.p = fdm->body_inertial_rotvel.p;
@@ -474,12 +475,12 @@ void decode_imupacket(struct NpsFdm * fdm, byte* buffer)
#if NPS_CRRCSIM_DEBUG
printf("decode imu | accel %f %f %f | gyro %f %f %f\n",
fdm->body_accel.x,
fdm->body_accel.y,
fdm->body_accel.z,
fdm->body_inertial_rotvel.p,
fdm->body_inertial_rotvel.q,
fdm->body_inertial_rotvel.r);
fdm->body_accel.x,
fdm->body_accel.y,
fdm->body_accel.z,
fdm->body_inertial_rotvel.p,
fdm->body_inertial_rotvel.q,
fdm->body_inertial_rotvel.r);
#endif
}
@@ -491,30 +492,30 @@ void decode_imupacket(struct NpsFdm * fdm, byte* buffer)
/***************************************************************************************
* send servo command over udp
***************************************************************************************/
static void send_servo_cmd(struct _crrcsim* io, double* commands)
static void send_servo_cmd(struct _crrcsim *io, double *commands)
{
//cnt_cmd[1] = ch1:elevator, cnt_cmd[0] = ch0:aileron, cnt_cmd[2] = ch2:throttle
word cnt_cmd[3];
byte data[24]={0,};
byte data[24] = {0,};
short i = 0;
word sum=0;
word sum = 0;
word roll = (word)((65535/4)*commands[NPS_CRRCSIM_COMMAND_ROLL] + (65536/2));
word pitch = (word)((65535/4)*commands[NPS_CRRCSIM_COMMAND_PITCH] + (65536/2));
word roll = (word)((65535 / 4) * commands[NPS_CRRCSIM_COMMAND_ROLL] + (65536 / 2));
word pitch = (word)((65535 / 4) * commands[NPS_CRRCSIM_COMMAND_PITCH] + (65536 / 2));
cnt_cmd[0] = roll;
cnt_cmd[1] = -pitch;
cnt_cmd[2] = (word)(65535*commands[NPS_CRRCSIM_COMMAND_THROTTLE]);
cnt_cmd[2] = (word)(65535 * commands[NPS_CRRCSIM_COMMAND_THROTTLE]);
#if NPS_CRRCSIM_DEBUG
printf("send servo %f %f %f | %d %d %d | %d %d\n",
commands[0],
commands[1],
commands[2],
cnt_cmd[0],
cnt_cmd[1],
cnt_cmd[2],
roll, pitch);
commands[0],
commands[1],
commands[2],
cnt_cmd[0],
cnt_cmd[1],
cnt_cmd[2],
roll, pitch);
#endif
data[0] = 0x55;
@@ -535,14 +536,14 @@ static void send_servo_cmd(struct _crrcsim* io, double* commands)
//checksum
sum = 0xa6; //0x53+0x53
for(i=4;i<22;i++) sum += data[i];
for (i = 4; i < 22; i++) { sum += data[i]; }
data[22] = (byte)(sum >> 8);
data[23] = (byte)sum;
//sendout the command packet
if (io->socket >= 0) {
send(io->socket, (char*)data, 24, MSG_NOSIGNAL);
send(io->socket, (char *)data, 24, MSG_NOSIGNAL);
}
}
sw/simulator/nps/nps_fdm_jsbsim.cpp
+155 -139
View File
File diff suppressed because it is too large Load Diff
sw/simulator/nps/nps_flightgear.c
+22 -20
View File
@@ -22,9 +22,9 @@ static struct {
} flightgear;
double htond (double x)
double htond(double x)
{
int * p = (int*)&x;
int *p = (int *)&x;
int tmp = p[0];
p[0] = htonl(p[1]);
p[1] = htonl(tmp);
@@ -33,20 +33,21 @@ double htond (double x)
}
float htonf (float x)
float htonf(float x)
{
int * p = (int *)&x;
int *p = (int *)&x;
*p = htonl(*p);
return x;
}
void nps_flightgear_init(const char* host, unsigned int port, unsigned int time_offset) {
void nps_flightgear_init(const char *host, unsigned int port, unsigned int time_offset)
{
int so_reuseaddr = 1;
struct protoent * pte = getprotobyname("UDP");
flightgear.socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto);
struct protoent *pte = getprotobyname("UDP");
flightgear.socket = socket(PF_INET, SOCK_DGRAM, pte->p_proto);
setsockopt(flightgear.socket, SOL_SOCKET, SO_REUSEADDR,
&so_reuseaddr, sizeof(so_reuseaddr));
&so_reuseaddr, sizeof(so_reuseaddr));
flightgear.addr.sin_family = PF_INET;
flightgear.addr.sin_port = htons(port);
flightgear.addr.sin_addr.s_addr = inet_addr(host);
@@ -62,7 +63,8 @@ void nps_flightgear_init(const char* host, unsigned int port, unsigned int time
* For visualization with moving surfaces (elevator, propeller etc).
* start fgfs with --native-fdm=socket... option
*/
void nps_flightgear_send_fdm() {
void nps_flightgear_send_fdm()
{
struct FGNetFDM fgfdm;
memset(&fgfdm, 0, sizeof(fgfdm));
@@ -93,12 +95,11 @@ void nps_flightgear_send_fdm() {
// Engine
fgfdm.num_engines = htonl(fdm.num_engines);
for(int k=0; k<FG_NET_FDM_MAX_ENGINES; k++) {
for (int k = 0; k < FG_NET_FDM_MAX_ENGINES; k++) {
// Temprary hack to clearly show when the engine is running
if (fdm.eng_state[k] == 1) {
fgfdm.rpm[k] = htonf(fdm.rpm[k]);
}
else {
} else {
fgfdm.rpm[k] = htonf(0.0);
}
fgfdm.eng_state[k] = htonl(fdm.eng_state[k]);
@@ -112,11 +113,11 @@ void nps_flightgear_send_fdm() {
fgfdm.left_flap = htonf(fdm.flap);
fgfdm.right_flap = htonf(fdm.flap);
if (sendto(flightgear.socket, (char*)(&fgfdm), sizeof(fgfdm), 0,
(struct sockaddr*)&flightgear.addr, sizeof(flightgear.addr)) == -1) {
fprintf(stderr, "error sending to FlightGear\n");
fflush(stderr);
}
if (sendto(flightgear.socket, (char *)(&fgfdm), sizeof(fgfdm), 0,
(struct sockaddr *)&flightgear.addr, sizeof(flightgear.addr)) == -1) {
fprintf(stderr, "error sending to FlightGear\n");
fflush(stderr);
}
}
/**
@@ -126,7 +127,8 @@ void nps_flightgear_send_fdm() {
*
* This is the default option
*/
void nps_flightgear_send() {
void nps_flightgear_send()
{
struct FGNetGUI gui;
@@ -165,8 +167,8 @@ void nps_flightgear_send() {
gui.gs_deviation_deg = 0.;
if (sendto(flightgear.socket, (char*)(&gui), sizeof(gui), 0,
(struct sockaddr*)&flightgear.addr, sizeof(flightgear.addr)) == -1) {
if (sendto(flightgear.socket, (char *)(&gui), sizeof(gui), 0,
(struct sockaddr *)&flightgear.addr, sizeof(flightgear.addr)) == -1) {
fprintf(stderr, "error sending to FlightGear\n");
fflush(stderr);
}
sw/simulator/nps/nps_flightgear.h
+1 -1
View File
@@ -2,7 +2,7 @@
#define NPS_FLIGHTGEAR_H
extern void nps_flightgear_init(const char* host, unsigned int port, unsigned int time_offset);
extern void nps_flightgear_init(const char *host, unsigned int port, unsigned int time_offset);
extern void nps_flightgear_send();
extern void nps_flightgear_send_fdm();
sw/simulator/nps/nps_ivy.h
+2 -2
View File
@@ -1,8 +1,8 @@
#ifndef NPS_IVY
#define NPS_IVY
extern void nps_ivy_common_init(char* ivy_bus);
extern void nps_ivy_init(char* ivy_bus);
extern void nps_ivy_common_init(char *ivy_bus);
extern void nps_ivy_init(char *ivy_bus);
extern void nps_ivy_display(void);
#ifdef USE_MISSION_COMMANDS_IN_NPS
sw/simulator/nps/nps_ivy_common.c
+84 -67
View File
@@ -27,40 +27,41 @@
#include NPS_SENSORS_PARAMS
/* Gaia Ivy functions */
static void on_WORLD_ENV(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
/* Datalink Ivy functions */
static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_GET_SETTING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_GET_SETTING(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_PING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_PING(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_BLOCK(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
#ifdef RADIO_CONTROL_TYPE_DATALINK
static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_RC_3CH(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_RC_4CH(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_RC_4CH(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
#endif
void nps_ivy_common_init(char* ivy_bus) {
const char* agent_name = AIRFRAME_NAME"_NPS";
const char* ready_msg = AIRFRAME_NAME"_NPS Ready";
void nps_ivy_common_init(char *ivy_bus)
{
const char *agent_name = AIRFRAME_NAME"_NPS";
const char *ready_msg = AIRFRAME_NAME"_NPS Ready";
IvyInit(agent_name, ready_msg, NULL, NULL, NULL, NULL);
IvyBindMsg(on_WORLD_ENV, NULL, "^(\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
@@ -76,9 +77,9 @@ void nps_ivy_common_init(char* ivy_bus) {
#endif
#ifdef __APPLE__
const char* default_ivy_bus = "224.255.255.255";
const char *default_ivy_bus = "224.255.255.255";
#else
const char* default_ivy_bus = "127.255.255.255";
const char *default_ivy_bus = "127.255.255.255";
#endif
if (ivy_bus == NULL) {
IvyStart(default_ivy_bus);
@@ -91,9 +92,9 @@ void nps_ivy_common_init(char* ivy_bus) {
* Parse WORLD_ENV message from gaia.
*
*/
static void on_WORLD_ENV(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[])
static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
// wind speed in m/s
struct FloatVect3 wind;
@@ -124,11 +125,13 @@ static void on_WORLD_ENV(IvyClientPtr app __attribute__ ((unused)),
#include "generated/settings.h"
#include "dl_protocol.h"
#include "subsystems/datalink/downlink.h"
static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (atoi(argv[1]) != AC_ID)
static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (atoi(argv[1]) != AC_ID) {
return;
}
/* HACK:
* we actually don't want to allow changing settings if datalink is disabled,
@@ -144,13 +147,16 @@ static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
printf("setting %d %f\n", index, value);
}
static void on_DL_GET_SETTING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (atoi(argv[1]) != AC_ID)
static void on_DL_GET_SETTING(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (atoi(argv[1]) != AC_ID) {
return;
if (!autopilot.datalink_enabled)
}
if (!autopilot.datalink_enabled) {
return;
}
uint8_t index = atoi(argv[2]);
float value = settings_get_value(index);
@@ -158,22 +164,27 @@ static void on_DL_GET_SETTING(IvyClientPtr app __attribute__ ((unused)),
printf("get setting %d %f\n", index, value);
}
static void on_DL_PING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[] __attribute__ ((unused))) {
if (!autopilot.datalink_enabled)
static void on_DL_PING(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[] __attribute__((unused)))
{
if (!autopilot.datalink_enabled) {
return;
}
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
}
static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]){
if (atoi(argv[2]) != AC_ID)
static void on_DL_BLOCK(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (atoi(argv[2]) != AC_ID) {
return;
if (!autopilot.datalink_enabled)
}
if (!autopilot.datalink_enabled) {
return;
}
int block = atoi(argv[1]);
nav_goto_block(block);
@@ -181,11 +192,13 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
}
#ifdef RADIO_CONTROL_TYPE_DATALINK
static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]){
if (!autopilot.datalink_enabled)
static void on_DL_RC_3CH(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
uint8_t throttle_mode = atoi(argv[2]);
int8_t roll = atoi(argv[3]);
@@ -194,13 +207,16 @@ static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)),
//printf("rc_3ch: throttle_mode %d, roll %d, pitch %d\n", throttle_mode, roll, pitch);
}
static void on_DL_RC_4CH(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]){
if (atoi(argv[1]) != AC_ID)
static void on_DL_RC_4CH(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (atoi(argv[1]) != AC_ID) {
return;
if (!autopilot.datalink_enabled)
}
if (!autopilot.datalink_enabled) {
return;
}
uint8_t mode = atoi(argv[2]);
uint8_t throttle = atoi(argv[3]);
@@ -213,7 +229,8 @@ static void on_DL_RC_4CH(IvyClientPtr app __attribute__ ((unused)),
#endif
void nps_ivy_display(void) {
void nps_ivy_display(void)
{
IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
AC_ID,
DegOfRad(fdm.body_ecef_rotvel.p),
@@ -246,22 +263,22 @@ void nps_ivy_display(void) {
(fdm.ltpprz_pos.z));
IvySendMsg("%d NPS_GYRO_BIAS %f %f %f",
AC_ID,
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.x)+sensors.gyro.bias_initial.x),
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.y)+sensors.gyro.bias_initial.y),
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.z)+sensors.gyro.bias_initial.z));
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.x) + sensors.gyro.bias_initial.x),
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.y) + sensors.gyro.bias_initial.y),
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.z) + sensors.gyro.bias_initial.z));
/* transform magnetic field to body frame */
struct DoubleVect3 h_body;
double_quat_vmult(&h_body, &fdm.ltp_to_body_quat, &fdm.ltp_h);
IvySendMsg("%d NPS_SENSORS_SCALED %f %f %f %f %f %f",
AC_ID,
((sensors.accel.value.x - sensors.accel.neutral.x)/NPS_ACCEL_SENSITIVITY_XX),
((sensors.accel.value.y - sensors.accel.neutral.y)/NPS_ACCEL_SENSITIVITY_YY),
((sensors.accel.value.z - sensors.accel.neutral.z)/NPS_ACCEL_SENSITIVITY_ZZ),
h_body.x,
h_body.y,
h_body.z);
AC_ID,
((sensors.accel.value.x - sensors.accel.neutral.x) / NPS_ACCEL_SENSITIVITY_XX),
((sensors.accel.value.y - sensors.accel.neutral.y) / NPS_ACCEL_SENSITIVITY_YY),
((sensors.accel.value.z - sensors.accel.neutral.z) / NPS_ACCEL_SENSITIVITY_ZZ),
h_body.x,
h_body.y,
h_body.z);
IvySendMsg("%d NPS_WIND %f %f %f",
AC_ID,
sw/simulator/nps/nps_ivy_fixedwing.c
+11 -8
View File
@@ -11,11 +11,12 @@
#include "nps_autopilot.h"
/* fixedwing specific Datalink Ivy functions */
void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
void on_DL_MOVE_WP(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
void nps_ivy_init(char* ivy_bus) {
void nps_ivy_init(char *ivy_bus)
{
/* init ivy and bind some messages common to fw and rotorcraft */
nps_ivy_common_init(ivy_bus);
@@ -31,11 +32,13 @@ void nps_ivy_init(char* ivy_bus) {
#define MOfCm(_x) (((float)(_x))/100.)
#define MOfMM(_x) (((float)(_x))/1000.)
void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
void on_DL_MOVE_WP(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
if (atoi(argv[2]) == AC_ID) {
uint8_t wp_id = atoi(argv[1]);
sw/simulator/nps/nps_ivy_mission_commands.c
+143 -115
View File
@@ -18,70 +18,76 @@
extern uint8_t dl_buffer[MSG_SIZE];
/* mission specific Datalink Ivy functions */
static void on_DL_MISSION_GOTO_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_MISSION_GOTO_WP(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_MISSION_GOTO_WP_LLA(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_MISSION_GOTO_WP_LLA(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_MISSION_CIRCLE(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_MISSION_CIRCLE(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_MISSION_CIRCLE_LLA(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_MISSION_CIRCLE_LLA(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_MISSION_SEGMENT(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_MISSION_SEGMENT(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_MISSION_SEGMENT_LLA(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_MISSION_SEGMENT_LLA(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_MISSION_PATH(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_MISSION_PATH(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_MISSION_PATH_LLA(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_MISSION_PATH_LLA(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_GOTO_MISSION(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_GOTO_MISSION(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_NEXT_MISSION(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_NEXT_MISSION(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
static void on_DL_END_MISSION(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_END_MISSION(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
void nps_ivy_mission_commands_init(void) {
void nps_ivy_mission_commands_init(void)
{
IvyBindMsg(on_DL_MISSION_GOTO_WP, NULL, "^(\\S*) MISSION_GOTO_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_MISSION_GOTO_WP_LLA, NULL, "^(\\S*) MISSION_GOTO_WP_LLA (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_MISSION_CIRCLE, NULL, "^(\\S*) MISSION_CIRCLE (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_MISSION_CIRCLE_LLA, NULL, "^(\\S*) MISSION_CIRCLE_LLA (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_MISSION_SEGMENT, NULL, "^(\\S*) MISSION_SEGMENT (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_MISSION_SEGMENT_LLA, NULL, "^(\\S*) MISSION_SEGMENT_LLA (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_MISSION_PATH, NULL, "^(\\S*) MISSION_PATH (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_MISSION_PATH_LLA, NULL, "^(\\S*) MISSION_PATH_LLA (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_MISSION_CIRCLE_LLA, NULL,
"^(\\S*) MISSION_CIRCLE_LLA (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_MISSION_SEGMENT, NULL,
"^(\\S*) MISSION_SEGMENT (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_MISSION_SEGMENT_LLA, NULL,
"^(\\S*) MISSION_SEGMENT_LLA (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_MISSION_PATH, NULL,
"^(\\S*) MISSION_PATH (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_MISSION_PATH_LLA, NULL,
"^(\\S*) MISSION_PATH_LLA (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_GOTO_MISSION, NULL, "^(\\S*) GOTO_MISSION (\\S*) (\\S*)");
IvyBindMsg(on_DL_NEXT_MISSION, NULL, "^(\\S*) NEXT_MISSION (\\S*)");
IvyBindMsg(on_DL_END_MISSION, NULL, "^(\\S*) END_MISSION (\\S*)");
@@ -91,175 +97,193 @@ void nps_ivy_mission_commands_init(void) {
#include "generated/settings.h"
#include "dl_protocol.h"
#include "subsystems/datalink/downlink.h"
static void on_DL_MISSION_GOTO_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
static void on_DL_MISSION_GOTO_WP(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
for(i=1; i<5 ; i++){ //target components
dummy = (float)(atof(argv[2+i]));
memcpy(&dl_buffer[i*4], &dummy, 4);
for (i = 1; i < 5 ; i++) { //target components
dummy = (float)(atof(argv[2 + i]));
memcpy(&dl_buffer[i * 4], &dummy, 4);
}
mission_parse_GOTO_WP();
}
static void on_DL_MISSION_GOTO_WP_LLA(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
static void on_DL_MISSION_GOTO_WP_LLA(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
uint8_t i = 0;
int32_t dummy;
for(i=1; i<4 ; i++){ //target components (lat, lon, alt in int)
dummy = (int32_t)(atof(argv[2+i]));
memcpy(&dl_buffer[i*4], &dummy, 4);
for (i = 1; i < 4 ; i++) { //target components (lat, lon, alt in int)
dummy = (int32_t)(atof(argv[2 + i]));
memcpy(&dl_buffer[i * 4], &dummy, 4);
}
float d = (float)(atof(argv[6]));
memcpy(&dl_buffer[i*4], &d, 4); // duration
memcpy(&dl_buffer[i * 4], &d, 4); // duration
mission_parse_GOTO_WP_LLA();
}
static void on_DL_MISSION_CIRCLE(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
static void on_DL_MISSION_CIRCLE(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
for(i=1; i<6 ; i++){ //target components
dummy = (float)(atof(argv[2+i]));
memcpy(&dl_buffer[i*4], &dummy, 4);
for (i = 1; i < 6 ; i++) { //target components
dummy = (float)(atof(argv[2 + i]));
memcpy(&dl_buffer[i * 4], &dummy, 4);
}
mission_parse_CIRCLE();
}
static void on_DL_MISSION_CIRCLE_LLA(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
static void on_DL_MISSION_CIRCLE_LLA(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
uint8_t i = 0;
int32_t dummy;
for(i=1; i<4 ; i++){ //target components (lat, lon, alt in int)
dummy = (int32_t)(atof(argv[2+i]));
memcpy(&dl_buffer[i*4], &dummy, 4);
for (i = 1; i < 4 ; i++) { //target components (lat, lon, alt in int)
dummy = (int32_t)(atof(argv[2 + i]));
memcpy(&dl_buffer[i * 4], &dummy, 4);
}
float d = (float)(atof(argv[6])); // radius in m
memcpy(&dl_buffer[4*4], &d, 4);
memcpy(&dl_buffer[4 * 4], &d, 4);
d = (float)(atof(argv[7])); // duration
memcpy(&dl_buffer[5*4], &d, 4);
memcpy(&dl_buffer[5 * 4], &d, 4);
mission_parse_CIRCLE_LLA();
}
static void on_DL_MISSION_SEGMENT(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
static void on_DL_MISSION_SEGMENT(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
for(i=1; i<7 ; i++){ //target components
dummy = (float)(atof(argv[2+i]));
memcpy(&dl_buffer[i*4], &dummy, 4);
for (i = 1; i < 7 ; i++) { //target components
dummy = (float)(atof(argv[2 + i]));
memcpy(&dl_buffer[i * 4], &dummy, 4);
}
mission_parse_SEGMENT();
}
static void on_DL_MISSION_SEGMENT_LLA(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
static void on_DL_MISSION_SEGMENT_LLA(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
uint8_t i = 0;
int32_t dummy;
for(i=1; i<6 ; i++){ //target components
dummy = (int32_t)(atof(argv[2+i]));
memcpy(&dl_buffer[i*4], &dummy, 4);
for (i = 1; i < 6 ; i++) { //target components
dummy = (int32_t)(atof(argv[2 + i]));
memcpy(&dl_buffer[i * 4], &dummy, 4);
}
float d = (float)(atof(argv[8]));
memcpy(&dl_buffer[i*4], &d, 4);
memcpy(&dl_buffer[i * 4], &d, 4);
mission_parse_SEGMENT_LLA();
}
static void on_DL_MISSION_PATH(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
static void on_DL_MISSION_PATH(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
for(i=1; i<13 ; i++){ //target components
dummy = (float)(atof(argv[2+i]));
memcpy(&dl_buffer[i*4], &dummy, 4);
for (i = 1; i < 13 ; i++) { //target components
dummy = (float)(atof(argv[2 + i]));
memcpy(&dl_buffer[i * 4], &dummy, 4);
}
dl_buffer[i*4] = (uint8_t)(atoi(argv[2+i])); //path nb
dl_buffer[i * 4] = (uint8_t)(atoi(argv[2 + i])); //path nb
mission_parse_PATH();
}
static void on_DL_MISSION_PATH_LLA(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
static void on_DL_MISSION_PATH_LLA(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
uint8_t i = 0;
int32_t dummy;
for(i=1; i<12 ; i++){ //target components
dummy = (int32_t)(atof(argv[2+i]));
memcpy(&dl_buffer[i*4], &dummy, 4);
for (i = 1; i < 12 ; i++) { //target components
dummy = (int32_t)(atof(argv[2 + i]));
memcpy(&dl_buffer[i * 4], &dummy, 4);
}
float d = (float)(atof(argv[2+12])); // duration
memcpy(&dl_buffer[i*4], &d, 4);
dl_buffer[13*4] = (uint8_t)(atoi(argv[2+13])); //path nb
float d = (float)(atof(argv[2 + 12])); // duration
memcpy(&dl_buffer[i * 4], &d, 4);
dl_buffer[13 * 4] = (uint8_t)(atoi(argv[2 + 13])); //path nb
mission_parse_PATH_LLA();
}
static void on_DL_GOTO_MISSION(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
static void on_DL_GOTO_MISSION(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //mission_id
@@ -267,22 +291,26 @@ static void on_DL_GOTO_MISSION(IvyClientPtr app __attribute__ ((unused)),
mission_parse_GOTO_MISSION();
}
static void on_DL_NEXT_MISSION(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
static void on_DL_NEXT_MISSION(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
mission_parse_NEXT_MISSION();
}
static void on_DL_END_MISSION(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
static void on_DL_END_MISSION(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
sw/simulator/nps/nps_ivy_rotorcraft.c
+11 -8
View File
@@ -20,11 +20,12 @@
/* rotorcraft specificDatalink Ivy functions */
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
void nps_ivy_init(char* ivy_bus) {
void nps_ivy_init(char *ivy_bus)
{
/* init ivy and bind some messages common to fw and rotorcraft */
nps_ivy_common_init(ivy_bus);
IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
@@ -38,11 +39,13 @@ void nps_ivy_init(char* ivy_bus) {
#include "generated/settings.h"
#include "dl_protocol.h"
#include "subsystems/datalink/downlink.h"
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (!autopilot.datalink_enabled) {
return;
}
if (atoi(argv[2]) == AC_ID) {
uint8_t wp_id = atoi(argv[1]);
sw/simulator/nps/nps_main.c
+67 -56
View File
@@ -47,47 +47,51 @@ static struct {
double host_time_factor;
double sim_time;
double display_time;
char* fg_host;
char *fg_host;
unsigned int fg_port;
unsigned int fg_time_offset;
int fg_fdm;
char* js_dev;
char* spektrum_dev;
char *js_dev;
char *spektrum_dev;
int rc_script;
char* ivy_bus;
char *ivy_bus;
} nps_main;
static bool_t nps_main_parse_options(int argc, char** argv);
static bool_t nps_main_parse_options(int argc, char **argv);
static void nps_main_init(void);
static void nps_main_display(void);
static void nps_main_run_sim_step(void);
static gboolean nps_main_periodic(gpointer data __attribute__ ((unused)));
static gboolean nps_main_periodic(gpointer data __attribute__((unused)));
int pauseSignal = 0;
void tstp_hdl(int n __attribute__ ((unused))) {
void tstp_hdl(int n __attribute__((unused)))
{
if (pauseSignal) {
pauseSignal = 0;
signal (SIGTSTP, SIG_DFL);
signal(SIGTSTP, SIG_DFL);
raise(SIGTSTP);
} else {
pauseSignal = 1;
}
}
void cont_hdl (int n __attribute__ ((unused))) {
signal (SIGCONT, cont_hdl);
signal (SIGTSTP, tstp_hdl);
printf("Press <enter> to continue.\n");
}
double time_to_double(struct timeval *t) {
return ((double)t->tv_sec + (double)(t->tv_usec * 1e-6));
void cont_hdl(int n __attribute__((unused)))
{
signal(SIGCONT, cont_hdl);
signal(SIGTSTP, tstp_hdl);
printf("Press <enter> to continue.\n");
}
int main (int argc, char** argv) {
double time_to_double(struct timeval *t)
{
return ((double)t->tv_sec + (double)(t->tv_usec * 1e-6));
}
if (!nps_main_parse_options(argc, argv)) return 1;
int main(int argc, char **argv)
{
if (!nps_main_parse_options(argc, argv)) { return 1; }
/* disable buffering for stdout,
* so it properly works in paparazzi center
@@ -110,7 +114,8 @@ int main (int argc, char** argv) {
}
static void nps_main_init(void) {
static void nps_main_init(void)
{
nps_main.sim_time = 0.;
nps_main.display_time = 0.;
@@ -126,22 +131,21 @@ static void nps_main_init(void) {
printf("Simulating with dt of %f\n", SIM_DT);
enum NpsRadioControlType rc_type;
char* rc_dev = NULL;
char *rc_dev = NULL;
if (nps_main.js_dev) {
rc_type = JOYSTICK;
rc_dev = nps_main.js_dev;
}
else if (nps_main.spektrum_dev) {
} else if (nps_main.spektrum_dev) {
rc_type = SPEKTRUM;
rc_dev = nps_main.spektrum_dev;
}
else {
} else {
rc_type = SCRIPT;
}
nps_autopilot_init(rc_type, nps_main.rc_script, rc_dev);
if (nps_main.fg_host)
if (nps_main.fg_host) {
nps_flightgear_init(nps_main.fg_host, nps_main.fg_port, nps_main.fg_time_offset);
}
#if DEBUG_NPS_TIME
printf("host_time_factor,host_time_elapsed,host_time_now,scaled_initial_time,sim_time_before,display_time_before,sim_time_after,display_time_after\n");
@@ -151,7 +155,8 @@ static void nps_main_init(void) {
static void nps_main_run_sim_step(void) {
static void nps_main_run_sim_step(void)
{
// printf("sim at %f\n", nps_main.sim_time);
nps_atmosphere_update(SIM_DT);
@@ -167,14 +172,14 @@ static void nps_main_run_sim_step(void) {
}
static void nps_main_display(void) {
static void nps_main_display(void)
{
// printf("display at %f\n", nps_main.display_time);
nps_ivy_display();
if (nps_main.fg_host) {
if (nps_main.fg_fdm) {
nps_flightgear_send_fdm();
}
else {
} else {
nps_flightgear_send();
}
}
@@ -209,7 +214,8 @@ void nps_set_time_factor(float time_factor)
}
static gboolean nps_main_periodic(gpointer data __attribute__ ((unused))) {
static gboolean nps_main_periodic(gpointer data __attribute__((unused)))
{
struct timeval tv_now;
double host_time_now;
@@ -221,16 +227,18 @@ static gboolean nps_main_periodic(gpointer data __attribute__ ((unused))) {
gettimeofday(&tv_now, NULL);
t1 = time_to_double(&tv_now);
/* unscale to initial real time*/
irt = t1 - (t1 - nps_main.scaled_initial_time)*nps_main.host_time_factor;
irt = t1 - (t1 - nps_main.scaled_initial_time) * nps_main.host_time_factor;
printf("Press <enter> to continue (or CTRL-Z to suspend).\nEnter a new time factor if needed (current: %f): ", nps_main.host_time_factor);
printf("Press <enter> to continue (or CTRL-Z to suspend).\nEnter a new time factor if needed (current: %f): ",
nps_main.host_time_factor);
fflush(stdout);
if (fgets(line,127,stdin)) {
if ((sscanf(line," %le ", &tf) == 1)) {
if (tf > 0 && tf < 1000)
if (fgets(line, 127, stdin)) {
if ((sscanf(line, " %le ", &tf) == 1)) {
if (tf > 0 && tf < 1000) {
nps_main.host_time_factor = tf;
}
}
printf("Time factor is %f\n", nps_main.host_time_factor);
printf("Time factor is %f\n", nps_main.host_time_factor);
}
gettimeofday(&tv_now, NULL);
t2 = time_to_double(&tv_now);
@@ -238,16 +246,17 @@ static gboolean nps_main_periodic(gpointer data __attribute__ ((unused))) {
irt += t2 - t1;
nps_main.real_initial_time += t2 - t1;
/* convert to scaled initial real time */
nps_main.scaled_initial_time = t2 - (t2 - irt)/nps_main.host_time_factor;
nps_main.scaled_initial_time = t2 - (t2 - irt) / nps_main.host_time_factor;
pauseSignal = 0;
}
gettimeofday (&tv_now, NULL);
gettimeofday(&tv_now, NULL);
host_time_now = time_to_double(&tv_now);
double host_time_elapsed = nps_main.host_time_factor *(host_time_now - nps_main.scaled_initial_time);
double host_time_elapsed = nps_main.host_time_factor * (host_time_now - nps_main.scaled_initial_time);
#if DEBUG_NPS_TIME
printf("%f,%f,%f,%f,%f,%f,",nps_main.host_time_factor,host_time_elapsed,host_time_now,nps_main.scaled_initial_time,nps_main.sim_time,nps_main.display_time);
printf("%f,%f,%f,%f,%f,%f,", nps_main.host_time_factor, host_time_elapsed, host_time_now, nps_main.scaled_initial_time,
nps_main.sim_time, nps_main.display_time);
#endif
int cnt = 0;
@@ -274,7 +283,7 @@ static gboolean nps_main_periodic(gpointer data __attribute__ ((unused))) {
}
#if DEBUG_NPS_TIME
printf("%f,%f\n",nps_main.sim_time,nps_main.display_time);
printf("%f,%f\n", nps_main.sim_time, nps_main.display_time);
#endif
return TRUE;
@@ -282,7 +291,8 @@ static gboolean nps_main_periodic(gpointer data __attribute__ ((unused))) {
}
static bool_t nps_main_parse_options(int argc, char** argv) {
static bool_t nps_main_parse_options(int argc, char **argv)
{
nps_main.fg_host = NULL;
nps_main.fg_port = 5501;
@@ -294,19 +304,19 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
nps_main.host_time_factor = 1.0;
nps_main.fg_fdm = 0;
static const char* usage =
"Usage: %s [options]\n"
" Options :\n"
" -h Display this help\n"
" --fg_host <flight gear host> e.g. 127.0.0.1\n"
" --fg_port <flight gear port> e.g. 5501\n"
" --fg_time_offset <offset in seconds> e.g. 21600 for 6h\n"
" -j --js_dev <optional joystick index> e.g. 1 (default 0)\n"
" --spektrum_dev <spektrum device> e.g. /dev/ttyUSB0\n"
" --rc_script <number> e.g. 0\n"
" --ivy_bus <ivy bus> e.g. 127.255.255.255\n"
" --time_factor <factor> e.g. 2.5\n"
" --fg_fdm";
static const char *usage =
"Usage: %s [options]\n"
" Options :\n"
" -h Display this help\n"
" --fg_host <flight gear host> e.g. 127.0.0.1\n"
" --fg_port <flight gear port> e.g. 5501\n"
" --fg_time_offset <offset in seconds> e.g. 21600 for 6h\n"
" -j --js_dev <optional joystick index> e.g. 1 (default 0)\n"
" --spektrum_dev <spektrum device> e.g. /dev/ttyUSB0\n"
" --rc_script <number> e.g. 0\n"
" --ivy_bus <ivy bus> e.g. 127.255.255.255\n"
" --time_factor <factor> e.g. 2.5\n"
" --fg_fdm";
while (1) {
@@ -326,8 +336,9 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
int option_index = 0;
int c = getopt_long(argc, argv, "jh",
long_options, &option_index);
if (c == -1)
if (c == -1) {
break;
}
switch (c) {
case 0:
sw/simulator/nps/nps_radio_control.c
+42 -37
View File
@@ -32,21 +32,22 @@ struct NpsRadioControl nps_radio_control;
void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char* js_dev) {
void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char *js_dev)
{
nps_radio_control.next_update = 0.;
nps_radio_control.type = type;
nps_radio_control.num_script = num_script;
switch (type) {
case JOYSTICK:
nps_radio_control_joystick_init(js_dev);
break;
case SPEKTRUM:
nps_radio_control_spektrum_init(js_dev);
break;
case SCRIPT:
break;
case JOYSTICK:
nps_radio_control_joystick_init(js_dev);
break;
case SPEKTRUM:
nps_radio_control_spektrum_init(js_dev);
break;
case SCRIPT:
break;
}
}
@@ -73,7 +74,8 @@ static rc_script scripts[] = {
#define RADIO_CONTROL_TAKEOFF_TIME 8
bool_t nps_radio_control_available(double time) {
bool_t nps_radio_control_available(double time)
{
if (time >= nps_radio_control.next_update) {
nps_radio_control.next_update += RADIO_CONTROL_DT;
@@ -85,13 +87,13 @@ bool_t nps_radio_control_available(double time) {
nps_radio_control.yaw = nps_joystick.yaw;
nps_radio_control.mode = nps_joystick.mode;
//printf("throttle: %f, roll: %f, pitch: %f, yaw: %f\n", nps_joystick.throttle, nps_joystick.roll, nps_joystick.pitch, nps_joystick.yaw);
} else
if (nps_radio_control.type == SCRIPT) {
if (time < RADIO_CONTROL_TAKEOFF_TIME)
radio_control_script_takeoff(time);
else
scripts[nps_radio_control.num_script](time);
} else if (nps_radio_control.type == SCRIPT) {
if (time < RADIO_CONTROL_TAKEOFF_TIME) {
radio_control_script_takeoff(time);
} else {
scripts[nps_radio_control.num_script](time);
}
}
return TRUE;
}
return FALSE;
@@ -106,21 +108,24 @@ bool_t nps_radio_control_available(double time) {
*
*/
void radio_control_script_takeoff(double time) {
void radio_control_script_takeoff(double time)
{
nps_radio_control.roll = 0.;
nps_radio_control.pitch = 0.;
nps_radio_control.yaw = 0.;
nps_radio_control.throttle = 0.;
nps_radio_control.mode = MODE_SWITCH_MANUAL;
/* starts motors */
if (time < 1.)
if (time < 1.) {
nps_radio_control.yaw = 1.;
else
} else {
nps_radio_control.yaw = 0.;
}
}
void radio_control_script_hover(double time __attribute__ ((unused))) {
void radio_control_script_hover(double time __attribute__((unused)))
{
nps_radio_control.throttle = 0.99;
nps_radio_control.mode = MODE_SWITCH_AUTO2;
nps_radio_control.roll = 0.;
@@ -128,57 +133,57 @@ void radio_control_script_hover(double time __attribute__ ((unused))) {
}
void radio_control_script_step_roll(double time) {
void radio_control_script_step_roll(double time)
{
nps_radio_control.throttle = 0.99;
nps_radio_control.mode = MODE_SWITCH_AUTO2;
if (((int32_t)rint((time*0.5)))%2) {
if (((int32_t)rint((time * 0.5))) % 2) {
nps_radio_control.roll = 0.2;
nps_radio_control.yaw = 0.5;
}
else {
} else {
nps_radio_control.roll = -0.2;
nps_radio_control.yaw = 0.;
}
}
void radio_control_script_step_pitch(double time) {
void radio_control_script_step_pitch(double time)
{
nps_radio_control.roll = 0.;
nps_radio_control.yaw = 0.;
nps_radio_control.throttle = 0.99;
nps_radio_control.mode = MODE_SWITCH_AUTO2;
if (((int32_t)rint((time*0.5)))%2) {
if (((int32_t)rint((time * 0.5))) % 2) {
nps_radio_control.pitch = 0.2;
}
else {
} else {
nps_radio_control.pitch = -0.2;
}
}
void radio_control_script_step_yaw(double time) {
void radio_control_script_step_yaw(double time)
{
nps_radio_control.roll = 0.;
nps_radio_control.pitch = 0.;
nps_radio_control.throttle = 0.99;
nps_radio_control.mode = MODE_SWITCH_AUTO2;
if (((int32_t)rint((time*0.5)))%2) {
if (((int32_t)rint((time * 0.5))) % 2) {
nps_radio_control.yaw = 0.5;
}
else {
} else {
nps_radio_control.yaw = -0.5;
}
}
void radio_control_script_ff(double time __attribute__ ((unused))) {
void radio_control_script_ff(double time __attribute__((unused)))
{
nps_radio_control.throttle = 0.99;
nps_radio_control.mode = MODE_SWITCH_AUTO2;
if (time < RADIO_CONTROL_TAKEOFF_TIME+3)
if (time < RADIO_CONTROL_TAKEOFF_TIME + 3) {
nps_radio_control.pitch = -1.;
else if (time < RADIO_CONTROL_TAKEOFF_TIME+6) {
} else if (time < RADIO_CONTROL_TAKEOFF_TIME + 6) {
// nps_radio_control.roll = 0.5;
nps_radio_control.pitch = -1.;
}
else {
} else {
nps_radio_control.pitch = 0.;
nps_radio_control.roll = 0.;
}
sw/simulator/nps/nps_radio_control.h
+2 -2
View File
@@ -34,7 +34,7 @@ enum NpsRadioControlType {
SPEKTRUM
};
extern void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char* js_dev);
extern void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char *js_dev);
extern bool_t nps_radio_control_available(double time);
@@ -48,7 +48,7 @@ struct NpsRadioControl {
double mode;
enum NpsRadioControlType type;
int num_script;
char* js_dev;
char *js_dev;
};
extern struct NpsRadioControl nps_radio_control;
sw/simulator/nps/nps_radio_control_joystick.c
+54 -65
View File
@@ -127,7 +127,8 @@ SDL_Event sdl_event;
*
* @returns 0 on success
*/
int nps_radio_control_joystick_init(const char* device) {
int nps_radio_control_joystick_init(const char *device)
{
nps_joystick.throttle = 0.5;
nps_joystick.roll = 0.;
@@ -139,8 +140,7 @@ int nps_radio_control_joystick_init(const char* device) {
int device_index = atoi(device);
//Initialize SDL with joystick support and event support (through video)
if (SDL_Init(SDL_INIT_JOYSTICK|SDL_INIT_VIDEO) < 0)
{
if (SDL_Init(SDL_INIT_JOYSTICK | SDL_INIT_VIDEO) < 0) {
printf("Could not initialize SDL: %s.\n", SDL_GetError());
exit(-1);
}
@@ -149,55 +149,47 @@ int nps_radio_control_joystick_init(const char* device) {
atexit(SDL_Quit);
//Start the event handler, disable all but joystick button events and quit handler
SDL_EventState(SDL_ACTIVEEVENT,SDL_IGNORE);
SDL_EventState(SDL_KEYDOWN,SDL_IGNORE);
SDL_EventState(SDL_KEYUP,SDL_IGNORE);
SDL_EventState(SDL_MOUSEMOTION,SDL_IGNORE);
SDL_EventState(SDL_MOUSEBUTTONDOWN,SDL_IGNORE);
SDL_EventState(SDL_MOUSEBUTTONUP,SDL_IGNORE);
SDL_EventState(SDL_JOYAXISMOTION,SDL_IGNORE);
SDL_EventState(SDL_JOYBALLMOTION,SDL_IGNORE);
SDL_EventState(SDL_JOYHATMOTION,SDL_IGNORE);
SDL_EventState(SDL_ACTIVEEVENT, SDL_IGNORE);
SDL_EventState(SDL_KEYDOWN, SDL_IGNORE);
SDL_EventState(SDL_KEYUP, SDL_IGNORE);
SDL_EventState(SDL_MOUSEMOTION, SDL_IGNORE);
SDL_EventState(SDL_MOUSEBUTTONDOWN, SDL_IGNORE);
SDL_EventState(SDL_MOUSEBUTTONUP, SDL_IGNORE);
SDL_EventState(SDL_JOYAXISMOTION, SDL_IGNORE);
SDL_EventState(SDL_JOYBALLMOTION, SDL_IGNORE);
SDL_EventState(SDL_JOYHATMOTION, SDL_IGNORE);
//SDL_EventState(SDL_JOYBUTTONDOWN,SDL_IGNORE);
SDL_EventState(SDL_JOYBUTTONUP,SDL_IGNORE);
SDL_EventState(SDL_VIDEORESIZE,SDL_IGNORE);
SDL_EventState(SDL_VIDEOEXPOSE,SDL_IGNORE);
SDL_EventState(SDL_JOYBUTTONUP, SDL_IGNORE);
SDL_EventState(SDL_VIDEORESIZE, SDL_IGNORE);
SDL_EventState(SDL_VIDEOEXPOSE, SDL_IGNORE);
//SDL_EventState(SDL_QUIT,SDL_IGNORE);
SDL_EventState(SDL_USEREVENT,SDL_IGNORE);
SDL_EventState(SDL_SYSWMEVENT,SDL_IGNORE);
SDL_EventState(SDL_USEREVENT, SDL_IGNORE);
SDL_EventState(SDL_SYSWMEVENT, SDL_IGNORE);
//Check there are actually joysticks attached
if (!SDL_NumJoysticks())
{
if (!SDL_NumJoysticks()) {
printf("No joysticks attached! Quitting.\n");
exit(-1);
}
//Open the desired joystick and make sure it will work
sdl_joystick = SDL_JoystickOpen(device_index);
if (!sdl_joystick)
{
if (!sdl_joystick) {
printf("Joystick corresponding to SDL Index %d failed to open! Exiting.\n", device_index);
exit(-1);
}
else if (SDL_JoystickNumAxes(sdl_joystick) < JS_NB_AXIS)
{
} else if (SDL_JoystickNumAxes(sdl_joystick) < JS_NB_AXIS) {
printf("Selected joystick does not support enough axes!\n");
printf("Number of axes required: %i\n", JS_NB_AXIS);
printf("Number of axes available: %i\n",SDL_JoystickNumAxes(sdl_joystick));
printf("Number of axes available: %i\n", SDL_JoystickNumAxes(sdl_joystick));
SDL_JoystickClose(sdl_joystick);
exit(-1);
}
else if (SDL_JoystickNumButtons(sdl_joystick) < JS_NB_BUTTONS)
{
} else if (SDL_JoystickNumButtons(sdl_joystick) < JS_NB_BUTTONS) {
printf("Selected joystick does not support enough buttons!\n");
printf("Buttons supported: %d needed: %d\n", SDL_JoystickNumButtons(sdl_joystick), JS_NB_BUTTONS);
SDL_JoystickClose(sdl_joystick);
exit(-1);
}
else
{
printf("Using joystick named: %s\n",SDL_JoystickName(device_index));
} else {
printf("Using joystick named: %s\n", SDL_JoystickName(device_index));
}
return 0;
@@ -206,64 +198,61 @@ int nps_radio_control_joystick_init(const char* device) {
/**
* Updates joystick buttons from events, directly reads current axis positions..
*/
void nps_radio_control_joystick_update(void) {
void nps_radio_control_joystick_update(void)
{
#if NPS_JS_AXIS_THROTTLE_REVERSED
nps_joystick.throttle = ((float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_THROTTLE)) - 32767.)/-65534.;
nps_joystick.throttle = ((float)(-1 * SDL_JoystickGetAxis(sdl_joystick, NPS_JS_AXIS_THROTTLE)) - 32767.) / -65534.;
#else
nps_joystick.throttle = ((float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_THROTTLE)) - 32767.)/-65534.;
nps_joystick.throttle = ((float)(SDL_JoystickGetAxis(sdl_joystick, NPS_JS_AXIS_THROTTLE)) - 32767.) / -65534.;
#endif
#if NPS_JS_AXIS_ROLL_REVERSED
nps_joystick.roll = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_ROLL))/32767.;
nps_joystick.roll = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick, NPS_JS_AXIS_ROLL)) / 32767.;
#else
nps_joystick.roll = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_ROLL))/32767.;
nps_joystick.roll = (float)(SDL_JoystickGetAxis(sdl_joystick, NPS_JS_AXIS_ROLL)) / 32767.;
#endif
#if NPS_JS_AXIS_PITCH_REVERSED
nps_joystick.pitch = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_PITCH))/32767.;
nps_joystick.pitch = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick, NPS_JS_AXIS_PITCH)) / 32767.;
#else
nps_joystick.pitch = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_PITCH))/32767.;
nps_joystick.pitch = (float)(SDL_JoystickGetAxis(sdl_joystick, NPS_JS_AXIS_PITCH)) / 32767.;
#endif
#if NPS_JS_AXIS_YAW_REVERSED
nps_joystick.yaw = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_YAW))/32767.;
nps_joystick.yaw = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick, NPS_JS_AXIS_YAW)) / 32767.;
#else
nps_joystick.yaw = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_YAW))/32767.;
nps_joystick.yaw = (float)(SDL_JoystickGetAxis(sdl_joystick, NPS_JS_AXIS_YAW)) / 32767.;
#endif
// if an axis is asigned to the mode, use it instead of the buttons
#ifdef NPS_JS_AXIS_MODE
#if NPS_JS_AXIS_MODE_REVERSED
nps_joystick.mode = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_MODE))/32767.;
nps_joystick.mode = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick, NPS_JS_AXIS_MODE)) / 32767.;
#else
nps_joystick.mode = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_MODE))/32767.;
nps_joystick.mode = (float)(SDL_JoystickGetAxis(sdl_joystick, NPS_JS_AXIS_MODE)) / 32767.;
#endif
#endif
while(SDL_PollEvent(&sdl_event))
{
switch(sdl_event.type)
{
case SDL_JOYBUTTONDOWN:
{
switch(sdl_event.jbutton.button)
{
while (SDL_PollEvent(&sdl_event)) {
switch (sdl_event.type) {
case SDL_JOYBUTTONDOWN: {
switch (sdl_event.jbutton.button) {
#ifndef NPS_JS_AXIS_MODE
case NPS_JS_BUTTON_MODE_MANUAL:
nps_joystick.mode = MODE_SWITCH_MANUAL;
break;
case NPS_JS_BUTTON_MODE_MANUAL:
nps_joystick.mode = MODE_SWITCH_MANUAL;
break;
case NPS_JS_BUTTON_MODE_AUTO1:
nps_joystick.mode = MODE_SWITCH_AUTO1;
break;
case NPS_JS_BUTTON_MODE_AUTO1:
nps_joystick.mode = MODE_SWITCH_AUTO1;
break;
case NPS_JS_BUTTON_MODE_AUTO2:
nps_joystick.mode = MODE_SWITCH_AUTO2;
break;
case NPS_JS_BUTTON_MODE_AUTO2:
nps_joystick.mode = MODE_SWITCH_AUTO2;
break;
#endif
default:
//ignore
break;
}
default:
//ignore
break;
}
break;
}
break;
case SDL_QUIT:
printf("Quitting...\n");
sw/simulator/nps/nps_radio_control_joystick.h
+1 -1
View File
@@ -23,7 +23,7 @@
#ifndef NPS_RADIO_CONTROL_JOYSTICK_H
#define NPS_RADIO_CONTROL_JOYSTICK_H
extern int nps_radio_control_joystick_init(const char* device);
extern int nps_radio_control_joystick_init(const char *device);
extern void nps_radio_control_joystick_update(void);
struct NpsJoystick {
sw/simulator/nps/nps_radio_control_spektrum.c
+51 -46
View File
@@ -19,13 +19,14 @@
static int sp_fd;
static gboolean on_serial_data_received(GIOChannel *source,
GIOCondition condition,
gpointer data);
static void parse_data(char* buf, int len);
GIOCondition condition,
gpointer data);
static void parse_data(char *buf, int len);
static void handle_frame(void);
int nps_radio_control_spektrum_init(const char* device) {
int nps_radio_control_spektrum_init(const char *device)
{
if ((sp_fd = open(device, O_RDWR | O_NONBLOCK)) < 0) {
printf("opening %s (%s)\n", device, strerror(errno));
@@ -33,14 +34,14 @@ int nps_radio_control_spektrum_init(const char* device) {
}
struct termios termios;
/* input modes */
termios.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|INPCK|ISTRIP|INLCR|IGNCR
|ICRNL|IUCLC|IXON|IXANY|IXOFF|IMAXBEL);
termios.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | INPCK | ISTRIP | INLCR | IGNCR
| ICRNL | IUCLC | IXON | IXANY | IXOFF | IMAXBEL);
termios.c_iflag |= IGNPAR;
/* control modes*/
termios.c_cflag &= ~(CSIZE|PARENB|CRTSCTS|PARODD|HUPCL);
termios.c_cflag |= CREAD|CS8|CSTOPB|CLOCAL;
termios.c_cflag &= ~(CSIZE | PARENB | CRTSCTS | PARODD | HUPCL);
termios.c_cflag |= CREAD | CS8 | CSTOPB | CLOCAL;
/* local modes */
termios.c_lflag &= ~(ISIG|ICANON|IEXTEN|ECHO|FLUSHO|PENDIN);
termios.c_lflag &= ~(ISIG | ICANON | IEXTEN | ECHO | FLUSHO | PENDIN);
termios.c_lflag |= NOFLSH;
/* speed */
speed_t speed = B115200;
@@ -53,9 +54,9 @@ int nps_radio_control_spektrum_init(const char* device) {
printf("setting term attributes (%s)\n", strerror(errno));
return -1;
}
GIOChannel* channel = g_io_channel_unix_new(sp_fd);
GIOChannel *channel = g_io_channel_unix_new(sp_fd);
g_io_channel_set_encoding(channel, NULL, NULL);
g_io_add_watch (channel, G_IO_IN , on_serial_data_received, NULL);
g_io_add_watch(channel, G_IO_IN , on_serial_data_received, NULL);
return 0;
}
@@ -63,19 +64,20 @@ int nps_radio_control_spektrum_init(const char* device) {
static gboolean on_serial_data_received(GIOChannel *source,
GIOCondition condition __attribute__ ((unused)),
gpointer data __attribute__ ((unused))) {
GIOCondition condition __attribute__((unused)),
gpointer data __attribute__((unused)))
{
char buf[255];
gsize bytes_read;
GError* _err = NULL;
GError *_err = NULL;
GIOStatus st = g_io_channel_read_chars(source, buf, 255, &bytes_read, &_err);
if (!_err) {
if (st == G_IO_STATUS_NORMAL)
if (st == G_IO_STATUS_NORMAL) {
parse_data(buf, bytes_read);
}
else {
}
} else {
printf("error reading serial: %s\n", _err->message);
g_error_free (_err);
g_error_free(_err);
}
return TRUE;
}
@@ -94,45 +96,48 @@ uint8_t status = STA_UNINIT;
static uint8_t frame_buf[FRAME_LEN];
static uint32_t idx = 0;
static void parse_data(char* buf, int len) {
static void parse_data(char *buf, int len)
{
int i;
for (i=0; i<len; i++) {
for (i = 0; i < len; i++) {
int8_t c = buf[i ];
switch (status) {
case STA_UNINIT:
if (c==SYNC_1)
status = STA_GOT_SYNC_1;
break;
case STA_GOT_SYNC_1:
if (c==SYNC_2) {
status = STA_GOT_SYNC_2;
idx = 0;
}
else
status = STA_UNINIT;
break;
case STA_GOT_SYNC_2:
frame_buf[idx] = c;
idx++;
if (idx == FRAME_LEN) {
status = STA_UNINIT;
handle_frame();
}
break;
case STA_UNINIT:
if (c == SYNC_1) {
status = STA_GOT_SYNC_1;
}
break;
case STA_GOT_SYNC_1:
if (c == SYNC_2) {
status = STA_GOT_SYNC_2;
idx = 0;
} else {
status = STA_UNINIT;
}
break;
case STA_GOT_SYNC_2:
frame_buf[idx] = c;
idx++;
if (idx == FRAME_LEN) {
status = STA_UNINIT;
handle_frame();
}
break;
}
}
}
#define CHANNEL_OF_FRAME(i) ((((frame_buf[2*i]<<8) + frame_buf[2*i+1])&0x03FF)-512)
static void handle_frame(void) {
static void handle_frame(void)
{
nps_radio_control.roll = (float)CHANNEL_OF_FRAME(0)/-340.;
nps_radio_control.throttle = (float)(CHANNEL_OF_FRAME(1)+340)/680.;
nps_radio_control.pitch = (float)CHANNEL_OF_FRAME(2)/-340.;
nps_radio_control.yaw = (float)CHANNEL_OF_FRAME(3)/-340.;
nps_radio_control.mode = (float)CHANNEL_OF_FRAME(5)/340.;
nps_radio_control.roll = (float)CHANNEL_OF_FRAME(0) / -340.;
nps_radio_control.throttle = (float)(CHANNEL_OF_FRAME(1) + 340) / 680.;
nps_radio_control.pitch = (float)CHANNEL_OF_FRAME(2) / -340.;
nps_radio_control.yaw = (float)CHANNEL_OF_FRAME(3) / -340.;
nps_radio_control.mode = (float)CHANNEL_OF_FRAME(5) / 340.;
// printf("%f %f %f %f %f \n", nps_radio_control.roll, nps_radio_control.throttle, nps_radio_control.pitch, nps_radio_control.yaw, nps_radio_control.mode);
sw/simulator/nps/nps_radio_control_spektrum.h
+1 -1
View File
@@ -1,7 +1,7 @@
#ifndef NPS_RADIO_CONTROL_SPEKTRUM_H
#define NPS_RADIO_CONTROL_SPEKTRUM_H
extern int nps_radio_control_spektrum_init(const char* device);
extern int nps_radio_control_spektrum_init(const char *device);
#endif /* NPS_RADIO_CONTROL_SPEKTRUM_H */
sw/simulator/nps/nps_random.c
+72 -54
View File
@@ -16,7 +16,7 @@
static void r250_init(int seed);
//static unsigned int r250( void );
static double dr250( void );
static double dr250(void);
/*
* randclg
@@ -31,19 +31,22 @@ static unsigned long int randlcg(void);
#endif
void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev) {
void double_vect3_add_gaussian_noise(struct DoubleVect3 *vect, struct DoubleVect3 *std_dev)
{
vect->x += get_gaussian_noise() * std_dev->x;
vect->y += get_gaussian_noise() * std_dev->y;
vect->z += get_gaussian_noise() * std_dev->z;
}
void float_vect3_add_gaussian_noise(struct FloatVect3* vect, struct FloatVect3* std_dev) {
void float_vect3_add_gaussian_noise(struct FloatVect3 *vect, struct FloatVect3 *std_dev)
{
vect->x += get_gaussian_noise() * std_dev->x;
vect->y += get_gaussian_noise() * std_dev->y;
vect->z += get_gaussian_noise() * std_dev->z;
}
void float_rates_add_gaussian_noise(struct FloatRates* vect, struct FloatRates* std_dev) {
void float_rates_add_gaussian_noise(struct FloatRates *vect, struct FloatRates *std_dev)
{
vect->p += get_gaussian_noise() * std_dev->p;
vect->q += get_gaussian_noise() * std_dev->q;
vect->r += get_gaussian_noise() * std_dev->r;
@@ -51,18 +54,20 @@ void float_rates_add_gaussian_noise(struct FloatRates* vect, struct FloatRates*
void double_vect3_get_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev) {
void double_vect3_get_gaussian_noise(struct DoubleVect3 *vect, struct DoubleVect3 *std_dev)
{
vect->x = get_gaussian_noise() * std_dev->x;
vect->y = get_gaussian_noise() * std_dev->y;
vect->z = get_gaussian_noise() * std_dev->z;
}
void double_vect3_update_random_walk(struct DoubleVect3* rw, struct DoubleVect3* std_dev, double dt, double thau) {
void double_vect3_update_random_walk(struct DoubleVect3 *rw, struct DoubleVect3 *std_dev, double dt, double thau)
{
struct DoubleVect3 drw;
double_vect3_get_gaussian_noise(&drw, std_dev);
struct DoubleVect3 tmp;
VECT3_SMUL(tmp, *rw, (-1./thau));
VECT3_SMUL(tmp, *rw, (-1. / thau));
VECT3_ADD(drw, tmp);
VECT3_SMUL(drw, drw, dt);
VECT3_ADD(*rw, drw);
@@ -76,34 +81,36 @@ void double_vect3_update_random_walk(struct DoubleVect3* rw, struct DoubleVect3*
/*
http://www.taygeta.com/random/gaussian.html
*/
double get_gaussian_noise(void) {
double get_gaussian_noise(void)
{
double x1;
static int nb_call = 0;
static double x2, w;
if (nb_call==0) r250_init(0);
if (nb_call == 0) { r250_init(0); }
nb_call++;
if (nb_call%2) {
if (nb_call % 2) {
do {
x1 = 2.0 * dr250() - 1.0;
x2 = 2.0 * dr250() - 1.0;
w = x1 * x1 + x2 * x2;
} while ( w >= 1.0 );
} while (w >= 1.0);
w = sqrt( (-2.0 * log( w ) ) / w );
w = sqrt((-2.0 * log(w)) / w);
return x1 * w;
}
else
} else {
return x2 * w;
}
}
#else
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdlib.h>
double get_gaussian_noise(void) {
static gsl_rng * r = NULL;
double get_gaussian_noise(void)
{
static gsl_rng *r = NULL;
// select random number generator
if (!r) r = gsl_rng_alloc (gsl_rng_mt19937);
if (!r) { r = gsl_rng_alloc(gsl_rng_mt19937); }
return gsl_ran_gaussian(r, 1.);
}
#endif
@@ -124,7 +131,7 @@ double get_gaussian_noise(void) {
#if WORD_BIT == 32
#ifndef BITS
#define BITS 32
#define BITS 32
#endif
#else
#ifndef BITS
@@ -156,26 +163,29 @@ double get_gaussian_noise(void) {
static unsigned int r250_buffer[ 250 ];
static int r250_index;
static void r250_init(int sd) {
static void r250_init(int sd)
{
int j, k;
unsigned int mask, msb;
set_seed( sd );
set_seed(sd);
r250_index = 0;
for (j = 0; j < 250; j++) /* fill r250 buffer with BITS-1 bit values */
for (j = 0; j < 250; j++) { /* fill r250 buffer with BITS-1 bit values */
r250_buffer[j] = randlcg();
}
for (j = 0; j < 250; j++) /* set some MSBs to 1 */
if ( randlcg() > HALF_RANGE )
for (j = 0; j < 250; j++) /* set some MSBs to 1 */
if (randlcg() > HALF_RANGE) {
r250_buffer[j] |= MSB;
}
msb = MSB; /* turn on diagonal bit */
mask = ALL_BITS; /* turn off the leftmost bits */
msb = MSB; /* turn on diagonal bit */
mask = ALL_BITS; /* turn off the leftmost bits */
for (j = 0; j < BITS; j++) {
k = STEP * j + 3; /* select a word to operate on */
k = STEP * j + 3; /* select a word to operate on */
r250_buffer[k] &= mask; /* turn off bits left of the diagonal */
r250_buffer[k] |= msb; /* turn on the diagonal bit */
r250_buffer[k] |= msb; /* turn on the diagonal bit */
mask >>= 1;
msb >>= 1;
}
@@ -184,22 +194,25 @@ static void r250_init(int sd) {
#if 0
/* returns a random unsigned integer */
static unsigned int r250(void) {
register int j;
static unsigned int r250(void)
{
register int j;
register unsigned int new_rand;
if ( r250_index >= 147 )
j = r250_index - 147; /* wrap pointer around */
else
if (r250_index >= 147) {
j = r250_index - 147; /* wrap pointer around */
} else {
j = r250_index + 103;
}
new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ];
r250_buffer[ r250_index ] = new_rand;
if ( r250_index >= 249 ) /* increment pointer for next time */
if (r250_index >= 249) { /* increment pointer for next time */
r250_index = 0;
else
} else {
r250_index++;
}
return new_rand;
@@ -207,22 +220,25 @@ static unsigned int r250(void) {
#endif
/* returns a random double in range 0..1 */
static double dr250(void) {
register int j;
static double dr250(void)
{
register int j;
register unsigned int new_rand;
if ( r250_index >= 147 )
j = r250_index - 147; /* wrap pointer around */
else
if (r250_index >= 147) {
j = r250_index - 147; /* wrap pointer around */
} else {
j = r250_index + 103;
}
new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ];
r250_buffer[ r250_index ] = new_rand;
if ( r250_index >= 249 ) /* increment pointer for next time */
if (r250_index >= 249) { /* increment pointer for next time */
r250_index = 0;
else
} else {
r250_index++;
}
return (double)new_rand / ALL_BITS;
@@ -240,7 +256,8 @@ static long int my_remainder = LONG_MAX % 16807L;
static long int seed_val = 1L;
static long set_seed(long int sd) {
static long set_seed(long int sd)
{
return seed_val = sd;
}
@@ -249,23 +266,24 @@ static long set_seed(long int sd) {
//}
/* returns a random unsigned integer */
unsigned long int randlcg() {
if ( seed_val <= quotient )
unsigned long int randlcg()
{
if (seed_val <= quotient) {
seed_val = (seed_val * 16807L) % LONG_MAX;
else
{
long int high_part = seed_val / quotient;
long int low_part = seed_val % quotient;
} else {
long int high_part = seed_val / quotient;
long int low_part = seed_val % quotient;
long int test = 16807L * low_part - my_remainder * high_part;
if ( test > 0 )
seed_val = test;
else
seed_val = test + LONG_MAX;
long int test = 16807L * low_part - my_remainder * high_part;
if (test > 0) {
seed_val = test;
} else {
seed_val = test + LONG_MAX;
}
}
return seed_val;
}
sw/simulator/nps/nps_random.h
+6 -5
View File
@@ -4,12 +4,13 @@
#include "math/pprz_algebra_double.h"
extern double get_gaussian_noise(void);
extern void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev);
extern void double_vect3_get_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev);
extern void double_vect3_update_random_walk(struct DoubleVect3* rw, struct DoubleVect3* std_dev, double dt, double thau);
extern void double_vect3_add_gaussian_noise(struct DoubleVect3 *vect, struct DoubleVect3 *std_dev);
extern void double_vect3_get_gaussian_noise(struct DoubleVect3 *vect, struct DoubleVect3 *std_dev);
extern void double_vect3_update_random_walk(struct DoubleVect3 *rw, struct DoubleVect3 *std_dev, double dt,
double thau);
extern void float_vect3_add_gaussian_noise(struct FloatVect3* vect, struct FloatVect3* std_dev);
extern void float_rates_add_gaussian_noise(struct FloatRates* vect, struct FloatRates* std_dev);
extern void float_vect3_add_gaussian_noise(struct FloatVect3 *vect, struct FloatVect3 *std_dev);
extern void float_rates_add_gaussian_noise(struct FloatRates *vect, struct FloatRates *std_dev);