Skip to content

Commit

Permalink
SITL: add x8-m frame to SITL
Browse files Browse the repository at this point in the history
  • Loading branch information
@bnsgeyer
bnsgeyer committed Jun 7, 2024
1 parent 7901fba commit 9e1fbee
Show file tree
Hide file tree
Showing 3 changed files with 249 additions and 1 deletion.
210 changes: 210 additions & 0 deletions libraries/SITL/SIM_Frame.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <AP_Baro/AP_Baro.h>
#include <AP_Filesystem/AP_Filesystem.h>
#include "SIM_Aircraft.h"
#include <GCS_MAVLink/GCS.h>

#include <stdio.h>
#include <sys/stat.h>
Expand Down Expand Up @@ -287,6 +288,7 @@ static Frame supported_frames[] =
Frame("quad", 4, quad_plus_motors),
Frame("copter", 4, quad_plus_motors),
Frame("x", 4, quad_x_motors),
Frame("x8", 4, quad_x_motors),
Frame("bfxrev", 4, quad_bf_x_rev_motors),
Frame("bfx", 4, quad_bf_x_motors),
Frame("djix", 4, quad_dji_x_motors),
Expand Down Expand Up @@ -617,3 +619,211 @@ void Frame::current_and_voltage(float &voltage, float &current)
}
}
#endif // AP_SIM_ENABLED
// calculate rotational and linear accelerations for x8 frame
void Frame::calculate_forces_x8(const Aircraft &aircraft,
const struct sitl_input &input,
Vector3f &rot_accel,
Vector3f &body_accel,
float* rpm,
bool use_drag)
{
// calculate rotational and linear accelerations

const float m2ftd = 1.0f/0.3046f;
const float m2ftn = 0.3046f;

// Updated model using sweeps in aerodrome
float Mu = 1.421f * m2ftd; //value has been converted from rad/s/s/ft/s to rad/s/s/m/s
float Lv = -1.36f * m2ftd; //value has been converted from rad/s/s/ft/s to rad/s/s/m/s
float Xu = -0.2179f;
float Yv = -0.201f;
float Nr = -0.5513f;
float Nped = 12.07f;
float Zcol = -71.64f * m2ftn; //value has been converted from ft/s/s to m/s/s
float Zw = -0.4061 * m2ftd; //value has been converted from rad/s/s/ft/s to rad/s/s/m/s
float Xlon = -12.74f * m2ftn; //value has been converted from ft/s/s to m/s/s
float Mlon = 112.93f;
float Lag = 24.94f;
float Lead = 1.54f;
float Ylat = 13.79f * m2ftn; //value has been converted from ft/s/s to m/s/s
float Llat = 144.32f;
uint16_t _time_delay_rp = 0;
uint16_t _time_delay_ped = 14;
uint16_t _time_delay_col = 26;

static uint64_t last_calc_us;
static uint16_t printed = 2000;

uint64_t now_us = AP_HAL::micros64();
float dt = 0.0f;
if (last_calc_us != 0) {
dt = (now_us - last_calc_us)*1.0e-6;
}
last_calc_us = now_us;

if (_time_delay_rp == 0 || is_zero(dt)) {
for (uint8_t i = 0; i < 2; i++) {
_servos_delayed[i] = input.servos[i];
}
} else if (servos_stored_buffer == nullptr) {
uint16_t buffer_size = constrain_int16(_time_delay_rp, 1, 100) * 0.001f / dt;
servos_stored_buffer = new ObjectBuffer<servos_stored>(buffer_size);
while (servos_stored_buffer->space() != 0) {
push_to_buffer(input.servos);
}
for (uint8_t i = 0; i < 2; i++) {
_servos_delayed[i] = input.servos[i];
}
} else {
pull_from_buffer(_servos_delayed);
push_to_buffer(input.servos);
}

if (_time_delay_col == 0 || is_zero(dt)) {
_servo_delayed_col = input.servos[2];
} else if (servo_stored_col_buffer == nullptr) {
uint16_t buffer_size = constrain_int16(_time_delay_col, 1, 100) * 0.001f / dt;
servo_stored_col_buffer = new ObjectBuffer<servo_stored>(buffer_size);
while (servo_stored_col_buffer->space() != 0) {
push_to_buffer_col(input.servos);
}
_servo_delayed_col = input.servos[2];
} else {
pull_from_buffer_col(_servo_delayed_col);
push_to_buffer_col(input.servos);
}

if (_time_delay_ped == 0 || is_zero(dt)) {
_servo_delayed_ped = input.servos[3];
} else if (servo_stored_ped_buffer == nullptr) {
uint16_t buffer_size = constrain_int16(_time_delay_ped, 1, 100) * 0.001f / dt;
servo_stored_ped_buffer = new ObjectBuffer<servo_stored>(buffer_size);
while (servo_stored_ped_buffer->space() != 0) {
push_to_buffer_ped(input.servos);
}
_servo_delayed_ped = input.servos[3];
} else {
pull_from_buffer_ped(_servo_delayed_ped);
push_to_buffer_ped(input.servos);
}

static float Dlonlag;
static float Dlatlag;
static float Dcollag;
static float Dpedlag;

float _roll_in = (_servos_delayed[0] / 500.0f) - 3.0f; // roll
float _pitch_in = (_servos_delayed[1] / 500.0f) - 3.0f; //pitch
float _throttle_in = (_servo_delayed_col / 1000.0f) - 1.0f; // throttle
float _yaw_in = (_servo_delayed_ped / 500.0f) - 3.0f; // yaw

if (is_zero(_throttle_in)) {
_pitch_in = 0;
_roll_in = 0;
_yaw_in = 0;
}

Vector3f velocity_air_bf = aircraft.get_dcm().transposed() * aircraft.get_velocity_air_ef();

const Vector3f &gyro = aircraft.get_gyro();
// rotational acceleration (in rad/s/s?) in body frame
rot_accel.x = (Lv)*(velocity_air_bf.y)+(Llat)*(Dlatlag);
rot_accel.y = (Mu)*(velocity_air_bf.x)+(Mlon)*(Dlonlag);
rot_accel.z = (Nr)*(gyro.z)+((Nped)-(Lag*Lead))*(Dpedlag)+(Lag*Lead)*(_yaw_in);

float lateral_y_thrust = (Yv)*(velocity_air_bf.y)+(Ylat)*(Dlatlag);
float lateral_x_thrust = (Xu)*(velocity_air_bf.x)+(Xlon)*Dlonlag;
float thrust = Zcol * Dcollag + (Zw * velocity_air_bf.z) + 0.1f * Zcol;
body_accel = Vector3f(lateral_x_thrust, lateral_y_thrust, thrust);

float Dlatlag_dot = (-Lag)*(Dlatlag)+(Lag)*(_roll_in);
float Dlonlag_dot = (-Lag)*(Dlonlag)+(Lag)*(_pitch_in);
float Dcollag_dot = (-Lag)*(Dcollag)+(Lag)*(_throttle_in);
float Dpedlag_dot = (-Lag)*(Dpedlag)+(Lag)*(_yaw_in);

if (printed > 0) {
printed -= 1;
} else {
// gcs().send_text(MAV_SEVERITY_WARNING, "_roll_in %f _pitch_in %f _throttle_in %f _yaw_in %f", _roll_in, _pitch_in, _throttle_in, _yaw_in);
// gcs().send_text(MAV_SEVERITY_WARNING, "input.servos[0] %f 1 %f 2 %f 3 %f", (double)input.servos[0], (double)input.servos[1], (double)input.servos[2], (double)input.servos[3]);
// gcs().send_text(MAV_SEVERITY_WARNING, "rot_accel.x %f y %f z %f", rot_accel.x, rot_accel.y, rot_accel.z);
// gcs().send_text(MAV_SEVERITY_WARNING, "Dlatlag %f Dlonlag %f Dcollag %f Dpedlag %f", Dlatlag, Dlonlag, Dcollag, Dpedlag);
// gcs().send_text(MAV_SEVERITY_WARNING, "body_accel.x %f y %f z %f Dcollag %f", body_accel.x, body_accel.y, body_accel.z, Dcollag);
// gcs().send_text(MAV_SEVERITY_WARNING, "Servos delayed[0] %f [1] %f [2] %f ped %f", (double)_servos_delayed[0], (double)_servos_delayed[1], (double)_servos_delayed[2], (double)_servo_delayed_ped);
printed = 2000;
}

Dlonlag += Dlonlag_dot * dt;
Dlatlag += Dlatlag_dot * dt;
Dcollag += Dcollag_dot * dt;
Dpedlag += Dpedlag_dot * dt;

}

// push servo input to buffer
void Frame::push_to_buffer(const uint16_t servos_input[16])
{
servos_stored sample;
sample.servo1 = servos_input[0];
sample.servo2 = servos_input[1];

servos_stored_buffer->push(sample);

}

// pull servo delay from buffer
void Frame::pull_from_buffer(uint16_t servos_delayed[3])
{
servos_stored sample;
if (!servos_stored_buffer->pop(sample)) {
// no sample
return;
}
servos_delayed[0] = sample.servo1;
servos_delayed[1] = sample.servo2;

}

// push servo input to buffer COL
void Frame::push_to_buffer_col(const uint16_t servos_input[16])
{
servo_stored sample;
sample.servo = servos_input[2];

servo_stored_col_buffer->push(sample);

}

// pull servo delay from buffer COL
void Frame::pull_from_buffer_col(uint16_t &servo_delayed)
{
servo_stored sample;
if (!servo_stored_col_buffer->pop(sample)) {
// no sample
return;
}
servo_delayed = sample.servo;

}

// push servo input to buffer PED
void Frame::push_to_buffer_ped(const uint16_t servos_input[16])
{
servo_stored sample;
sample.servo = servos_input[3];

servo_stored_ped_buffer->push(sample);

}

// pull servo delay from buffer PED
void Frame::pull_from_buffer_ped(uint16_t &servo_delayed)
{
servo_stored sample;
if (!servo_stored_ped_buffer->pop(sample)) {
// no sample
return;
}
servo_delayed = sample.servo;
}

