Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: rearrange Grippers and Sprayers in SITL

Browse Source
This commit is contained in:
Pierre Kancir 2018-07-31 22:33:23 +10:00 committed by Peter Barker
parent 96c54550e5
commit 37f7cc4bcf
21 changed files with 295 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/SITL/SIM_Aircraft.cpp
View File

@ -84,7 +84,6 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
// allow for orientation settings, such as with tailsitters
enum ap_var_type ptype;
ahrs_orientation = (AP_Int8 *)AP_Param::find("AHRS_ORIENTATION", &ptype);
terrain = reinterpret_cast<AP_Terrain *>(AP_Param::find_object("TERRAIN_"));
}
@ -737,4 +736,24 @@ void Aircraft::extrapolate_sensors(float delta_time)
velocity_air_bf = dcm.transposed() * velocity_air_ef;
}
void Aircraft::update_external_payload(const struct sitl_input &input)
{
external_payload_mass = 0;
// update sprayer
if (sprayer && sprayer->is_enabled()) {
sprayer->update(input);
external_payload_mass += sprayer->payload_mass();
}
// update grippers
if (gripper && gripper->is_enabled()) {
gripper->set_alt(hagl());
gripper->update(input);
external_payload_mass += gripper->payload_mass();
}
if (gripper_epm && gripper_epm->is_enabled()) {
gripper_epm->update(input);
external_payload_mass += gripper_epm->payload_mass();
}
}

35
libraries/SITL/SIM_Aircraft.h
View File

@ -21,8 +21,11 @@
#include <AP_Math/AP_Math.h>
#include "SITL.h"
#include "SITL_Input.h"
#include <AP_Terrain/AP_Terrain.h>
#include "SIM_Sprayer.h"
#include "SIM_Gripper_Servo.h"
#include "SIM_Gripper_EPM.h"
namespace SITL {
@ -30,25 +33,9 @@ namespace SITL {
parent class for all simulator types
*/
class Aircraft {
friend class Gripper_Servo;
public:
Aircraft(const char *home_str, const char *frame_str);
/*
structure passed in giving servo positions as PWM values in
microseconds
*/
struct sitl_input {
uint16_t servos[16];
struct {
float speed; // m/s
float direction; // degrees 0..360
float turbulence;
float dir_z; //degrees -90..90
} wind;
};
/*
set simulation speedup
*/
@ -111,7 +98,7 @@ public:
return mag_bf;
}
virtual float gross_mass() const { return mass; }
float gross_mass() const { return mass + external_payload_mass; }
const Location &get_location() const { return location; }
@ -121,6 +108,10 @@ public:
attitude.from_rotation_matrix(dcm);
}
void set_sprayer(Sprayer *_sprayer) { sprayer = _sprayer; }
void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; }
void set_gripper_epm(Gripper_EPM *_gripper_epm) { gripper_epm = _gripper_epm; }
protected:
SITL *sitl;
Location home;
@ -139,6 +130,7 @@ protected:
Vector3f velocity_air_bf; // velocity relative to airmass, body frame
Vector3f position; // meters, NED from origin
float mass; // kg
float external_payload_mass = 0.0f; // kg
Vector3f accel_body; // m/s/s NED, body frame
float airspeed; // m/s, apparent airspeed
float airspeed_pitot; // m/s, apparent airspeed, as seen by fwd pitot tube
@ -235,6 +227,9 @@ protected:
// extrapolate sensors by a given delta time in seconds
void extrapolate_sensors(float delta_time);
// update external payload/sensor dynamic
void update_external_payload(const struct sitl_input &input);
private:
uint64_t last_time_us = 0;
uint32_t frame_counter = 0;
@ -253,6 +248,10 @@ private:
} smoothing;
LowPassFilterFloat servo_filter[4];
Sprayer *sprayer;
Gripper_Servo *gripper;
Gripper_EPM *gripper_epm;
};
} // namespace SITL

8
libraries/SITL/SIM_Calibration.cpp
View File

