diff --git a/libraries/SITL/SIM_SilentWings.cpp b/libraries/SITL/SIM_SilentWings.cpp old mode 100755 new mode 100644 index b6068e8597..424108e5ff --- a/libraries/SITL/SIM_SilentWings.cpp +++ b/libraries/SITL/SIM_SilentWings.cpp @@ -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 diff --git a/libraries/SITL/SIM_SilentWings.h b/libraries/SITL/SIM_SilentWings.h old mode 100755 new mode 100644 index a112c7e045..50739387a3 --- a/libraries/SITL/SIM_SilentWings.h +++ b/libraries/SITL/SIM_SilentWings.h @@ -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; diff --git a/libraries/SITL/examples/SilentWings/Missions/Plockton.waypoints b/libraries/SITL/examples/SilentWings/Missions/Plockton.waypoints old mode 100755 new mode 100644 diff --git a/libraries/SITL/examples/SilentWings/Missions/Starmoen.waypoints b/libraries/SITL/examples/SilentWings/Missions/Starmoen.waypoints old mode 100755 new mode 100644 diff --git a/libraries/SITL/examples/SilentWings/Situations/ASW27B_Plockton_1000.stn b/libraries/SITL/examples/SilentWings/Situations/ASW27B_Plockton_1000.stn old mode 100755 new mode 100644 index 873331aad9..b3ebe15235 --- a/libraries/SITL/examples/SilentWings/Situations/ASW27B_Plockton_1000.stn +++ b/libraries/SITL/examples/SilentWings/Situations/ASW27B_Plockton_1000.stn @@ -71,4 +71,3 @@ weather { } } - diff --git a/libraries/SITL/examples/SilentWings/Situations/LS-8b_Starmoen_800.stn b/libraries/SITL/examples/SilentWings/Situations/LS-8b_Starmoen_800.stn old mode 100755 new mode 100644 index e9369faaeb..c067e65bae --- a/libraries/SITL/examples/SilentWings/Situations/LS-8b_Starmoen_800.stn +++ b/libraries/SITL/examples/SilentWings/Situations/LS-8b_Starmoen_800.stn @@ -113,4 +113,3 @@ weather { } } -