Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 08:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: added ship takeoff/landing simulation

Browse Source 
this allows for a ship with a given radius and speed, allowing for
testing of ship takeoff and landing systems
This commit is contained in:
Andrew Tridgell 2020-03-18 10:07:11 +11:00
parent 91aaa7876d
commit f9f8822e47
6 changed files with 335 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/SITL/SIM_Aircraft.cpp
View File

@ -532,6 +532,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
}
position.z = -(ground_level + frame_height - home.alt * 0.01f + ground_height_difference());
// get speed of ground movement (for ship takeoff/landing)
float yaw_rate = 0;
const Vector2f ship_movement = sitl->shipsim.get_ground_speed_adjustment(location, yaw_rate);
const Vector3f gnd_movement(ship_movement.x, ship_movement.y, 0);
switch (ground_behavior) {
case GROUND_BEHAVIOR_NONE:
break;
@ -539,10 +544,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
// zero roll/pitch, but keep yaw
float r, p, y;
dcm.to_euler(&r, &p, &y);
y = y + yaw_rate * delta_time;
dcm.from_euler(0.0f, 0.0f, y);
// no X or Y movement
velocity_ef.x = 0.0f;
velocity_ef.y = 0.0f;
// X, Y movement tracks ground movement
velocity_ef.x = gnd_movement.x;
velocity_ef.y = gnd_movement.y;
if (velocity_ef.z > 0.0f) {
velocity_ef.z = 0.0f;
}
@ -561,6 +567,7 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
} else {
p = MAX(p, 0);
}
y = y + yaw_rate * delta_time;
dcm.from_euler(0.0f, p, y);
// only fwd movement
Vector3f v_bf = dcm.transposed() * velocity_ef;
@ -568,11 +575,25 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
if (v_bf.x < 0.0f) {
v_bf.x = 0.0f;
}
Vector3f gnd_movement_bf = dcm.transposed() * gnd_movement;
// lateral speed equals ground movement
v_bf.y = gnd_movement_bf.y;
if (!gnd_movement_bf.is_zero()) {
// fwd speed slowly approaches ground movement to simulate wheel friction
const float tconst = 20; // seconds
const float alpha = delta_time/(delta_time+tconst/M_2PI);
v_bf.x += (gnd_movement.x - v_bf.x) * alpha;
}
velocity_ef = dcm * v_bf;
if (velocity_ef.z > 0.0f) {
velocity_ef.z = 0.0f;
}
gyro.zero();
gyro.z = yaw_rate;
use_smoothing = true;
break;
}
@ -580,11 +601,15 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
// point straight up
float r, p, y;
dcm.to_euler(&r, &p, &y);
y = y + yaw_rate * delta_time;
dcm.from_euler(0.0f, radians(90), y);
// no movement
if (accel_earth.z > -1.1*GRAVITY_MSS) {
velocity_ef.zero();
}
// X, Y movement tracks ground movement
velocity_ef.x = gnd_movement.x;
velocity_ef.y = gnd_movement.y;
gyro.zero();
use_smoothing = true;
break;
@ -816,6 +841,8 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
if (richenpower) {
richenpower->update(input);
}
sitl->shipsim.update();
}
void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel)

1
libraries/SITL/SIM_QuadPlane.cpp
View File

@ -127,6 +127,7 @@ void QuadPlane::update(const struct sitl_input &input)
accel_body += quad_accel_body;
update_dynamics(rot_accel);
update_external_payload(input);
// update lat/lon/altitude
update_position();

213
libraries/SITL/SIM_Ship.cpp Normal file
View File

