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"),
exit(1);
}
ssize_t sent = sock.sendto(buf, buflen, _sw_address, _sw_port);
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"),
exit(1);
}
if (sent < buflen) {
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));
ssize_t nread = sock.recv(&tmp_pkt, sizeof(pkt), 0);
// nread == -1 (255) means no data has arrived
if (nread != sizeof(pkt)) {
return false;
}
}
memcpy(&pkt, &tmp_pkt, sizeof(pkt));
// data received successfully
@ -170,8 +170,8 @@ bool SilentWings::recv_fdm(void)
void SilentWings::process_packet()
{
// pkt.timestamp is the time of day in SilentWings, measured in ms
// since midnight.
// pkt.timestamp is the time of day in SilentWings, measured in ms
// since midnight.
// TO DO: check what happens when a flight in SW crosses midnight
if (inited_first_pkt_timestamp) {
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;
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.
gyro = Vector3f(radians(pkt.d_roll), radians(pkt.d_pitch), radians(pkt.d_yaw));
// SilentWings provides velocity in body frame.
@ -202,7 +202,7 @@ void SilentWings::process_packet()
position.y = posdelta.y;
position.z = posdelta.z;
update_position();
// 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.
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;
update_position();
}
// Auto-adjust to Silent Wings' frame rate
// 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).
double deltat = (AP_HAL::millis() - last_data_time_ms) / 1000.0f;
if (deltat < 0.01 && deltat > 0) {
adjust_frame_time(1.0/deltat);
}
last_data_time_ms = AP_HAL::millis();
report.data_count++;
report.frame_count++;
if (0) {
printf("Delta: %f Time: %" PRIu64 "\n", deltat, time_now_us);
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(.)
send_servos(input);
}
// This clause is triggered if and only if we haven't received
// any data packets yet (and therefore didn't attempt
// extrapolating data via interim_update(.) either).
@ -301,16 +301,16 @@ void SilentWings::update(const struct sitl_input &input)
sync_frame_time();
}
}
update_mag_field_bf();
uint32_t now = AP_HAL::millis();
if (report.last_report_ms == 0) {
report.last_report_ms = now;
printf("Resetting last report time to now\n");
}
if (now - report.last_report_ms > 5000) {
float dt = (now - report.last_report_ms) * 1.0e-3f;
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.data_count = 0;
report.frame_count = 0;
}
}
}
#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_longitude; // Degrees longitude,
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_forward; // m Ground straight ahead (NOT IMPLEMENTED YET)
float roll; // Degrees
@ -62,11 +62,11 @@ private:
float d_pitch; // Deg/sec Pitch speed
float d_yaw; // Deg/sec Yaw speed
float vx; // m/s Velocity vector in body-axis
float vy;
float vz;
float vy;
float vz;
float vx_wind; // m/s Velocity vector in body-axis, relative to the wind
float vy_wind;
float vz_wind;
float vz_wind;
float v_eas; // m/s Equivalent (indicated) air speed.
float ax; // m/s^2 Acceleration vector in body axis
float ay;
@ -80,7 +80,7 @@ private:
float density; // Air density at aircraft altitude
float temperature; // Degrees Celcius Air temperature at aircraft altitude
} pkt;
struct {
uint32_t last_report_ms;
uint32_t data_count;
@ -90,25 +90,25 @@ private:
bool recv_fdm(void);
void process_packet();
bool interim_update();
/* 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;
/* Sends control inputs to the Silent Wings. */
void send_servos(const struct sitl_input &input);
/* Timestamp of the latest data packet received from Silent Wings. */
uint32_t last_data_time_ms;
/* Timestamp of the first data packet received from Silent Wings. */
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;
/* ArduPlane's internal time when the first packet from Silent Wings is received. */
uint64_t time_base_us;
SocketAPM sock;
const char *_sw_address = "127.0.0.1";
int _port_in = 6060;

View File

View File

View File

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

View File

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