Skip to content

Commit

Permalink
SITL: allow for X8-m passthrough of motors inputs and add input logging
Browse files Browse the repository at this point in the history
  • Loading branch information
@bnsgeyer
bnsgeyer committed Jun 8, 2024
1 parent dbde230 commit 3ce9c68
Show file tree
Hide file tree
Showing 2 changed files with 93 additions and 55 deletions.
147 changes: 92 additions & 55 deletions libraries/SITL/SIM_Frame.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include <AP_Filesystem/AP_Filesystem.h>
#include "SIM_Aircraft.h"
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>

#include <stdio.h>
#include <sys/stat.h>
Expand Down Expand Up @@ -627,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 @@ -651,67 +652,94 @@ 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;

uint16_t servos_raw[16];
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] = input.servos[i];
_servos_delayed[i] = servos_raw[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);
push_to_buffer(servos_raw);
}
for (uint8_t i = 0; i < 2; i++) {
_servos_delayed[i] = input.servos[i];
_servos_delayed[i] = servos_raw[i];
}
} else {
pull_from_buffer(_servos_delayed);
push_to_buffer(input.servos);
push_to_buffer(servos_raw);
}

// this adds time delay for throttle inputs
if (_time_delay_col == 0 || is_zero(dt)) {
_servo_delayed_col = input.servos[2];
_servo_delayed_col = servos_raw[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);
push_to_buffer_col(servos_raw);
}
_servo_delayed_col = input.servos[2];
_servo_delayed_col = servos_raw[2];
} else {
pull_from_buffer_col(_servo_delayed_col);
push_to_buffer_col(input.servos);
push_to_buffer_col(servos_raw);
}

// this adds time delay for pedal inputs
if (_time_delay_ped == 0 || is_zero(dt)) {
_servo_delayed_ped = input.servos[3];
_servo_delayed_ped = servos_raw[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);
push_to_buffer_ped(servos_raw);
}
_servo_delayed_ped = input.servos[3];
_servo_delayed_ped = servos_raw[3];
} else {
pull_from_buffer_ped(_servo_delayed_ped);
push_to_buffer_ped(input.servos);
push_to_buffer_ped(servos_raw);
}

static float Dlonlag;
static float Dlatlag;
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 @@ -723,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 Expand Up @@ -827,3 +842,25 @@ void Frame::pull_from_buffer_ped(uint16_t &servo_delayed)
servo_delayed = sample.servo;
}

void Frame::Log_Write_SimData(float dlong, float dlat, float dthr, float dped)
{
// @LoggerMessage: SIMD
// @Description: Sim data packet
// @Vehicles: Copter
// @Field: TimeUS: Time since system startup
// @Field: dlng: longitudinal input
// @Field: dlat: lateral input
// @Field: dthr: throttle input
// @Field: dped: pedal input
AP::logger().WriteStreaming(
"SIMD",
"TimeUS,dlng,dlat,dthr,dped",
"s----",
"F0000",
"Qffff",
AP_HAL::micros64(),
dlong,
dlat,
dthr,
dped);
}
1 change: 1 addition & 0 deletions libraries/SITL/SIM_Frame.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -202,5 +202,6 @@ class Frame {
void push_to_buffer_col(const uint16_t servos_input[16]);
void pull_from_buffer_col(uint16_t &servo_delayed);

void Log_Write_SimData(float dlong, float dlat, float dthr, float dped);
};
}

0 comments on commit 3ce9c68

Please sign in to comment.