@ -0,0 +1,213 @@
/*
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 ship takeoff/landing
*/
#include "SIM_Ship.h"
#include "SITL.h"
#include <stdio.h>
#include "SIM_Aircraft.h"
#include <AP_HAL_SITL/SITL_State.h>
// use a spare channel for send. This is static to avoid mavlink
// header import in SIM_Ship.h
static const mavlink_channel_t mavlink_ch = (mavlink_channel_t)(MAVLINK_COMM_0+6);
using namespace SITL;
// SITL Ship parameters
const AP_Param::GroupInfo ShipSim::var_info[] = {
AP_GROUPINFO("ENABLE", 1, ShipSim, enable, 0),
AP_GROUPINFO("SPEED", 2, ShipSim, speed, 3),
AP_GROUPINFO("PSIZE", 3, ShipSim, path_size, 1000),
AP_GROUPINFO("SYSID", 4, ShipSim, sys_id, 17),
AP_GROUPINFO("DSIZE", 5, ShipSim, deck_size, 10),
AP_GROUPEND
};
/*
update a simulated vehicle
*/
void Ship::update(float delta_t)
{
// acceletate over time to keep EKF happy
const float max_accel = 3.0;
const float dspeed_max = max_accel * delta_t;
speed = constrain_float(sim->speed.get(), speed-dspeed_max, speed+dspeed_max);
// calculate how far around the circle we go
float circumference = M_PI * sim->path_size.get();
float dist = delta_t * speed;
float dangle = (dist / circumference) * 360.0;
if (delta_t > 0) {
yaw_rate = radians(dangle) / delta_t;
}
heading_deg += dangle;
heading_deg = wrap_360(heading_deg);
Vector2f dpos(dist, 0);
dpos.rotate(radians(heading_deg));
position += dpos;
}
ShipSim::ShipSim()
{
if (!valid_channel(mavlink_ch)) {
AP_HAL::panic("Invalid mavlink channel for ShipSim");
}
AP_Param::setup_object_defaults(this, var_info);
}
/*
get ground speed adjustment if we are landed on the ship
*/
Vector2f ShipSim::get_ground_speed_adjustment(const Location &loc, float &yaw_rate)
{
if (!enable) {
yaw_rate = 0;
return Vector2f(0,0);
}
Location shiploc = home;
shiploc.offset(ship.position.x, ship.position.y);
if (loc.get_distance(shiploc) > deck_size) {
yaw_rate = 0;
return Vector2f(0,0);
}
Vector2f vel(ship.speed, 0);
vel.rotate(radians(ship.heading_deg));
yaw_rate = ship.yaw_rate;
return vel;
}
/*
update the ShipSim peripheral state
*/
void ShipSim::update(void)
{
if (!enable) {
return;
}
auto *sitl = AP::sitl();
uint32_t now_us = AP_HAL::micros();
if (!initialised) {
home = sitl->state.home;
if (home.lat == 0 && home.lng == 0) {
return;
}
initialised = true;
::printf("ShipSim home %f %f\n", home.lat*1.0e-7, home.lng*1.0e-7);
ship.sim = this;
last_update_us = now_us;
last_report_ms = AP_HAL::millis();
}
float dt = (now_us - last_update_us)*1.0e-6;
last_update_us = now_us;
ship.update(dt);
uint32_t now_ms = AP_HAL::millis();
if (now_ms - last_report_ms >= reporting_period_ms) {
last_report_ms = now_ms;
send_report();
}
}
/*
send a report to the vehicle control code over MAVLink
*/
void ShipSim::send_report(void)
{
if (!mavlink_connected && mav_socket.connect(target_address, target_port)) {
::printf("ShipSim connected to %s:%u\n", target_address, (unsigned)target_port);
mavlink_connected = true;
}
if (!mavlink_connected) {
return;
}
uint32_t now = AP_HAL::millis();
mavlink_message_t msg;
uint16_t len;
uint8_t buf[300];
const uint8_t component_id = MAV_COMP_ID_USER10;
if (now - last_heartbeat_ms >= 1000) {
last_heartbeat_ms = now;
mavlink_msg_heartbeat_pack_chan(sys_id.get(),
component_id,
mavlink_ch,
&msg,
MAV_TYPE_SURFACE_BOAT,
MAV_AUTOPILOT_INVALID,
0,
0,
0);
len = mavlink_msg_to_send_buffer(buf, &msg);
mav_socket.send(buf, len);
}
/*
send a GLOBAL_POSITION_INT messages
*/
Location loc = home;
loc.offset(ship.position.x, ship.position.y);
int32_t alt;
bool have_alt = false;
#if AP_TERRAIN_AVAILABLE
auto &terrain = AP::terrain();
float height;
if (terrain.enabled() && terrain.height_amsl(loc, height, true)) {
alt = height * 1000;
have_alt = true;
}
#endif
if (!have_alt) {
// assume home altitude
alt = home.alt;
}
Vector2f vel(ship.speed, 0);
vel.rotate(radians(ship.heading_deg));
mavlink_msg_global_position_int_pack_chan(sys_id,
component_id,
mavlink_ch,
&msg,
now,
loc.lat,
loc.lng,
alt,
0,
vel.x*100,
vel.y*100,
0,
ship.heading_deg*100);
len = mavlink_msg_to_send_buffer(buf, &msg);
if (len > 0) {
mav_socket.send(buf, len);
}
}

