-
Notifications
You must be signed in to change notification settings - Fork 17.7k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
SITL: Add simulator for tethered vehicle
- Loading branch information
1 parent
b7ace6c
commit e10cbb3
Showing
8 changed files
with
444 additions
and
17 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,309 @@ | ||
| /* | ||
| This program is free software: you can redistribute it and/or modify | ||
| it under the terms of the GNU General Public License as published by | ||
| the Free Software Foundation, either version 3 of the License, or | ||
| (at your option) any later version. | ||
| This program is distributed in the hope that it will be useful, | ||
| but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
| GNU General Public License for more details. | ||
| You should have received a copy of the GNU General Public License | ||
| along with this program. If not, see <http://www.gnu.org/licenses/>. | ||
| */ | ||
| /* | ||
| simulate a static tether attached to the vehicle and ground | ||
| */ | ||
|
|
||
| #include "SIM_config.h" | ||
|
|
||
| #if AP_SIM_TETHER_ENABLED | ||
|
|
||
| #include "SIM_Tether.h" | ||
| #include "SITL.h" | ||
| #include <stdio.h> | ||
| #include "SIM_Aircraft.h" | ||
| #include <AP_HAL_SITL/SITL_State.h> | ||
| #include <GCS_MAVLink/GCS.h> | ||
| #include <AP_Logger/AP_Logger.h> | ||
|
|
||
| using namespace SITL; | ||
|
|
||
| // TetherSim parameters | ||
| const AP_Param::GroupInfo TetherSim::var_info[] = { | ||
| // @Param: ENABLE | ||
| // @DisplayName: Tether Simulation Enable/Disable | ||
| // @Description: Enable or disable the tether simulation | ||
| // @Values: 0:Disabled,1:Enabled | ||
| // @User: Advanced | ||
| AP_GROUPINFO_FLAGS("ENABLE", 1, TetherSim, enable, 0, AP_PARAM_FLAG_ENABLE), | ||
|
|
||
| // @Param: DENSITY | ||
| // @DisplayName: Tether Wire Density | ||
| // @Description: Linear mass density of the tether wire | ||
| // @Range: 0 1 | ||
| // @User: Advanced | ||
| AP_GROUPINFO("DENSITY", 2, TetherSim, tether_linear_density, 0.0167), | ||
|
|
||
| // @Param: LINELEN | ||
| // @DisplayName: Tether Maximum Line Length | ||
| // @Description: Maximum length of the tether line in meters | ||
| // @Units: m | ||
| // @Range: 0 100 | ||
| // @User: Advanced | ||
| AP_GROUPINFO("LINELEN", 3, TetherSim, max_line_length, 100.0), | ||
|
|
||
| // @Param: SYSID | ||
| // @DisplayName: Tether Simulation MAVLink System ID | ||
| // @Description: MAVLink system ID for the tether simulation, used to distinguish it from other systems on the network | ||
| // @Range: 0 255 | ||
| // @User: Advanced | ||
| AP_GROUPINFO("SYSID", 4, TetherSim, sys_id, 2), | ||
|
|
||
| // @Param: STUCK | ||
| // @DisplayName: Tether Stuck Enable/Disable | ||
| // @Description: Enable or disable a stuck tether simulation | ||
| // @Values: 0:Disabled,1:Enabled | ||
| // @User: Advanced | ||
| AP_GROUPINFO("STUCK", 5, TetherSim, tether_stuck, 0), | ||
|
|
||
| // @Param: SPGCNST | ||
| // @DisplayName: Tether Spring Constant | ||
| // @Description: Spring constant for the tether to simulate elastic forces when stretched beyond its maximum length | ||
| // @Range: 0 255 | ||
| // @User: Advanced | ||
| AP_GROUPINFO("SPGCNST", 6, TetherSim, tether_spring_constant, 100), | ||
|
|
||
| AP_GROUPEND | ||
| }; | ||
|
|
||
| // TetherSim handles interaction with main vehicle | ||
| TetherSim::TetherSim() | ||
| { | ||
| AP_Param::setup_object_defaults(this, var_info); | ||
| } | ||
|
|
||
|
|
||
| void TetherSim::update(const Location& veh_pos) | ||
| { | ||
| if (!enable) { | ||
| return; | ||
| } | ||
|
|
||
| if (veh_pos.is_zero()) { | ||
| return; | ||
| } | ||
|
|
||
| // initialise fixed tether ground location | ||
| const uint32_t now_us = AP_HAL::micros(); | ||
| if (!initialised) { | ||
| // capture EKF origin | ||
| auto *sitl = AP::sitl(); | ||
| const Location ekf_origin = sitl->state.home; | ||
| if (ekf_origin.lat == 0 && ekf_origin.lng == 0) { | ||
| return; | ||
| } | ||
|
|
||
| // more initialisation | ||
| last_update_us = now_us; | ||
| initialised = true; | ||
| } | ||
|
|
||
| // calculate dt and update tether forces | ||
| const float dt = (now_us - last_update_us)*1.0e-6; | ||
| last_update_us = now_us; | ||
|
|
||
| update_tether_force(veh_pos, dt); | ||
|
|
||
| // send tether location to GCS at 5hz | ||
| // TO-Do: add provision to make the tether movable | ||
| const uint32_t now_ms = AP_HAL::millis(); | ||
| if (now_ms - last_report_ms >= reporting_period_ms) { | ||
| last_report_ms = now_ms; | ||
| send_report(); | ||
| write_log(); | ||
| } | ||
| } | ||
|
|
||
| // get earth-frame forces on the vehicle from the tether | ||
| // returns true on success and fills in forces_ef argument, false on failure | ||
| bool TetherSim::get_forces_on_vehicle(Vector3f& forces_ef) const | ||
| { | ||
| if (!enable) { | ||
| return false; | ||
| } | ||
|
|
||
| forces_ef = veh_forces_ef; | ||
| return true; | ||
| } | ||
|
|
||
| // send a report to the vehicle control code over MAVLink | ||
| void TetherSim::send_report(void) | ||
| { | ||
| if (!mavlink_connected && mav_socket.connect(target_address, target_port)) { | ||
| ::printf("Tether System connected to %s:%u\n", target_address, (unsigned)target_port); | ||
| mavlink_connected = true; | ||
| } | ||
| if (!mavlink_connected) { | ||
| return; | ||
| } | ||
|
|
||
| // get current time | ||
| uint32_t now_ms = AP_HAL::millis(); | ||
|
|
||
| // send heartbeat at 1hz | ||
| const uint8_t component_id = MAV_COMP_ID_USER11; | ||
| if (now_ms - last_heartbeat_ms >= 1000) { | ||
| last_heartbeat_ms = now_ms; | ||
|
|
||
| const mavlink_heartbeat_t heartbeat{ | ||
| custom_mode: 0, | ||
| type : MAV_TYPE_GROUND_ROVER, | ||
| autopilot : MAV_AUTOPILOT_INVALID, | ||
| base_mode: 0, | ||
| system_status: 0, | ||
| mavlink_version: 0, | ||
| }; | ||
|
|
||
| mavlink_message_t msg; | ||
| mavlink_msg_heartbeat_encode_status( | ||
| sys_id.get(), | ||
| component_id, | ||
| &mav_status, | ||
| &msg, | ||
| &heartbeat); | ||
| uint8_t buf[300]; | ||
| const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); | ||
| mav_socket.send(buf, len); | ||
| } | ||
|
|
||
| // send a GLOBAL_POSITION_INT messages | ||
| { | ||
| Location tether_anchor_loc; | ||
| int32_t alt_amsl_cm, alt_rel_cm; | ||
| if (!get_tether_ground_location(tether_anchor_loc) || | ||
| !tether_anchor_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm) || | ||
| !tether_anchor_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_rel_cm)) { | ||
| return; | ||
| } | ||
| const mavlink_global_position_int_t global_position_int{ | ||
| time_boot_ms: now_ms, | ||
| lat: tether_anchor_loc.lat, | ||
| lon: tether_anchor_loc.lng, | ||
| alt: alt_amsl_cm * 10, // amsl alt in mm | ||
| relative_alt: alt_rel_cm * 10, // relative alt in mm | ||
| vx: 0, // velocity in cm/s | ||
| vy: 0, // velocity in cm/s | ||
| vz: 0, // velocity in cm/s | ||
| hdg: 0 // heading in centi-degrees | ||
| }; | ||
| mavlink_message_t msg; | ||
| mavlink_msg_global_position_int_encode_status(sys_id, component_id, &mav_status, &msg, &global_position_int); | ||
| uint8_t buf[300]; | ||
| const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); | ||
| if (len > 0) { | ||
| mav_socket.send(buf, len); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| void TetherSim::write_log() | ||
| { | ||
| #if HAL_LOGGING_ENABLED | ||
| // write log of tether state | ||
| // @LoggerMessage: TETH | ||
| // @Description: Tether state | ||
| // @Field: TimeUS: Time since system startup | ||
| // @Field: Len: Tether length | ||
| // @Field: VFN: Force on vehicle in North direction | ||
| // @Field: VFE: Force on vehicle in East direction | ||
| // @Field: VFD: Force on vehicle in Down direction | ||
| AP::logger().WriteStreaming("TETH", | ||
| "TimeUS,Len,VFN,VFE,VFD", // labels | ||
| "s----", // units | ||
| "F----", // multipliers | ||
| "Qffff", // format | ||
| AP_HAL::micros64(), | ||
| (float)tether_length, | ||
| (double)veh_forces_ef.x, | ||
| (double)veh_forces_ef.y, | ||
| (double)veh_forces_ef.z); | ||
| #endif | ||
| } | ||
| // returns true on success and fills in tether_ground_loc argument, false on failure | ||
| bool TetherSim::get_tether_ground_location(Location& tether_ground_loc) const | ||
| { | ||
| // get EKF origin | ||
| auto *sitl = AP::sitl(); | ||
| if (sitl == nullptr) { | ||
| return false; | ||
| } | ||
| const Location ekf_origin = sitl->state.home; | ||
| if (ekf_origin.lat == 0 && ekf_origin.lng == 0) { | ||
| return false; | ||
| } | ||
|
|
||
| tether_ground_loc = ekf_origin; | ||
| return true; | ||
| } | ||
|
|
||
| void TetherSim::update_tether_force(const Location& veh_pos, float dt) | ||
| { | ||
|
|
||
| Location tether_anchor_loc; | ||
| if (!get_tether_ground_location(tether_anchor_loc)) { | ||
| return; | ||
| } | ||
|
|
||
| // Step 1: Calculate the vector from the tether anchor to the vehicle | ||
| Vector3f tether_vector = veh_pos.get_distance_NED(tether_anchor_loc); | ||
| tether_length = tether_vector.length(); | ||
|
|
||
| // Step 2: Check if tether is taut (length exceeds maximum allowed length) or stuck | ||
| if (tether_length > max_line_length) { | ||
|
|
||
| // Calculate the stretch beyond the maximum length | ||
| float stretch = MAX(tether_length - max_line_length, 0.0f); | ||
|
|
||
| // Apply a spring-like penalty force proportional to the stretch | ||
| float penalty_force_magnitude = tether_spring_constant * stretch; | ||
|
|
||
| // Direction of force is along the tether, pulling toward the anchor | ||
| veh_forces_ef = tether_vector.normalized() * penalty_force_magnitude; | ||
|
|
||
| GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Tether: Exceeded maximum length."); | ||
|
|
||
| return; | ||
| } | ||
|
|
||
| if (tether_stuck) { | ||
|
|
||
| // Calculate the stretch beyond the maximum length | ||
| float stretch = MAX(tether_length - tether_not_stuck_length, 0.0f); | ||
|
|
||
| // Apply a spring-like penalty force proportional to the stretch | ||
| float penalty_force_magnitude = tether_spring_constant * stretch; | ||
|
|
||
| // Direction of force is directly along the tether, towards the tether anchor point | ||
| veh_forces_ef = tether_vector.normalized() * penalty_force_magnitude; | ||
|
|
||
| GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Tether: Stuck."); | ||
|
|
||
| return; | ||
| } else { | ||
| tether_not_stuck_length = tether_length; | ||
| } | ||
|
|
||
| // Step 3: Calculate the weight of the tether being lifted | ||
| // The weight is proportional to the current tether length | ||
| const float tether_weight_force = tether_linear_density * tether_length * GRAVITY_MSS; | ||
|
|
||
| // Step 4: Calculate the tension force | ||
| Vector3f tension_force_NED = tether_vector.normalized() * tether_weight_force; | ||
|
|
||
| // Step 5: Apply the force to the vehicle | ||
| veh_forces_ef = tension_force_NED; | ||
| } | ||
|
|
||
| #endif |
Oops, something went wrong.