diff --git a/sw/simulator/nps/nps_atmosphere.c b/sw/simulator/nps/nps_atmosphere.c index 729ed878c3..4302f2c37b 100644 --- a/sw/simulator/nps/nps_atmosphere.c +++ b/sw/simulator/nps/nps_atmosphere.c @@ -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; } diff --git a/sw/simulator/nps/nps_autopilot.h b/sw/simulator/nps/nps_autopilot.h index c25b66b2ae..9576f5afce 100644 --- a/sw/simulator/nps/nps_autopilot.h +++ b/sw/simulator/nps/nps_autopilot.h @@ -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); diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index 3d25f3cf5a..e2c721eef9 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -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 #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); diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index 510f5f7ac1..12359455b5 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -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 #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); diff --git a/sw/simulator/nps/nps_electrical.c b/sw/simulator/nps/nps_electrical.c index 97e66eb908..f0a6363fc1 100644 --- a/sw/simulator/nps/nps_electrical.c +++ b/sw/simulator/nps/nps_electrical.c @@ -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; } diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h index c97f098fad..ef444f46ca 100644 --- a/sw/simulator/nps/nps_fdm.h +++ b/sw/simulator/nps/nps_fdm.h @@ -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); diff --git a/sw/simulator/nps/nps_fdm_crrcsim.c b/sw/simulator/nps/nps_fdm_crrcsim.c index 00ff4c8258..ecb211db71 100644 --- a/sw/simulator/nps/nps_fdm_crrcsim.c +++ b/sw/simulator/nps/nps_fdm_crrcsim.c @@ -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, <pdef, &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, <pdef, &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); } } diff --git a/sw/simulator/nps/nps_fdm_jsbsim.cpp b/sw/simulator/nps/nps_fdm_jsbsim.cpp index bcd7fd8853..445586d886 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.cpp +++ b/sw/simulator/nps/nps_fdm_jsbsim.cpp @@ -122,18 +122,18 @@ using namespace JSBSim; using namespace std; -static void feed_jsbsim(double* commands, int commands_nb); +static void feed_jsbsim(double *commands, int commands_nb); static void feed_jsbsim(double throttle, double aileron, double elevator, double rudder); static void fetch_state(void); static int check_for_nan(void); -static void jsbsimvec_to_vec(DoubleVect3* fdm_vector, const FGColumnVector3* jsb_vector); -static void jsbsimloc_to_loc(EcefCoor_d* fdm_location, const FGLocation* jsb_location); -static void jsbsimquat_to_quat(DoubleQuat* fdm_quat, const FGQuaternion* jsb_quat); -static void jsbsimvec_to_rate(DoubleRates* fdm_rate, const FGColumnVector3* jsb_vector); -static void llh_from_jsbsim(LlaCoor_d* fdm_lla, FGPropagate* propagate); -static void lla_from_jsbsim_geodetic(LlaCoor_d* fdm_lla, FGPropagate* propagate); -static void lla_from_jsbsim_geocentric(LlaCoor_d* fdm_lla, FGPropagate* propagate); +static void jsbsimvec_to_vec(DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector); +static void jsbsimloc_to_loc(EcefCoor_d *fdm_location, const FGLocation *jsb_location); +static void jsbsimquat_to_quat(DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat); +static void jsbsimvec_to_rate(DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector); +static void llh_from_jsbsim(LlaCoor_d *fdm_lla, FGPropagate *propagate); +static void lla_from_jsbsim_geodetic(LlaCoor_d *fdm_lla, FGPropagate *propagate); +static void lla_from_jsbsim_geocentric(LlaCoor_d *fdm_lla, FGPropagate *propagate); static void init_jsbsim(double dt); static void init_ltp(void); @@ -142,7 +142,7 @@ static void init_ltp(void); struct NpsFdm fdm; /// The JSBSim executive object -static FGFDMExec* FDMExec; +static FGFDMExec *FDMExec; static struct LtpDef_d ltpdef; @@ -155,13 +155,14 @@ double vehicle_radius_max; /// Timestep used for higher fidelity near the ground double min_dt; -void nps_fdm_init(double dt) { +void nps_fdm_init(double dt) +{ fdm.init_dt = dt; fdm.curr_dt = dt; //Sets up the high fidelity timestep as a multiple of the normal timestep - for (min_dt = (1.0/dt); min_dt < (1/MIN_DT); min_dt += (1/dt)){} - min_dt = (1/min_dt); + for (min_dt = (1.0 / dt); min_dt < (1 / MIN_DT); min_dt += (1 / dt)) {} + min_dt = (1 / min_dt); fdm.nan_count = 0; @@ -181,7 +182,8 @@ void nps_fdm_init(double dt) { } -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) +{ #ifdef NPS_JSBSIM_LAUNCHSPEED static bool_t already_launched = FALSE; @@ -250,23 +252,23 @@ void nps_fdm_run_step(bool_t launch __attribute__((unused)), double* commands, i void nps_fdm_set_wind(double speed, double dir) { - FGWinds* Winds = FDMExec->GetWinds(); + FGWinds *Winds = FDMExec->GetWinds(); Winds->SetWindspeed(FeetOfMeters(speed)); Winds->SetWindPsi(dir); } void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down) { - FGWinds* Winds = FDMExec->GetWinds(); + FGWinds *Winds = FDMExec->GetWinds(); Winds->SetWindNED(FeetOfMeters(wind_north), FeetOfMeters(wind_east), FeetOfMeters(wind_down)); } void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity) { - FGWinds* Winds = FDMExec->GetWinds(); + FGWinds *Winds = FDMExec->GetWinds(); /* wind speed used for turbulence */ - Winds->SetWindspeed20ft(FeetOfMeters(wind_speed)/2); + Winds->SetWindspeed20ft(FeetOfMeters(wind_speed) / 2); Winds->SetProbabilityOfExceedence(turbulence_severity); } @@ -276,15 +278,16 @@ void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity) * @param commands Pointer to array of doubles holding actuator commands * @param commands_nb Number of commands (length of array) */ -static void feed_jsbsim(double* commands, int commands_nb) { +static void feed_jsbsim(double *commands, int commands_nb) +{ #ifdef NPS_ACTUATOR_NAMES char buf[64]; - const char* names[] = NPS_ACTUATOR_NAMES; + const char *names[] = NPS_ACTUATOR_NAMES; string property; int i; - for (i=0; i < commands_nb; i++) { - sprintf(buf,"fcs/%s",names[i]); + for (i = 0; i < commands_nb; i++) { + sprintf(buf, "fcs/%s", names[i]); property = string(buf); FDMExec->GetPropertyManager()->GetNode(property)->SetDouble("", commands[i]); } @@ -300,8 +303,8 @@ static void feed_jsbsim(double* commands, int commands_nb) { static void feed_jsbsim(double throttle, double aileron, double elevator, double rudder) { - FGFCS* FCS = FDMExec->GetFCS(); - FGPropulsion* FProp = FDMExec->GetPropulsion(); + FGFCS *FCS = FDMExec->GetFCS(); + FGPropulsion *FProp = FDMExec->GetPropulsion(); // Set trims FCS->SetPitchTrimCmd(NPS_JSBSIM_PITCH_TRIM); @@ -318,8 +321,7 @@ static void feed_jsbsim(double throttle, double aileron, double elevator, double if (throttle > 0.01) { FProp->SetStarter(1); - } - else { + } else { FProp->SetStarter(0); } } @@ -329,23 +331,24 @@ static void feed_jsbsim(double throttle, double aileron, double elevator, double /** * Populates the NPS fdm struct after a simulation step. */ -static void fetch_state(void) { +static void fetch_state(void) +{ fdm.time = FDMExec->GetPropertyManager()->GetNode("simulation/sim-time-sec")->getDoubleValue(); #if DEBUG_NPS_JSBSIM - printf("%f,",fdm.time); + printf("%f,", fdm.time); #endif - FGPropagate* propagate = FDMExec->GetPropagate(); - FGAccelerations* accelerations = FDMExec->GetAccelerations(); + FGPropagate *propagate = FDMExec->GetPropagate(); + FGAccelerations *accelerations = FDMExec->GetAccelerations(); fdm.on_ground = FDMExec->GetGroundReactions()->GetWOW(); /* * position */ - jsbsimloc_to_loc(&fdm.ecef_pos,&propagate->GetLocation()); + jsbsimloc_to_loc(&fdm.ecef_pos, &propagate->GetLocation()); fdm.hmsl = propagate->GetAltitudeASLmeters(); /* @@ -363,20 +366,20 @@ static void fetch_state(void) { #endif /* in LTP frame */ - jsbsimvec_to_vec((DoubleVect3*)&fdm.ltp_ecef_vel, &propagate->GetVel()); - const FGColumnVector3& fg_ltp_ecef_accel = propagate->GetTb2l() * accelerations->GetUVWdot(); - jsbsimvec_to_vec((DoubleVect3*)&fdm.ltp_ecef_accel, &fg_ltp_ecef_accel); + jsbsimvec_to_vec((DoubleVect3 *)&fdm.ltp_ecef_vel, &propagate->GetVel()); + const FGColumnVector3 &fg_ltp_ecef_accel = propagate->GetTb2l() * accelerations->GetUVWdot(); + jsbsimvec_to_vec((DoubleVect3 *)&fdm.ltp_ecef_accel, &fg_ltp_ecef_accel); #if DEBUG_NPS_JSBSIM printf("%f,%f,%f,", fdm.ltp_ecef_accel.x, fdm.ltp_ecef_accel.y, fdm.ltp_ecef_accel.z); #endif /* in ECEF frame */ - const FGColumnVector3& fg_ecef_ecef_vel = propagate->GetECEFVelocity(); - jsbsimvec_to_vec((DoubleVect3*)&fdm.ecef_ecef_vel, &fg_ecef_ecef_vel); + const FGColumnVector3 &fg_ecef_ecef_vel = propagate->GetECEFVelocity(); + jsbsimvec_to_vec((DoubleVect3 *)&fdm.ecef_ecef_vel, &fg_ecef_ecef_vel); - const FGColumnVector3& fg_ecef_ecef_accel = propagate->GetTb2ec() * accelerations->GetUVWdot(); - jsbsimvec_to_vec((DoubleVect3*)&fdm.ecef_ecef_accel, &fg_ecef_ecef_accel); + const FGColumnVector3 &fg_ecef_ecef_accel = propagate->GetTb2ec() * accelerations->GetUVWdot(); + jsbsimvec_to_vec((DoubleVect3 *)&fdm.ecef_ecef_accel, &fg_ecef_ecef_accel); #if DEBUG_NPS_JSBSIM printf("%f,%f,%f,", fdm.ecef_ecef_accel.x, fdm.ecef_ecef_accel.y, fdm.ecef_ecef_accel.z); @@ -401,7 +404,7 @@ static void fetch_state(void) { fdm.agl = MetersOfFeet(propagate->GetDistanceAGL()); #if DEBUG_NPS_JSBSIM - printf("%f\n",fdm.agl); + printf("%f\n", fdm.agl); #endif /* @@ -429,23 +432,27 @@ static void fetch_state(void) { /* * wind */ - const FGColumnVector3& fg_wind_ned = FDMExec->GetWinds()->GetTotalWindNED(); + const FGColumnVector3 &fg_wind_ned = FDMExec->GetWinds()->GetTotalWindNED(); jsbsimvec_to_vec(&fdm.wind, &fg_wind_ned); /* * Control surface positions * */ - fdm.rudder = (FDMExec->GetPropertyManager()->GetNode("fcs/rudder-pos-rad")->getDoubleValue())/NPS_JSBSIM_RUDDER_MAX_RAD; - fdm.left_aileron = (-1*FDMExec->GetPropertyManager()->GetNode("fcs/left-aileron-pos-rad")->getDoubleValue())/NPS_JSBSIM_AILERON_MAX_RAD; - fdm.right_aileron = (FDMExec->GetPropertyManager()->GetNode("fcs/right-aileron-pos-rad")->getDoubleValue())/NPS_JSBSIM_AILERON_MAX_RAD; - fdm.elevator = (FDMExec->GetPropertyManager()->GetNode("fcs/elevator-pos-rad")->getDoubleValue())/NPS_JSBSIM_ELEVATOR_MAX_RAD; - fdm.flap = (FDMExec->GetPropertyManager()->GetNode("fcs/flap-pos-rad")->getDoubleValue())/NPS_JSBSIM_FLAP_MAX_RAD; + fdm.rudder = (FDMExec->GetPropertyManager()->GetNode("fcs/rudder-pos-rad")->getDoubleValue()) / + NPS_JSBSIM_RUDDER_MAX_RAD; + fdm.left_aileron = (-1 * FDMExec->GetPropertyManager()->GetNode("fcs/left-aileron-pos-rad")->getDoubleValue()) / + NPS_JSBSIM_AILERON_MAX_RAD; + fdm.right_aileron = (FDMExec->GetPropertyManager()->GetNode("fcs/right-aileron-pos-rad")->getDoubleValue()) / + NPS_JSBSIM_AILERON_MAX_RAD; + fdm.elevator = (FDMExec->GetPropertyManager()->GetNode("fcs/elevator-pos-rad")->getDoubleValue()) / + NPS_JSBSIM_ELEVATOR_MAX_RAD; + fdm.flap = (FDMExec->GetPropertyManager()->GetNode("fcs/flap-pos-rad")->getDoubleValue()) / NPS_JSBSIM_FLAP_MAX_RAD; /* * Propulsion */ - FGPropulsion* FGProp = FDMExec->GetPropulsion(); + FGPropulsion *FGProp = FDMExec->GetPropulsion(); fdm.num_engines = FGProp->GetNumEngines(); /* @@ -453,9 +460,9 @@ static void fetch_state(void) { * (even when the moment of inertia of the propeller has the right value) * As a result after switching the motor off */ - for(uint32_t k=0; kGetEngine(k); - FGThruster* FGThrst = FGEng->GetThruster(); + for (uint32_t k = 0; k < fdm.num_engines; k++) { + FGEngine *FGEng = FGProp->GetEngine(k); + FGThruster *FGThrst = FGEng->GetThruster(); fdm.eng_state[k] = FGEng->GetStarter(); fdm.rpm[k] = (float) FGThrst->GetRPM(); //printf("RPM: %f\n", fdm.rpm[k]); @@ -473,13 +480,14 @@ static void fetch_state(void) { * * @warning Needs PAPARAZZI_HOME defined to find the config files */ -static void init_jsbsim(double dt) { +static void init_jsbsim(double dt) +{ char buf[1024]; string rootdir; string jsbsim_ic_name; - sprintf(buf,"%s/conf/simulator/jsbsim/",getenv("PAPARAZZI_HOME")); + sprintf(buf, "%s/conf/simulator/jsbsim/", getenv("PAPARAZZI_HOME")); rootdir = string(buf); /* if jsbsim initial conditions are defined, use them @@ -497,11 +505,11 @@ static void init_jsbsim(double dt) { FDMExec->DisableOutput(); FDMExec->SetDebugLevel(0); // No DEBUG messages - if ( ! FDMExec->LoadModel( rootdir + "aircraft", - rootdir + "engine", - rootdir + "systems", - NPS_JSBSIM_MODEL, - false)){ + if (! FDMExec->LoadModel(rootdir + "aircraft", + rootdir + "engine", + rootdir + "systems", + NPS_JSBSIM_MODEL, + false)) { #ifdef DEBUG cerr << " JSBSim could not be started" << endl << endl; #endif @@ -518,8 +526,8 @@ static void init_jsbsim(double dt) { struct LlaCoor_d lla0; FGInitialCondition *IC = FDMExec->GetIC(); - if(!jsbsim_ic_name.empty()) { - if ( ! IC->Load(jsbsim_ic_name)) { + if (!jsbsim_ic_name.empty()) { + if (! IC->Load(jsbsim_ic_name)) { #ifdef DEBUG cerr << "Initialization unsuccessful" << endl; #endif @@ -529,8 +537,7 @@ static void init_jsbsim(double dt) { llh_from_jsbsim(&lla0, FDMExec->GetPropagate()); cout << "JSBSim initial conditions loaded from " << jsbsim_ic_name << endl; - } - else { + } else { // FGInitialCondition::SetAltitudeASLFtIC // requires this function to be called // before itself @@ -551,7 +558,7 @@ static void init_jsbsim(double dt) { lla0.lon = RadOfDeg(NAV_LON0 / 1e7); lla0.lat = gd_lat; - lla0.alt = (double)(NAV_ALT0+NAV_MSL0)/1000.0; + lla0.alt = (double)(NAV_ALT0 + NAV_MSL0) / 1000.0; } // initial commands to zero @@ -580,10 +587,10 @@ static void init_jsbsim(double dt) { vehicle_radius_max = 0.01; // specify not 0.0 in case no gear int num_gear = FDMExec->GetGroundReactions()->GetNumGearUnits(); int i; - for(i = 0; i < num_gear; i++) { + for (i = 0; i < num_gear; i++) { FGColumnVector3 gear_location = FDMExec->GetGroundReactions()->GetGearUnit(i)->GetBodyLocation(); double radius = MetersOfFeet(gear_location.Magnitude()); - if (radius > vehicle_radius_max) vehicle_radius_max = radius; + if (radius > vehicle_radius_max) { vehicle_radius_max = radius; } } } @@ -592,9 +599,10 @@ static void init_jsbsim(double dt) { * Initialize the ltp from the JSBSim location. * */ -static void init_ltp(void) { +static void init_ltp(void) +{ - FGPropagate* propagate = FDMExec->GetPropagate(); + FGPropagate *propagate = FDMExec->GetPropagate(); jsbsimloc_to_loc(&fdm.ecef_pos, &propagate->GetLocation()); ltp_def_from_ecef_d(<pdef, &fdm.ecef_pos); @@ -605,17 +613,17 @@ static void init_ltp(void) { #if !NPS_CALC_GEO_MAG && defined(AHRS_H_X) -PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (AHRS section).") + PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (AHRS section).") fdm.ltp_h.x = AHRS_H_X; fdm.ltp_h.y = AHRS_H_Y; fdm.ltp_h.z = AHRS_H_Z; #elif !NPS_CALC_GEO_MAG && defined(INS_H_X) -PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (INS section).") + PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file (INS section).") fdm.ltp_h.x = INS_H_X; fdm.ltp_h.y = INS_H_Y; fdm.ltp_h.z = INS_H_Z; #else -PRINT_CONFIG_MSG("Using WMM2010 model to calculate magnetic field at simulated location.") + PRINT_CONFIG_MSG("Using WMM2010 model to calculate magnetic field at simulated location.") /* calculation of magnetic field according to WMM2010 model */ double gha[MAXCOEFF]; @@ -649,7 +657,8 @@ PRINT_CONFIG_MSG("Using WMM2010 model to calculate magnetic field at simulated l * @param fdm_location Pointer to EcefCoor_d struct * @param jsb_location Pointer to FGLocation struct */ -static void jsbsimloc_to_loc(EcefCoor_d* fdm_location, const FGLocation* jsb_location){ +static void jsbsimloc_to_loc(EcefCoor_d *fdm_location, const FGLocation *jsb_location) +{ fdm_location->x = MetersOfFeet(jsb_location->Entry(1)); fdm_location->y = MetersOfFeet(jsb_location->Entry(2)); @@ -666,7 +675,8 @@ static void jsbsimloc_to_loc(EcefCoor_d* fdm_location, const FGLocation* jsb_loc * @param fdm_vector Pointer to DoubleVect3 struct * @param jsb_vector Pointer to FGColumnVector3 struct */ -static void jsbsimvec_to_vec(DoubleVect3* fdm_vector, const FGColumnVector3* jsb_vector) { +static void jsbsimvec_to_vec(DoubleVect3 *fdm_vector, const FGColumnVector3 *jsb_vector) +{ fdm_vector->x = MetersOfFeet(jsb_vector->Entry(1)); fdm_vector->y = MetersOfFeet(jsb_vector->Entry(2)); @@ -680,7 +690,8 @@ static void jsbsimvec_to_vec(DoubleVect3* fdm_vector, const FGColumnVector3* jsb * @param fdm_quat Pointer to DoubleQuat struct * @param jsb_quat Pointer to FGQuaternion struct */ -static void jsbsimquat_to_quat(DoubleQuat* fdm_quat, const FGQuaternion* jsb_quat){ +static void jsbsimquat_to_quat(DoubleQuat *fdm_quat, const FGQuaternion *jsb_quat) +{ fdm_quat->qi = jsb_quat->Entry(1); fdm_quat->qx = jsb_quat->Entry(2); @@ -695,7 +706,8 @@ static void jsbsimquat_to_quat(DoubleQuat* fdm_quat, const FGQuaternion* jsb_qua * @param fdm_rate Pointer to DoubleRates struct * @param jsb_vector Pointer to FGColumnVector3 struct */ -static void jsbsimvec_to_rate(DoubleRates* fdm_rate, const FGColumnVector3* jsb_vector) { +static void jsbsimvec_to_rate(DoubleRates *fdm_rate, const FGColumnVector3 *jsb_vector) +{ fdm_rate->p = jsb_vector->Entry(1); fdm_rate->q = jsb_vector->Entry(2); @@ -711,7 +723,8 @@ static void jsbsimvec_to_rate(DoubleRates* fdm_rate, const FGColumnVector3* jsb_ * @param fdm_lla Pointer to LlaCoor_d struct * @param propagate Pointer to JSBSim FGPropagate object */ -void llh_from_jsbsim(LlaCoor_d* fdm_lla, FGPropagate* propagate) { +void llh_from_jsbsim(LlaCoor_d *fdm_lla, FGPropagate *propagate) +{ fdm_lla->lat = propagate->GetGeodLatitudeRad(); fdm_lla->lon = propagate->GetLongitude(); @@ -730,7 +743,8 @@ void llh_from_jsbsim(LlaCoor_d* fdm_lla, FGPropagate* propagate) { * @param fdm_lla Pointer to LlaCoor_d struct * @param propagate Pointer to JSBSim FGPropagate object */ -void lla_from_jsbsim_geocentric(LlaCoor_d* fdm_lla, FGPropagate* propagate) { +void lla_from_jsbsim_geocentric(LlaCoor_d *fdm_lla, FGPropagate *propagate) +{ fdm_lla->lat = propagate->GetLatitude(); fdm_lla->lon = propagate->GetLongitude(); @@ -746,7 +760,8 @@ void lla_from_jsbsim_geocentric(LlaCoor_d* fdm_lla, FGPropagate* propagate) { * @param fdm_lla Pointer to LlaCoor_d struct * @param propagate Pointer to JSBSim FGPropagate object */ -void lla_from_jsbsim_geodetic(LlaCoor_d* fdm_lla, FGPropagate* propagate) { +void lla_from_jsbsim_geodetic(LlaCoor_d *fdm_lla, FGPropagate *propagate) +{ fdm_lla->lat = propagate->GetGeodLatitudeRad(); fdm_lla->lon = propagate->GetLongitude(); @@ -767,74 +782,75 @@ static int isnan(double f) { return (f != f); } * * @return Count of new NaNs. 0 for no new NaNs. */ -static int check_for_nan(void) { +static int check_for_nan(void) +{ int orig_nan_count = fdm.nan_count; /* Check all elements for nans */ - if (isnan(fdm.ecef_pos.x)) fdm.nan_count++; - if (isnan(fdm.ecef_pos.y)) fdm.nan_count++; - if (isnan(fdm.ecef_pos.z)) fdm.nan_count++; - if (isnan(fdm.ltpprz_pos.x)) fdm.nan_count++; - if (isnan(fdm.ltpprz_pos.y)) fdm.nan_count++; - if (isnan(fdm.ltpprz_pos.z)) fdm.nan_count++; - if (isnan(fdm.lla_pos.lon)) fdm.nan_count++; - if (isnan(fdm.lla_pos.lat)) fdm.nan_count++; - if (isnan(fdm.lla_pos.alt)) fdm.nan_count++; - if (isnan(fdm.hmsl)) fdm.nan_count++; + if (isnan(fdm.ecef_pos.x)) { fdm.nan_count++; } + if (isnan(fdm.ecef_pos.y)) { fdm.nan_count++; } + if (isnan(fdm.ecef_pos.z)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_pos.x)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_pos.y)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_pos.z)) { fdm.nan_count++; } + if (isnan(fdm.lla_pos.lon)) { fdm.nan_count++; } + if (isnan(fdm.lla_pos.lat)) { fdm.nan_count++; } + if (isnan(fdm.lla_pos.alt)) { fdm.nan_count++; } + if (isnan(fdm.hmsl)) { fdm.nan_count++; } // Skip debugging elements - if (isnan(fdm.ecef_ecef_vel.x)) fdm.nan_count++; - if (isnan(fdm.ecef_ecef_vel.y)) fdm.nan_count++; - if (isnan(fdm.ecef_ecef_vel.z)) fdm.nan_count++; - if (isnan(fdm.ecef_ecef_accel.x)) fdm.nan_count++; - if (isnan(fdm.ecef_ecef_accel.y)) fdm.nan_count++; - if (isnan(fdm.ecef_ecef_accel.z)) fdm.nan_count++; - if (isnan(fdm.body_ecef_vel.x)) fdm.nan_count++; - if (isnan(fdm.body_ecef_vel.y)) fdm.nan_count++; - if (isnan(fdm.body_ecef_vel.z)) fdm.nan_count++; - if (isnan(fdm.body_ecef_accel.x)) fdm.nan_count++; - if (isnan(fdm.body_ecef_accel.y)) fdm.nan_count++; - if (isnan(fdm.body_ecef_accel.z)) fdm.nan_count++; - if (isnan(fdm.ltp_ecef_vel.x)) fdm.nan_count++; - if (isnan(fdm.ltp_ecef_vel.y)) fdm.nan_count++; - if (isnan(fdm.ltp_ecef_vel.z)) fdm.nan_count++; - if (isnan(fdm.ltp_ecef_accel.x)) fdm.nan_count++; - if (isnan(fdm.ltp_ecef_accel.y)) fdm.nan_count++; - if (isnan(fdm.ltp_ecef_accel.z)) fdm.nan_count++; - if (isnan(fdm.ltpprz_ecef_vel.x)) fdm.nan_count++; - if (isnan(fdm.ltpprz_ecef_vel.y)) fdm.nan_count++; - if (isnan(fdm.ltpprz_ecef_vel.z)) fdm.nan_count++; - if (isnan(fdm.ltpprz_ecef_accel.x)) fdm.nan_count++; - if (isnan(fdm.ltpprz_ecef_accel.y)) fdm.nan_count++; - if (isnan(fdm.ltpprz_ecef_accel.z)) fdm.nan_count++; - if (isnan(fdm.ecef_to_body_quat.qi)) fdm.nan_count++; - if (isnan(fdm.ecef_to_body_quat.qx)) fdm.nan_count++; - if (isnan(fdm.ecef_to_body_quat.qy)) fdm.nan_count++; - if (isnan(fdm.ecef_to_body_quat.qz)) fdm.nan_count++; - if (isnan(fdm.ltp_to_body_quat.qi)) fdm.nan_count++; - if (isnan(fdm.ltp_to_body_quat.qx)) fdm.nan_count++; - if (isnan(fdm.ltp_to_body_quat.qy)) fdm.nan_count++; - if (isnan(fdm.ltp_to_body_quat.qz)) fdm.nan_count++; - if (isnan(fdm.ltp_to_body_eulers.phi)) fdm.nan_count++; - if (isnan(fdm.ltp_to_body_eulers.theta)) fdm.nan_count++; - if (isnan(fdm.ltp_to_body_eulers.psi)) fdm.nan_count++; - if (isnan(fdm.ltpprz_to_body_quat.qi)) fdm.nan_count++; - if (isnan(fdm.ltpprz_to_body_quat.qx)) fdm.nan_count++; - if (isnan(fdm.ltpprz_to_body_quat.qy)) fdm.nan_count++; - if (isnan(fdm.ltpprz_to_body_quat.qz)) fdm.nan_count++; - if (isnan(fdm.ltpprz_to_body_eulers.phi)) fdm.nan_count++; - if (isnan(fdm.ltpprz_to_body_eulers.theta)) fdm.nan_count++; - if (isnan(fdm.ltpprz_to_body_eulers.psi)) fdm.nan_count++; - if (isnan(fdm.body_ecef_rotvel.p)) fdm.nan_count++; - if (isnan(fdm.body_ecef_rotvel.q)) fdm.nan_count++; - if (isnan(fdm.body_ecef_rotvel.r)) fdm.nan_count++; - if (isnan(fdm.body_ecef_rotaccel.p)) fdm.nan_count++; - if (isnan(fdm.body_ecef_rotaccel.q)) fdm.nan_count++; - if (isnan(fdm.body_ecef_rotaccel.r)) fdm.nan_count++; - if (isnan(fdm.ltp_g.x)) fdm.nan_count++; - if (isnan(fdm.ltp_g.y)) fdm.nan_count++; - if (isnan(fdm.ltp_g.z)) fdm.nan_count++; - if (isnan(fdm.ltp_h.x)) fdm.nan_count++; - if (isnan(fdm.ltp_h.y)) fdm.nan_count++; - if (isnan(fdm.ltp_h.z)) fdm.nan_count++; + if (isnan(fdm.ecef_ecef_vel.x)) { fdm.nan_count++; } + if (isnan(fdm.ecef_ecef_vel.y)) { fdm.nan_count++; } + if (isnan(fdm.ecef_ecef_vel.z)) { fdm.nan_count++; } + if (isnan(fdm.ecef_ecef_accel.x)) { fdm.nan_count++; } + if (isnan(fdm.ecef_ecef_accel.y)) { fdm.nan_count++; } + if (isnan(fdm.ecef_ecef_accel.z)) { fdm.nan_count++; } + if (isnan(fdm.body_ecef_vel.x)) { fdm.nan_count++; } + if (isnan(fdm.body_ecef_vel.y)) { fdm.nan_count++; } + if (isnan(fdm.body_ecef_vel.z)) { fdm.nan_count++; } + if (isnan(fdm.body_ecef_accel.x)) { fdm.nan_count++; } + if (isnan(fdm.body_ecef_accel.y)) { fdm.nan_count++; } + if (isnan(fdm.body_ecef_accel.z)) { fdm.nan_count++; } + if (isnan(fdm.ltp_ecef_vel.x)) { fdm.nan_count++; } + if (isnan(fdm.ltp_ecef_vel.y)) { fdm.nan_count++; } + if (isnan(fdm.ltp_ecef_vel.z)) { fdm.nan_count++; } + if (isnan(fdm.ltp_ecef_accel.x)) { fdm.nan_count++; } + if (isnan(fdm.ltp_ecef_accel.y)) { fdm.nan_count++; } + if (isnan(fdm.ltp_ecef_accel.z)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_ecef_vel.x)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_ecef_vel.y)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_ecef_vel.z)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_ecef_accel.x)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_ecef_accel.y)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_ecef_accel.z)) { fdm.nan_count++; } + if (isnan(fdm.ecef_to_body_quat.qi)) { fdm.nan_count++; } + if (isnan(fdm.ecef_to_body_quat.qx)) { fdm.nan_count++; } + if (isnan(fdm.ecef_to_body_quat.qy)) { fdm.nan_count++; } + if (isnan(fdm.ecef_to_body_quat.qz)) { fdm.nan_count++; } + if (isnan(fdm.ltp_to_body_quat.qi)) { fdm.nan_count++; } + if (isnan(fdm.ltp_to_body_quat.qx)) { fdm.nan_count++; } + if (isnan(fdm.ltp_to_body_quat.qy)) { fdm.nan_count++; } + if (isnan(fdm.ltp_to_body_quat.qz)) { fdm.nan_count++; } + if (isnan(fdm.ltp_to_body_eulers.phi)) { fdm.nan_count++; } + if (isnan(fdm.ltp_to_body_eulers.theta)) { fdm.nan_count++; } + if (isnan(fdm.ltp_to_body_eulers.psi)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_to_body_quat.qi)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_to_body_quat.qx)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_to_body_quat.qy)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_to_body_quat.qz)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_to_body_eulers.phi)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_to_body_eulers.theta)) { fdm.nan_count++; } + if (isnan(fdm.ltpprz_to_body_eulers.psi)) { fdm.nan_count++; } + if (isnan(fdm.body_ecef_rotvel.p)) { fdm.nan_count++; } + if (isnan(fdm.body_ecef_rotvel.q)) { fdm.nan_count++; } + if (isnan(fdm.body_ecef_rotvel.r)) { fdm.nan_count++; } + if (isnan(fdm.body_ecef_rotaccel.p)) { fdm.nan_count++; } + if (isnan(fdm.body_ecef_rotaccel.q)) { fdm.nan_count++; } + if (isnan(fdm.body_ecef_rotaccel.r)) { fdm.nan_count++; } + if (isnan(fdm.ltp_g.x)) { fdm.nan_count++; } + if (isnan(fdm.ltp_g.y)) { fdm.nan_count++; } + if (isnan(fdm.ltp_g.z)) { fdm.nan_count++; } + if (isnan(fdm.ltp_h.x)) { fdm.nan_count++; } + if (isnan(fdm.ltp_h.y)) { fdm.nan_count++; } + if (isnan(fdm.ltp_h.z)) { fdm.nan_count++; } return (fdm.nan_count - orig_nan_count); } diff --git a/sw/simulator/nps/nps_flightgear.c b/sw/simulator/nps/nps_flightgear.c index c14a47a03f..3cd2895189 100644 --- a/sw/simulator/nps/nps_flightgear.c +++ b/sw/simulator/nps/nps_flightgear.c @@ -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 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 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 to continue (or CTRL-Z to suspend).\nEnter a new time factor if needed (current: %f): ", nps_main.host_time_factor); + printf("Press 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 e.g. 127.0.0.1\n" -" --fg_port e.g. 5501\n" -" --fg_time_offset e.g. 21600 for 6h\n" -" -j --js_dev e.g. 1 (default 0)\n" -" --spektrum_dev e.g. /dev/ttyUSB0\n" -" --rc_script e.g. 0\n" -" --ivy_bus e.g. 127.255.255.255\n" -" --time_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 e.g. 127.0.0.1\n" + " --fg_port e.g. 5501\n" + " --fg_time_offset e.g. 21600 for 6h\n" + " -j --js_dev e.g. 1 (default 0)\n" + " --spektrum_dev e.g. /dev/ttyUSB0\n" + " --rc_script e.g. 0\n" + " --ivy_bus e.g. 127.255.255.255\n" + " --time_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: diff --git a/sw/simulator/nps/nps_radio_control.c b/sw/simulator/nps/nps_radio_control.c index cd28f2e6d5..fa35ee97b4 100644 --- a/sw/simulator/nps/nps_radio_control.c +++ b/sw/simulator/nps/nps_radio_control.c @@ -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.; } diff --git a/sw/simulator/nps/nps_radio_control.h b/sw/simulator/nps/nps_radio_control.h index efde2b287b..2fd1663dc7 100644 --- a/sw/simulator/nps/nps_radio_control.h +++ b/sw/simulator/nps/nps_radio_control.h @@ -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; diff --git a/sw/simulator/nps/nps_radio_control_joystick.c b/sw/simulator/nps/nps_radio_control_joystick.c index 88b3d1dab9..4ae1af7bbc 100644 --- a/sw/simulator/nps/nps_radio_control_joystick.c +++ b/sw/simulator/nps/nps_radio_control_joystick.c @@ -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"); diff --git a/sw/simulator/nps/nps_radio_control_joystick.h b/sw/simulator/nps/nps_radio_control_joystick.h index 9b32ba581c..d485d052d5 100644 --- a/sw/simulator/nps/nps_radio_control_joystick.h +++ b/sw/simulator/nps/nps_radio_control_joystick.h @@ -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 { diff --git a/sw/simulator/nps/nps_radio_control_spektrum.c b/sw/simulator/nps/nps_radio_control_spektrum.c index 55f9602624..16cfc0dfab 100644 --- a/sw/simulator/nps/nps_radio_control_spektrum.c +++ b/sw/simulator/nps/nps_radio_control_spektrum.c @@ -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; ix += 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 #include #include -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; } diff --git a/sw/simulator/nps/nps_random.h b/sw/simulator/nps/nps_random.h index e31c1954d7..19102bd65f 100644 --- a/sw/simulator/nps/nps_random.h +++ b/sw/simulator/nps/nps_random.h @@ -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);