86
libraries/SITL/SIM_Ship.h Normal file
View File

@ -0,0 +1,86 @@
/*
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 ship takeoff and landing
*/
#pragma once
#include <AP_HAL/utility/Socket.h>
#include <AP_Math/AP_Math.h>
#include <AP_Common/Location.h>
namespace SITL {
/*
a class for individual simulated vehicles
*/
class ShipSim;
class Ship {
friend class ShipSim;
private:
void update(float delta_t);
Vector2f position;
float heading_deg;
float yaw_rate;
float speed;
ShipSim *sim;
};
class ShipSim {
public:
friend class Ship;
ShipSim();
void update(void);
static const struct AP_Param::GroupInfo var_info[];
/*
get a ground speed adjustment for a landed vehicle based on
whether it is on a ship
*/
Vector2f get_ground_speed_adjustment(const Location &loc, float &yaw_rate);
private:
AP_Int8 enable;
AP_Float speed;
AP_Float path_size;
AP_Float deck_size;
AP_Int8 sys_id;
Location home;
const char *target_address = "127.0.0.1";
const uint16_t target_port = 5762;
bool initialised;
Ship ship;
uint32_t last_update_us;
// reporting period in ms
const float reporting_period_ms = 200;
uint32_t last_report_ms;
uint32_t last_heartbeat_ms;
SocketAPM mav_socket { false };
bool mavlink_connected;
void send_report(void);
};
} // namespace SITL

1
libraries/SITL/SITL.cpp
View File

@ -77,6 +77,7 @@ const AP_Param::GroupInfo SITL::var_info[] = {
AP_GROUPINFO("FLOW_POS", 56, SITL, optflow_pos_offset, 0),
AP_GROUPINFO("ACC2_BIAS", 57, SITL, accel2_bias, 0),
AP_GROUPINFO("ENGINE_FAIL", 58, SITL, engine_fail, 0),
AP_SUBGROUPINFO(shipsim, "SHIP_", 59, SITL, ShipSim),
AP_SUBGROUPEXTENSION("", 60, SITL, var_mag),
AP_SUBGROUPEXTENSION("", 61, SITL, var_gps),
AP_SUBGROUPEXTENSION("", 62, SITL, var_info3),

4
libraries/SITL/SITL.h
View File

@ -18,6 +18,7 @@
#include "SIM_ToneAlarm.h"
#include "SIM_EFI_MegaSquirt.h"
#include "SIM_RichenPower.h"
#include "SIM_Ship.h"
namespace SITL {
@ -350,6 +351,9 @@ public:
Sprayer sprayer_sim;
// simulated ship takeoffs
ShipSim shipsim;
Gripper_Servo gripper_sim;
Gripper_EPM gripper_epm_sim;