SITL: fix executable permission and trailing whitespace
This commit is contained in:
parent
231c15a3f9
commit
b835395cf9
42
libraries/SITL/SIM_SilentWings.cpp
Executable file → Normal file
42
libraries/SITL/SIM_SilentWings.cpp
Executable file → Normal 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
24
libraries/SITL/SIM_SilentWings.h
Executable file → Normal 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;
|
||||||
|
0
libraries/SITL/examples/SilentWings/Missions/Plockton.waypoints
Executable file → Normal file
0
libraries/SITL/examples/SilentWings/Missions/Plockton.waypoints
Executable file → Normal file
0
libraries/SITL/examples/SilentWings/Missions/Starmoen.waypoints
Executable file → Normal file
0
libraries/SITL/examples/SilentWings/Missions/Starmoen.waypoints
Executable file → Normal file
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
Block a user