Skip to content

Commit

Permalink
SITL:cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
@bnsgeyer
bnsgeyer committed Jun 8, 2024
1 parent a7a96f7 commit 0ad36d4
Showing 1 changed file with 49 additions and 48 deletions.
97 changes: 49 additions & 48 deletions libraries/SITL/SIM_Frame.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -628,20 +628,20 @@ void Frame::calculate_forces_x8(const Aircraft &aircraft,
float* rpm,
bool use_drag)
{
// calculate rotational and linear accelerations
// calculate rotational and linear accelerations

const float m2ftd = 1.0f/0.3046f;
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
// 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;
Expand All @@ -652,23 +652,33 @@ void Frame::calculate_forces_x8(const Aircraft &aircraft,
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;
static uint64_t last_calc_us;
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;

// determine pitch, roll, yaw, throttle from servo inputs
uint16_t servos_raw[16];
servos_raw[0] = (uint16_t)((input.servos[1] - input.servos[0]) * 0.5f + 1500.0f);
servos_raw[1] = (uint16_t)((input.servos[2] - input.servos[3]) * 0.5f + 1500.0f);
servos_raw[2] = (uint16_t)((input.servos[0] + input.servos[1] + input.servos[2] + input.servos[3]) * 0.25f);
servos_raw[3] = (uint16_t)((((input.servos[0] + input.servos[1]) * 0.5f - (input.servos[2] + input.servos[3]) * 0.5f) * 0.5) + 1500.0f);

float servos_adj[16];
// A scale factor has to be used to make the motors class inputs match the calculated inputs from the servos.
float servo_sf = 0.893;
servos_adj[0]= (input.servos[0] - 1000) * servo_sf + 1000;
servos_adj[1]= (input.servos[1] - 1000) * servo_sf + 1000;
servos_adj[2]= (input.servos[2] - 1000) * servo_sf + 1000;
servos_adj[3]= (input.servos[3] - 1000) * servo_sf + 1000;

// this section turns the servo outputs into the motors class inputs in PWM
// it uses the frame quad/plus
// roll, pitch, and yaw scale is 1000-2000 with 1000 being -1 and 2000 being +1
// throttle scale is 1000-2000 with 1000 being 0 and 2000 being +1
servos_raw[0] = (uint16_t)((servos_adj[1] - servos_adj[0]) * 0.5f + 1500.0f);
servos_raw[1] = (uint16_t)((servos_adj[2] - servos_adj[3]) * 0.5f + 1500.0f);
servos_raw[2] = (uint16_t)((servos_adj[0] + servos_adj[1] + servos_adj[2] + servos_adj[3]) * 0.25f);
servos_raw[3] = (uint16_t)((((servos_adj[0] + servos_adj[1]) * 0.5f - (servos_adj[2] + servos_adj[3]) * 0.5f) * 0.5) + 1500.0f);

// this adds time delay for roll and pitch inputs
if (_time_delay_rp == 0 || is_zero(dt)) {
for (uint8_t i = 0; i < 2; i++) {
_servos_delayed[i] = servos_raw[i];
Expand All @@ -687,6 +697,7 @@ void Frame::calculate_forces_x8(const Aircraft &aircraft,
push_to_buffer(servos_raw);
}

// this adds time delay for throttle inputs
if (_time_delay_col == 0 || is_zero(dt)) {
_servo_delayed_col = servos_raw[2];
} else if (servo_stored_col_buffer == nullptr) {
Expand All @@ -701,6 +712,7 @@ void Frame::calculate_forces_x8(const Aircraft &aircraft,
push_to_buffer_col(servos_raw);
}

// this adds time delay for pedal inputs
if (_time_delay_ped == 0 || is_zero(dt)) {
_servo_delayed_ped = servos_raw[3];
} else if (servo_stored_ped_buffer == nullptr) {
Expand All @@ -720,12 +732,14 @@ void Frame::calculate_forces_x8(const Aircraft &aircraft,
static float Dcollag;
static float Dpedlag;

// calculated motors class inputs from servo_out for logging
float dlat_in = (servos_raw[0] / 500.0f) - 3.0f; // roll
float dlong_in = (servos_raw[1] / 500.0f) - 3.0f; //pitch
float dthr_in = (servos_raw[2] / 1000.0f) - 1.0f; // throttle
float dped_in = (servos_raw[3] / 500.0f) - 3.0f; // yaw
Log_Write_SimData(dlong_in, dlat_in, dthr_in, dped_in);

// calculated motors class inputs from delayed servo_out for state space model input
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
Expand All @@ -737,41 +751,28 @@ void Frame::calculate_forces_x8(const Aircraft &aircraft,
_yaw_in = 0;
}

Vector3f velocity_air_bf = aircraft.get_dcm().transposed() * aircraft.get_velocity_air_ef();
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);
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 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);
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
Expand Down

0 comments on commit 0ad36d4

Please sign in to comment.