diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index 4b6ab561f87962..b2c463281807d4 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -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; @@ -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]; @@ -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) { @@ -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) { @@ -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 @@ -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