mirror of https://github.com/ArduPilot/ardupilot
SITL: fix executable permission and trailing whitespace
This commit is contained in:
parent
231c15a3f9
commit
b835395cf9
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
1
libraries/SITL/examples/SilentWings/Situations/ASW27B_Plockton_1000.stn
Executable file → Normal file
1
libraries/SITL/examples/SilentWings/Situations/ASW27B_Plockton_1000.stn
Executable file → Normal file
|
@ -71,4 +71,3 @@ weather {
|
|||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
1
libraries/SITL/examples/SilentWings/Situations/LS-8b_Starmoen_800.stn
Executable file → Normal file
1
libraries/SITL/examples/SilentWings/Situations/LS-8b_Starmoen_800.stn
Executable file → Normal file
|
@ -113,4 +113,3 @@ weather {
|
|||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue