SITL: fix executable permission and trailing whitespace

This commit is contained in:
Pierre Kancir 2022-05-24 16:20:17 +02:00 committed by Randy Mackay
parent 231c15a3f9
commit b835395cf9
6 changed files with 33 additions and 35 deletions

42
libraries/SITL/SIM_SilentWings.cpp Executable file → Normal file
View File

@ -130,7 +130,7 @@ void SilentWings::send_servos(const struct sitl_input &input)
fprintf(stderr, "Fatal: Failed to allocate enough space for data\n"), fprintf(stderr, "Fatal: Failed to allocate enough space for data\n"),
exit(1); exit(1);
} }
ssize_t sent = sock.sendto(buf, buflen, _sw_address, _sw_port); ssize_t sent = sock.sendto(buf, buflen, _sw_address, _sw_port);
free(buf); free(buf);
@ -138,7 +138,7 @@ void SilentWings::send_servos(const struct sitl_input &input)
fprintf(stderr, "Fatal: Failed to send on control socket\n"), fprintf(stderr, "Fatal: Failed to send on control socket\n"),
exit(1); exit(1);
} }
if (sent < buflen) { if (sent < buflen) {
fprintf(stderr, "Failed to send all bytes on control socket\n"); fprintf(stderr, "Failed to send all bytes on control socket\n");
} }
@ -155,12 +155,12 @@ bool SilentWings::recv_fdm(void)
memset(&pkt, 0, sizeof(pkt)); memset(&pkt, 0, sizeof(pkt));
ssize_t nread = sock.recv(&tmp_pkt, sizeof(pkt), 0); ssize_t nread = sock.recv(&tmp_pkt, sizeof(pkt), 0);
// nread == -1 (255) means no data has arrived // nread == -1 (255) means no data has arrived
if (nread != sizeof(pkt)) { if (nread != sizeof(pkt)) {
return false; return false;
} }
memcpy(&pkt, &tmp_pkt, sizeof(pkt)); memcpy(&pkt, &tmp_pkt, sizeof(pkt));
// data received successfully // data received successfully
@ -170,8 +170,8 @@ bool SilentWings::recv_fdm(void)
void SilentWings::process_packet() void SilentWings::process_packet()
{ {
// pkt.timestamp is the time of day in SilentWings, measured in ms // pkt.timestamp is the time of day in SilentWings, measured in ms
// since midnight. // since midnight.
// TO DO: check what happens when a flight in SW crosses midnight // TO DO: check what happens when a flight in SW crosses midnight
if (inited_first_pkt_timestamp) { if (inited_first_pkt_timestamp) {
uint64_t tus = (pkt.timestamp - first_pkt_timestamp_ms) * 1.0e3f; uint64_t tus = (pkt.timestamp - first_pkt_timestamp_ms) * 1.0e3f;
@ -182,8 +182,8 @@ void SilentWings::process_packet()
time_base_us = time_now_us; time_base_us = time_now_us;
inited_first_pkt_timestamp = true; inited_first_pkt_timestamp = true;
} }
dcm.from_euler(radians(pkt.roll), radians(pkt.pitch), radians(pkt.yaw)); dcm.from_euler(radians(pkt.roll), radians(pkt.pitch), radians(pkt.yaw));
accel_body = Vector3f(pkt.ax * GRAVITY_MSS, pkt.ay * GRAVITY_MSS, pkt.az * GRAVITY_MSS); // This is g-load. accel_body = Vector3f(pkt.ax * GRAVITY_MSS, pkt.ay * GRAVITY_MSS, pkt.az * GRAVITY_MSS); // This is g-load.
gyro = Vector3f(radians(pkt.d_roll), radians(pkt.d_pitch), radians(pkt.d_yaw)); gyro = Vector3f(radians(pkt.d_roll), radians(pkt.d_pitch), radians(pkt.d_yaw));
// SilentWings provides velocity in body frame. // SilentWings provides velocity in body frame.
@ -202,7 +202,7 @@ void SilentWings::process_packet()
position.y = posdelta.y; position.y = posdelta.y;
position.z = posdelta.z; position.z = posdelta.z;
update_position(); update_position();
// In case Silent Wings' reported location and our location calculated using an offset from the home location diverge, we need // In case Silent Wings' reported location and our location calculated using an offset from the home location diverge, we need
// to reset the home location. // to reset the home location.
if (curr_location.get_distance(location) > 4 || abs(curr_location.alt - location.alt)*0.01f > 2.0f || !home_initialized) { if (curr_location.get_distance(location) > 4 || abs(curr_location.alt - location.alt)*0.01f > 2.0f || !home_initialized) {
@ -222,21 +222,21 @@ void SilentWings::process_packet()
home_initialized = true; home_initialized = true;
update_position(); update_position();
} }
// Auto-adjust to Silent Wings' frame rate // Auto-adjust to Silent Wings' frame rate
// This affects the data rate (without this adjustment, the data rate is // This affects the data rate (without this adjustment, the data rate is
// low no matter what the output_udp_rate in SW's options.dat file is). // low no matter what the output_udp_rate in SW's options.dat file is).
double deltat = (AP_HAL::millis() - last_data_time_ms) / 1000.0f; double deltat = (AP_HAL::millis() - last_data_time_ms) / 1000.0f;
if (deltat < 0.01 && deltat > 0) { if (deltat < 0.01 && deltat > 0) {
adjust_frame_time(1.0/deltat); adjust_frame_time(1.0/deltat);
} }
last_data_time_ms = AP_HAL::millis(); last_data_time_ms = AP_HAL::millis();
report.data_count++; report.data_count++;
report.frame_count++; report.frame_count++;
if (0) { if (0) {
printf("Delta: %f Time: %" PRIu64 "\n", deltat, time_now_us); printf("Delta: %f Time: %" PRIu64 "\n", deltat, time_now_us);
printf("Accel.x %f\n", accel_body.x); printf("Accel.x %f\n", accel_body.x);
@ -289,7 +289,7 @@ void SilentWings::update(const struct sitl_input &input)
// Time has been advanced by interim_update(.) // Time has been advanced by interim_update(.)
send_servos(input); send_servos(input);
} }
// This clause is triggered if and only if we haven't received // This clause is triggered if and only if we haven't received
// any data packets yet (and therefore didn't attempt // any data packets yet (and therefore didn't attempt
// extrapolating data via interim_update(.) either). // extrapolating data via interim_update(.) either).
@ -301,16 +301,16 @@ void SilentWings::update(const struct sitl_input &input)
sync_frame_time(); sync_frame_time();
} }
} }
update_mag_field_bf(); update_mag_field_bf();
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if (report.last_report_ms == 0) { if (report.last_report_ms == 0) {
report.last_report_ms = now; report.last_report_ms = now;
printf("Resetting last report time to now\n"); printf("Resetting last report time to now\n");
} }
if (now - report.last_report_ms > 5000) { if (now - report.last_report_ms > 5000) {
float dt = (now - report.last_report_ms) * 1.0e-3f; float dt = (now - report.last_report_ms) * 1.0e-3f;
printf("Data rate: %.1f FPS Frame rate: %.1f FPS\n", printf("Data rate: %.1f FPS Frame rate: %.1f FPS\n",
@ -318,7 +318,7 @@ void SilentWings::update(const struct sitl_input &input)
report.last_report_ms = now; report.last_report_ms = now;
report.data_count = 0; report.data_count = 0;
report.frame_count = 0; report.frame_count = 0;
} }
} }
#endif // HAL_SIM_SILENTWINGS_ENABLED #endif // HAL_SIM_SILENTWINGS_ENABLED