32 changes: 32 additions & 0 deletions libraries/SITL/SIM_Frame.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,13 @@ class Frame {
bool use_drag=true);
#endif // AP_SIM_ENABLED


// calculate rotational and linear accelerations for x8 frame
void calculate_forces_x8(const Aircraft &aircraft,
const struct sitl_input &input,
Vector3f &rot_accel, Vector3f &body_accel, float* rpm,
bool use_drag=true);

float terminal_velocity;
float terminal_rotation_rate;
uint8_t motor_offset;
Expand Down Expand Up @@ -170,5 +177,30 @@ class Frame {
void parse_float(picojson::value val, const char* label, float &param);
void parse_vector3(picojson::value val, const char* label, Vector3f &param);
#endif
struct servos_stored {
uint16_t servo1;
uint16_t servo2;

};
uint16_t _servos_delayed[2];
ObjectBuffer<servos_stored> *servos_stored_buffer;
void push_to_buffer(const uint16_t servos_input[16]);
void pull_from_buffer(uint16_t servos_delayed[2]);

struct servo_stored {
uint16_t servo;
};
// PED
uint16_t _servo_delayed_ped;
ObjectBuffer<servo_stored> *servo_stored_ped_buffer;
void push_to_buffer_ped(const uint16_t servos_input[16]);
void pull_from_buffer_ped(uint16_t &servo_delayed);

// COL
uint16_t _servo_delayed_col;
ObjectBuffer<servo_stored> *servo_stored_col_buffer;
void push_to_buffer_col(const uint16_t servos_input[16]);
void pull_from_buffer_col(uint16_t &servo_delayed);

};
}
8 changes: 7 additions & 1 deletion libraries/SITL/SIM_Multicopter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include "SIM_Multicopter.h"
#include <AP_Motors/AP_Motors.h>

#include <GCS_MAVLink/GCS.h>
#include <stdio.h>

using namespace SITL;
Expand All @@ -44,8 +45,13 @@ MultiCopter::MultiCopter(const char *frame_str) :
void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
{
motor_mask |= ((1U<<frame->num_motors)-1U) << frame->motor_offset;
frame->calculate_forces(*this, input, rot_accel, body_accel, rpm);
// frame->calculate_forces(*this, input, rot_accel, body_accel, rpm);

if (strncasecmp("x8", frame->name, strlen(frame->name)) == 0) {
frame->calculate_forces_x8(*this, input, rot_accel, body_accel, rpm);
} else {
frame->calculate_forces(*this, input, rot_accel, body_accel, rpm);
}
add_shove_forces(rot_accel, body_accel);
add_twist_forces(rot_accel);
}
Expand Down

0 comments on commit 9e1fbee

Please sign in to comment.