@ -30,7 +30,7 @@ SITL::Calibration::Calibration(const char *home_str, const char *frame_str)
mass = 1.5f;
}
void SITL::Calibration::update(const struct sitl_input& input)
void SITL::Calibration::update(const struct sitl_input &input)
{
Vector3f rot_accel{0, 0, 0};
@ -54,7 +54,7 @@ void SITL::Calibration::update(const struct sitl_input& input)
update_mag_field_bf();
}
void SITL::Calibration::_stop_control(const struct sitl_input& input,
void SITL::Calibration::_stop_control(const struct sitl_input &input,
Vector3f& rot_accel)
{
Vector3f desired_angvel{0, 0, 0};
@ -65,7 +65,7 @@ void SITL::Calibration::_stop_control(const struct sitl_input& input,
rot_accel *= 0.002f;
}
void SITL::Calibration::_attitude_control(const struct sitl_input& input,
void SITL::Calibration::_attitude_control(const struct sitl_input &input,
Vector3f& rot_accel)
{
float desired_roll = -M_PI + 2 * M_PI * (input.servos[5] - 1000) / 1000.f;
@ -103,7 +103,7 @@ void SITL::Calibration::_attitude_set(float desired_roll, float desired_pitch, f
rot_accel = error * (1.0f / dt);
}
void SITL::Calibration::_angular_velocity_control(const struct sitl_input& in,
void SITL::Calibration::_angular_velocity_control(const struct sitl_input &in,
Vector3f& rot_accel)
{
Vector3f axis{(float)(in.servos[5] - 1500),

8
libraries/SITL/SIM_Calibration.h
View File

@ -66,22 +66,22 @@ class Calibration : public Aircraft {
public:
Calibration(const char *home_str, const char *frame_str);
void update(const struct sitl_input& input);
void update(const struct sitl_input &input);
static Aircraft *create(const char *home_str, const char *frame_str) {
return new Calibration(home_str, frame_str);
}
private:
void _stop_control(const struct sitl_input& input, Vector3f& rot_accel);
void _stop_control(const struct sitl_input &input, Vector3f& rot_accel);
void _attitude_set(float desired_roll, float desired_pitch, float desired_yaw,
Vector3f& rot_accel);
void _attitude_control(const struct sitl_input& input,
void _attitude_control(const struct sitl_input &input,
Vector3f& rot_accel);
void _angular_velocity_control(const struct sitl_input& input,
void _angular_velocity_control(const struct sitl_input &input,
Vector3f& rot_accel);
void _calibration_poses(Vector3f& rot_accel);

2
libraries/SITL/SIM_Frame.cpp
View File

@ -202,7 +202,7 @@ Frame *Frame::find_frame(const char *name)
// calculate rotational and linear accelerations
void Frame::calculate_forces(const Aircraft &aircraft,
const Aircraft::sitl_input &input,
const struct sitl_input &input,
Vector3f &rot_accel,
Vector3f &body_accel)
{

2
libraries/SITL/SIM_Frame.h
View File

@ -48,7 +48,7 @@ public:
// calculate rotational and linear accelerations
void calculate_forces(const Aircraft &aircraft,
const Aircraft::sitl_input &input,
const struct sitl_input &input,
Vector3f &rot_accel, Vector3f &body_accel);
float terminal_velocity;

62
libraries/SITL/SIM_Gripper_EPM.cpp
View File

@ -17,42 +17,65 @@
*/
#include "SIM_Gripper_EPM.h"
#include "AP_HAL/AP_HAL.h"
#include <stdio.h>
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
using namespace SITL;
// table of user settable parameters
const AP_Param::GroupInfo Gripper_EPM::var_info[] = {
// @Param: ENABLE
// @DisplayName: Gripper servo Sim enable/disable
// @Description: Allows you to enable (1) or disable (0) the gripper servo simulation
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("ENABLE", 0, Gripper_EPM, gripper_emp_enable, 0),
// @Param: PIN
// @DisplayName: Gripper emp pin
// @Description: The pin number that the gripper emp is connected to. (start at 1)
// @Range: 0 15
// @User: Advanced
AP_GROUPINFO("PIN", 1, Gripper_EPM, gripper_emp_servo_pin, -1),
AP_GROUPEND
};
/*
update gripper state
*/
void Gripper_EPM::update_servobased(const Aircraft::sitl_input &input)
void Gripper_EPM::update_servobased(int16_t gripper_pwm)
{
if (! servo_based) {
if (!servo_based) {
return;
}
demand = (input.servos[gripper_servo]-1000) * 0.001f; // 0.0 - 1.0
if (demand < 0) { // never updated
demand = 0;
if (gripper_pwm >= 0) {
demand = (gripper_pwm - 1000) * 0.001f; // 0.0 - 1.0
if (is_negative(demand)) { // never updated
demand = 0.0f;
}
}
}
void Gripper_EPM::update_from_demand(const Aircraft::sitl_input &input)
void Gripper_EPM::update_from_demand()
{
const uint64_t now = AP_HAL::micros64();
const float dt = (now - last_update_us) * 1.0e-6f;
// decay the field
field_strength = field_strength * (100-field_decay_rate * dt)/100.0f;
field_strength = field_strength * (100.0f - field_decay_rate * dt) / 100.0f;
// note that "demand" here is just an on/off switch; we only care
// about which range it falls into
if (demand > 0.6) {
if (demand > 0.6f) {
// we are instructed to grip harder
field_strength = field_strength + (100.0f-field_strength) * field_strength_slew_rate/100.0f * dt;
} else if (demand < 0.4) {
field_strength = field_strength + (100.0f - field_strength) * field_strength_slew_rate / 100.0f * dt;
} else if (demand < 0.4f) {
// we are instructed to loosen grip
field_strength = field_strength * (100-field_degauss_rate * dt)/100.0f;
field_strength = field_strength * (100.0f - field_degauss_rate * dt) / 100.0f;
} else {
// neutral; no demanded change
}
@ -66,14 +89,15 @@ void Gripper_EPM::update_from_demand(const Aircraft::sitl_input &input)
}
last_update_us = now;
return;
}
void Gripper_EPM::update(const Aircraft::sitl_input &input)
void Gripper_EPM::update(const struct sitl_input &input)
{
update_servobased(input);
const int16_t gripper_pwm = gripper_emp_servo_pin >= 1 ? input.servos[gripper_emp_servo_pin-1] : -1;
update_from_demand(input);
update_servobased(gripper_pwm);
update_from_demand();
}
@ -83,7 +107,7 @@ bool Gripper_EPM::should_report()
return false;
}
if (fabs(reported_field_strength - field_strength) > 10.0f) {
if (abs(reported_field_strength - field_strength) > 10.0) {
return true;
}
@ -94,6 +118,6 @@ float Gripper_EPM::tesla()
{
// https://en.wikipedia.org/wiki/Orders_of_magnitude_(magnetic_field)
// 200N lifting capacity ~= 2.5T
const float percentage_to_tesla = 0.25;
return percentage_to_tesla * field_strength/100.0f;
const float percentage_to_tesla = 0.25f;
return static_cast<float>(percentage_to_tesla * field_strength / 100.0f);
}

24
libraries/SITL/SIM_Gripper_EPM.h
View File

@ -18,23 +18,31 @@
#pragma once
#include "SIM_Aircraft.h"
#include "stdint.h"
#include <AP_Param/AP_Param.h>
#include "SITL_Input.h"
namespace SITL {
class Gripper_EPM {
public:
const uint8_t gripper_servo;
Gripper_EPM() {
AP_Param::setup_object_defaults(this, var_info);
};
Gripper_EPM(const uint8_t _gripper_servo) :
gripper_servo(_gripper_servo)
{}
float payload_mass() const { return 0.0f; } // kg
// update field stength
void update(const struct Aircraft::sitl_input &input);
void update(const struct sitl_input &input);
static const struct AP_Param::GroupInfo var_info[];
bool is_enabled() const {return static_cast<bool>(gripper_emp_enable);}
private:
AP_Int8 gripper_emp_enable; // enable gripper sim
AP_Int8 gripper_emp_servo_pin;
const uint32_t report_interval = 100000; // microseconds
uint64_t last_report_us;
@ -52,8 +60,8 @@ private:
bool should_report();
void update_from_demand(const Aircraft::sitl_input &input);
void update_servobased(const struct Aircraft::sitl_input &input);
void update_from_demand();
void update_servobased(int16_t gripper_pwm);
float tesla();

49
libraries/SITL/SIM_Gripper_Servo.cpp
View File

@ -17,29 +17,57 @@
*/
#include "SIM_Gripper_Servo.h"
#include "AP_HAL/AP_HAL.h"
#include "AP_Math/AP_Math.h"
#include <stdio.h>
using namespace SITL;
// table of user settable parameters
const AP_Param::GroupInfo Gripper_Servo::var_info[] = {
// @Param: ENABLE
// @DisplayName: Gripper servo Sim enable/disable
// @Description: Allows you to enable (1) or disable (0) the gripper servo simulation
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("ENABLE", 0, Gripper_Servo, gripper_enable, 0),
// @Param: PIN
// @DisplayName: Gripper servo pin
// @Description: The pin number that the gripper servo is connected to. (start at 1)
// @Range: 0 15
// @User: Advanced
AP_GROUPINFO("PIN", 1, Gripper_Servo, gripper_servo_pin, -1),
AP_GROUPEND
};
/*
update gripper state
*/
void Gripper_Servo::update(const Aircraft::sitl_input &input)
void Gripper_Servo::update(const struct sitl_input &input)
{
const int16_t gripper_pwm = gripper_servo_pin >= 1 ? input.servos[gripper_servo_pin-1] : -1;
const uint64_t now = AP_HAL::micros64();
const float dt = (now - last_update_us) * 1.0e-6f;
// update gripper position
float position_demand = (input.servos[gripper_servo]-1000) * 0.001f;
if (position_demand < 0) { // never updated
position_demand = 0;
if (gripper_pwm < 0) {
last_update_us = now;
return;
}
const float position_max_change = position_slew_rate/100.0f * dt;
position = constrain_float(position_demand, position-position_max_change, position+position_max_change);
float position_demand = (gripper_pwm - 1000) * 0.001f;
if (is_negative(position_demand)) { // never updated
position_demand = 0.0f;
}
const float jaw_gap = gap*(1.0f-position);
const float position_max_change = position_slew_rate / 100.0f * dt;
position = constrain_float(position_demand, position - position_max_change, position + position_max_change);
const float jaw_gap = gap * (1.0f - position);
if (should_report()) {
::fprintf(stderr, "position_demand=%f jaw_gap=%f load=%f\n", position_demand, jaw_gap, load_mass);
last_report_us = now;
@ -47,7 +75,7 @@ void Gripper_Servo::update(const Aircraft::sitl_input &input)
}
if (jaw_gap < 5) {
if (aircraft->on_ground()) {
if (altitude <= 0.0f) {
load_mass = 1.0f; // attach the load
}
} else if (jaw_gap > 10) {
@ -55,7 +83,6 @@ void Gripper_Servo::update(const Aircraft::sitl_input &input)
}
last_update_us = now;
return;
}
bool Gripper_Servo::should_report()
@ -74,7 +101,7 @@ bool Gripper_Servo::should_report()
float Gripper_Servo::payload_mass() const
{
if (aircraft->hagl() < string_length) {
if (altitude < string_length) {
return 0.0f;
}
return load_mass;

26
libraries/SITL/SIM_Gripper_Servo.h
View File

@ -17,35 +17,35 @@
*/
#pragma once
#include "SIM_Aircraft.h"
#include "stdint.h"
#include <AP_Param/AP_Param.h>
#include "SITL_Input.h"
namespace SITL {
class Gripper_Servo {
public:
const uint8_t gripper_servo;
Gripper_Servo(const uint8_t _gripper_servo) :
gripper_servo(_gripper_servo)
{}
Gripper_Servo() {
AP_Param::setup_object_defaults(this, var_info);
};
// update Gripper state
void update(const struct Aircraft::sitl_input &input);
void update(const struct sitl_input &input);
float payload_mass() const; // kg
void set_aircraft(Aircraft *_aircraft) { aircraft = _aircraft; }
void set_alt(float alt) {altitude = alt;};
static const struct AP_Param::GroupInfo var_info[];
bool is_enabled() const {return static_cast<bool>(gripper_enable);}
private:
Aircraft *aircraft;
AP_Int8 gripper_enable; // enable gripper sim
AP_Int8 gripper_servo_pin;
const uint32_t report_interval = 1000000; // microseconds
uint64_t last_report_us;
const float gap = 30; // mm
float altitude;
float position; // percentage
float position_slew_rate = 35; // percentage
float reported_position = -1; // unlikely

2
libraries/SITL/SIM_ICEngine.cpp
View File

@ -24,7 +24,7 @@ using namespace SITL;
/*
update engine state, returning power output from 0 to 1
*/
float ICEngine::update(const Aircraft::sitl_input &input)
float ICEngine::update(const struct sitl_input &input)
{
bool have_ignition = ignition_servo>=0;
bool have_choke = choke_servo>=0;

2
libraries/SITL/SIM_ICEngine.h
View File

@ -39,7 +39,7 @@ public:
{}
// update motor state
float update(const struct Aircraft::sitl_input &input);
float update(const struct sitl_input &input);
private:
float last_output;

2
libraries/SITL/SIM_Motor.cpp
View File

@ -22,7 +22,7 @@
using namespace SITL;
// calculate rotational accel and thrust for a motor
void Motor::calculate_forces(const Aircraft::sitl_input &input,
void Motor::calculate_forces(const struct sitl_input &input,
const float thrust_scale,
uint8_t motor_offset,
Vector3f &rot_accel,

2
libraries/SITL/SIM_Motor.h
View File

@ -69,7 +69,7 @@ public:
pitch_max(_pitch_max)
{}
void calculate_forces(const Aircraft::sitl_input &input,
void calculate_forces(const struct sitl_input &input,
float thrust_scale,
uint8_t motor_offset,
Vector3f &rot_accel, // rad/sec

15
libraries/SITL/SIM_Multicopter.cpp
View File

@ -29,8 +29,6 @@ MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
{
mass = 1.5f;
gripper.set_aircraft(this);
frame = Frame::find_frame(frame_str);
if (frame == nullptr) {
printf("Frame '%s' not found", frame_str);
@ -66,6 +64,7 @@ void MultiCopter::update(const struct sitl_input &input)
calculate_forces(input, rot_accel, accel_body);
update_dynamics(rot_accel);
update_external_payload(input);
// update lat/lon/altitude
update_position();
@ -73,16 +72,4 @@ void MultiCopter::update(const struct sitl_input &input)
// update magnetic field
update_mag_field_bf();
// update sprayer
sprayer.update(input);
// update gripper
gripper.update(input);
gripper_epm.update(input);
}
float MultiCopter::gross_mass() const
{
return Aircraft::gross_mass() + sprayer.payload_mass() + gripper.payload_mass();
}

12
libraries/SITL/SIM_Multicopter.h
View File

@ -22,10 +22,6 @@
#include "SIM_Motor.h"
#include "SIM_Frame.h"
#include "SIM_Sprayer.h"
#include "SIM_Gripper_Servo.h"
#include "SIM_Gripper_EPM.h"
namespace SITL {
/*
@ -47,14 +43,6 @@ protected:
// calculate rotational and linear accelerations
void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
Frame *frame;
// The numbers below are the pwm output channels with "0" meaning the first output (aka RC1)
Sprayer sprayer{6, 7};
Gripper_Servo gripper{8};
Gripper_EPM gripper_epm{9};
float gross_mass() const override;
};
}

61
libraries/SITL/SIM_Sprayer.cpp
View File

@ -17,18 +17,48 @@
*/
#include "SIM_Sprayer.h"
#include <stdio.h>
#include "AP_HAL/AP_HAL.h"
#include "AP_Math/AP_Math.h"
using namespace SITL;
// table of user settable parameters
const AP_Param::GroupInfo Sprayer::var_info[] = {
// @Param: ENABLE
// @DisplayName: Gripper servo Sim enable/disable
// @Description: Allows you to enable (1) or disable (0) the gripper servo simulation
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("ENABLE", 0, Sprayer, sprayer_enable, 0),
// @Param: PUMP
// @DisplayName: Sprayer pump pin
// @Description: The pin number that the Sprayer pump is connected to. (start at 1)
// @Range: 0 15
// @User: Advanced
AP_GROUPINFO("PUMP", 1, Sprayer, sprayer_pump_pin, -1),
// @Param: SPIN
// @DisplayName: Sprayer spinner servo pin
// @Description: The pin number that the Sprayer spinner servo is connected to. (start at 1)
// @Range: 0 15
// @User: Advanced
AP_GROUPINFO("SPIN", 2, Sprayer, sprayer_spin_pin, -1),
AP_GROUPEND
};
/*
update sprayer state
*/
void Sprayer::update(const Aircraft::sitl_input &input)
void Sprayer::update(const struct sitl_input &input)
{
const int16_t pump_pwm = sprayer_pump_pin >= 1 ? input.servos[sprayer_pump_pin-1] : -1;
const int16_t spinner_pwm = sprayer_spin_pin >= 1 ? input.servos[sprayer_spin_pin-1] : -1;
const uint64_t now = AP_HAL::micros64();
const float dt = (now - last_update_us) * 1.0e-6f;
if (pump_pwm >= 0) {
// update remaining payload
if (capacity > 0) {
const double delta = last_pump_output * pump_max_rate * dt;
@ -39,34 +69,37 @@ void Sprayer::update(const Aircraft::sitl_input &input)
}
// update pump
float pump_demand = (input.servos[pump_servo]-1000) * 0.001f;
float pump_demand = (pump_pwm - 1000) * 0.001f;
// ::fprintf(stderr, "pump_demand=%f\n", pump_demand);
if (pump_demand < 0) { // never updated
pump_demand = 0;
}
const float pump_max_change = pump_slew_rate/100.0f * dt;
last_pump_output = constrain_float(pump_demand, last_pump_output-pump_max_change, last_pump_output+pump_max_change);
const float pump_max_change = pump_slew_rate / 100.0f * dt;
last_pump_output =
constrain_float(pump_demand, last_pump_output - pump_max_change, last_pump_output + pump_max_change);
last_pump_output = constrain_float(last_pump_output, 0, 1);
} else {
last_pump_output = 0.0f;
}
// update spinner (if any)
if (spinner_servo >= 0) {
const float spinner_demand = (input.servos[spinner_servo]-1000) * 0.001f;
if (spinner_pwm >= 0) {
const float spinner_demand = (spinner_pwm - 1000) * 0.001f;
const float spinner_max_change = spinner_slew_rate * 0.01f * dt;
last_spinner_output = constrain_float(spinner_demand, last_spinner_output-spinner_max_change, last_spinner_output+spinner_max_change);
last_spinner_output = constrain_float(spinner_demand,
last_spinner_output - spinner_max_change,
last_spinner_output + spinner_max_change);
last_spinner_output = constrain_float(last_spinner_output, 0, 1);
}
if (should_report()) {
printf("Remaining: %f litres\n", capacity);
printf("Pump: %f l/s\n", last_pump_output * pump_max_rate);
if (spinner_servo >= 0) {
printf("Spinner: %f rev/s\n", (last_spinner_output * spinner_max_rate)/360.0f);
if (spinner_pwm >= 0) {
printf("Spinner: %f rev/s\n", (last_spinner_output * spinner_max_rate) / 360.0f);
}
last_report_us = now;
}
last_update_us = now;
return;
}
bool Sprayer::should_report()

35
libraries/SITL/SIM_Sprayer.h
View File

@ -18,36 +18,41 @@
#pragma once
#include "SIM_Aircraft.h"
#include "stdint.h"
#include <AP_Param/AP_Param.h>
#include "SITL_Input.h"
namespace SITL {
class Sprayer {
public:
const uint8_t pump_servo;
const int8_t spinner_servo;
Sprayer(const uint8_t _pump_servo, int8_t _spinner_servo) :
pump_servo(_pump_servo),
spinner_servo(_spinner_servo)
{}
Sprayer() {
AP_Param::setup_object_defaults(this, var_info);
};
// update sprayer state
void update(const struct Aircraft::sitl_input &input);
void update(const struct sitl_input &input);
float payload_mass() const { return capacity; }; // kg; water, so kg=l
float payload_mass() const { return static_cast<float>(capacity); }; // kg; water, so kg=l
private:
static const struct AP_Param::GroupInfo var_info[];
bool is_enabled() const {return static_cast<bool>(sprayer_enable);}
private:
AP_Int8 sprayer_enable; // enable sprayer sim
AP_Int8 sprayer_pump_pin;
AP_Int8 sprayer_spin_pin;
const uint32_t report_interval = 1000000; // microseconds
uint64_t last_report_us;
const float pump_max_rate = 0.01; // litres/second
const float pump_slew_rate = 20; // percent/scond
const float pump_max_rate = 0.01f; // litres/second
const float pump_slew_rate = 20.0f; // percent/second
float last_pump_output; // percentage
const float spinner_max_rate = 3600; // degrees/second
const float spinner_slew_rate = 20; // percent/second
const float spinner_max_rate = 3600.0f; // degrees/second
const float spinner_slew_rate = 20.0f; // percent/second
float last_spinner_output; // percentage
double capacity = 0.25; // litres

10
libraries/SITL/SITL.cpp
View File

@ -122,6 +122,16 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
AP_GROUPINFO("MAG_ODI", 19, SITL, mag_offdiag, 0),
AP_GROUPINFO("MAG_ORIENT", 20, SITL, mag_orient, 0),
AP_GROUPINFO("RC_CHANCOUNT",21, SITL, rc_chancount, 16),
// @Group: SPR_
// @Path: ./SIM_Sprayer.cpp
AP_SUBGROUPINFO(sprayer_sim, "SPR_", 22, SITL, Sprayer),
// @Group: GRPS_
// @Path: ./SIM_Gripper_Servo.cpp
AP_SUBGROUPINFO(gripper_sim, "GRPS_", 23, SITL, Gripper_Servo),
// @Group: GRPE_
// @Path: ./SIM_Gripper_EPM.cpp
AP_SUBGROUPINFO(gripper_epm_sim, "GRPE_", 24, SITL, Gripper_EPM),
AP_GROUPEND
};

9
libraries/SITL/SITL.h
View File

@ -3,6 +3,10 @@
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include "SIM_Sprayer.h"
#include "SIM_Gripper_Servo.h"
#include "SIM_Gripper_EPM.h"
class DataFlash_Class;
namespace SITL {
@ -206,6 +210,11 @@ public:
// convert a set of roll rates from body frame to earth frame
static Vector3f convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro);
Sprayer sprayer_sim;
Gripper_Servo gripper_sim;
Gripper_EPM gripper_epm_sim;
};
} // namespace SITL

18
libraries/SITL/SITL_Input.h Normal file
View File

@ -0,0 +1,18 @@
#pragma once
#include <stdint.h>
/*
structure passed in giving servo positions as PWM values in
microseconds
*/
struct sitl_input {
uint16_t servos[16];
struct {
float speed; // m/s
float direction; // degrees 0..360
float turbulence;
float dir_z; //degrees -90..90
} wind;
};