24
libraries/SITL/SIM_SilentWings.h Executable file → Normal file
View File

@ -52,7 +52,7 @@ private:
double position_latitude; // Degrees Position latitude, double position_latitude; // Degrees Position latitude,
double position_longitude; // Degrees longitude, double position_longitude; // Degrees longitude,
float altitude_msl; // m Altitude w.r.t. the sea level float altitude_msl; // m Altitude w.r.t. the sea level
float altitude_ground; // m Altitude w.r.t. the ground level float altitude_ground; // m Altitude w.r.t. the ground level
float altitude_ground_45; // m Ground 45 degrees ahead (NOT IMPLEMENTED YET) float altitude_ground_45; // m Ground 45 degrees ahead (NOT IMPLEMENTED YET)
float altitude_ground_forward; // m Ground straight ahead (NOT IMPLEMENTED YET) float altitude_ground_forward; // m Ground straight ahead (NOT IMPLEMENTED YET)
float roll; // Degrees float roll; // Degrees
@ -62,11 +62,11 @@ private:
float d_pitch; // Deg/sec Pitch speed float d_pitch; // Deg/sec Pitch speed
float d_yaw; // Deg/sec Yaw speed float d_yaw; // Deg/sec Yaw speed
float vx; // m/s Velocity vector in body-axis float vx; // m/s Velocity vector in body-axis
float vy; float vy;
float vz; float vz;
float vx_wind; // m/s Velocity vector in body-axis, relative to the wind float vx_wind; // m/s Velocity vector in body-axis, relative to the wind
float vy_wind; float vy_wind;
float vz_wind; float vz_wind;
float v_eas; // m/s Equivalent (indicated) air speed. float v_eas; // m/s Equivalent (indicated) air speed.
float ax; // m/s^2 Acceleration vector in body axis float ax; // m/s^2 Acceleration vector in body axis
float ay; float ay;
@ -80,7 +80,7 @@ private:
float density; // Air density at aircraft altitude float density; // Air density at aircraft altitude
float temperature; // Degrees Celcius Air temperature at aircraft altitude float temperature; // Degrees Celcius Air temperature at aircraft altitude
} pkt; } pkt;
struct { struct {
uint32_t last_report_ms; uint32_t last_report_ms;
uint32_t data_count; uint32_t data_count;
@ -90,25 +90,25 @@ private:
bool recv_fdm(void); bool recv_fdm(void);
void process_packet(); void process_packet();
bool interim_update(); bool interim_update();
/* Create and set in/out socket for Silent Wings simulator */ /* Create and set in/out socket for Silent Wings simulator */
void set_interface_ports(const char* address, const int port_in, const int port_out) override; void set_interface_ports(const char* address, const int port_in, const int port_out) override;
/* Sends control inputs to the Silent Wings. */ /* Sends control inputs to the Silent Wings. */
void send_servos(const struct sitl_input &input); void send_servos(const struct sitl_input &input);
/* Timestamp of the latest data packet received from Silent Wings. */ /* Timestamp of the latest data packet received from Silent Wings. */
uint32_t last_data_time_ms; uint32_t last_data_time_ms;
/* Timestamp of the first data packet received from Silent Wings. */ /* Timestamp of the first data packet received from Silent Wings. */
uint32_t first_pkt_timestamp_ms; uint32_t first_pkt_timestamp_ms;
/* Indicates whether first_pkt_timestamp_ms has been initialized (i.e., any packets have been received from Silent Wings. */ /* Indicates whether first_pkt_timestamp_ms has been initialized (i.e., any packets have been received from Silent Wings. */
bool inited_first_pkt_timestamp; bool inited_first_pkt_timestamp;
/* ArduPlane's internal time when the first packet from Silent Wings is received. */ /* ArduPlane's internal time when the first packet from Silent Wings is received. */
uint64_t time_base_us; uint64_t time_base_us;
SocketAPM sock; SocketAPM sock;
const char *_sw_address = "127.0.0.1"; const char *_sw_address = "127.0.0.1";
int _port_in = 6060; int _port_in = 6060;

View File

View File

View File

@ -71,4 +71,3 @@ weather {
} }
} }

View File

@ -113,4 +113,3 @@ weather {
} }
} }