/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see .
*/
/*
simulator connector for FlightAxis
*/
#include "SIM_FlightAxis.h"
#include
#include
#include
#include
#include
#include
#include
#include
extern const AP_HAL::HAL& hal;
#define FLIGHTAXIS_SERVER_IP "192.168.2.48"
#define FLIGHTAXIS_SERVER_PORT 18083
namespace SITL {
// the asprintf() calls are not worth checking for SITL
#pragma GCC diagnostic ignored "-Wunused-result"
FlightAxis::FlightAxis(const char *home_str, const char *frame_str) :
Aircraft(home_str, frame_str)
{
use_time_sync = false;
rate_hz = 250 / target_speedup;
heli_demix = strstr(frame_str, "helidemix") != NULL;
rev4_servos = strstr(frame_str, "rev4") != NULL;
}
/*
extremely primitive SOAP parser that assumes the format used by FlightAxis
*/
void FlightAxis::parse_reply(const char *reply)
{
const char *reply0 = reply;
for (uint16_t i=0; i= (int32_t)sizeof(reply)) {
printf("Reply too large %i\n", expected_length);
return nullptr;
}
while (ret < expected_length) {
ssize_t ret2 = sock.recv(&reply[ret], sizeof(reply)-(1+ret), 100);
if (ret2 <= 0) {
return nullptr;
}
ret += ret2;
}
return strdup(reply);
}
void FlightAxis::exchange_data(const struct sitl_input &input)
{
if (!controller_started) {
printf("Starting controller\n");
// call a restore first. This allows us to connect after the aircraft is changed in RealFlight
char *reply = soap_request("RestoreOriginalControllerDevice", R"(
12
)");
free(reply);
reply = soap_request("InjectUAVControllerInterface", R"(
12
)");
free(reply);
activation_frame_counter = frame_counter;
controller_started = true;
}
float scaled_servos[8];
for (uint8_t i=0; i<8; i++) {
scaled_servos[i] = (input.servos[i] - 1000) / 1000.0f;
}
if (rev4_servos) {
// swap first 4 and last 4 servos, for quadplane testing
float saved[4];
memcpy(saved, &scaled_servos[0], sizeof(saved));
memcpy(&scaled_servos[0], &scaled_servos[4], sizeof(saved));
memcpy(&scaled_servos[4], saved, sizeof(saved));
}
if (heli_demix) {
// FlightAxis expects "roll/pitch/collective/yaw" input
float swash1 = scaled_servos[0];
float swash2 = scaled_servos[1];
float swash3 = scaled_servos[2];
float roll_rate = swash1 - swash2;
float pitch_rate = -((swash1+swash2) / 2.0f - swash3);
scaled_servos[0] = constrain_float(roll_rate + 0.5, 0, 1);
scaled_servos[1] = constrain_float(pitch_rate + 0.5, 0, 1);
}
char *reply = soap_request("ExchangeData", R"(
255
- %.4f
- %.4f
- %.4f
- %.4f
- %.4f
- %.4f
- %.4f
- %.4f
)",
scaled_servos[0],
scaled_servos[1],
scaled_servos[2],
scaled_servos[3],
scaled_servos[4],
scaled_servos[5],
scaled_servos[6],
scaled_servos[7]);
if (reply) {
parse_reply(reply);
free(reply);
}
}
/*
update the FlightAxis simulation by one time step
*/
void FlightAxis::update(const struct sitl_input &input)
{
Vector3f last_velocity_ef = velocity_ef;
exchange_data(input);
float dt_seconds = state.m_currentPhysicsTime_SEC - last_time_s;
if (dt_seconds <= 0) {
return;
}
if (initial_time_s <= 0) {
dt_seconds = 0.001f;
initial_time_s = state.m_currentPhysicsTime_SEC - dt_seconds;
}
Quaternion quat(state.m_orientationQuaternion_W,
state.m_orientationQuaternion_Z,
-state.m_orientationQuaternion_Y,
-state.m_orientationQuaternion_Z);
quat.rotation_matrix(dcm);
dcm.from_euler(radians(state.m_roll_DEG),
radians(state.m_inclination_DEG),
-radians(state.m_azimuth_DEG));
Quaternion quat2;
quat2.from_rotation_matrix(dcm);
gyro = Vector3f(radians(constrain_float(state.m_rollRate_DEGpSEC, -2000, 2000)),
radians(constrain_float(state.m_pitchRate_DEGpSEC, -2000, 2000)),
-radians(constrain_float(state.m_yawRate_DEGpSEC, -2000, 2000))) * target_speedup;
velocity_ef = Vector3f(state.m_velocityWorldU_MPS,
state.m_velocityWorldV_MPS,
state.m_velocityWorldW_MPS);
position = Vector3f(state.m_aircraftPositionY_MTR,
state.m_aircraftPositionX_MTR,
-state.m_altitudeAGL_MTR);
// offset based on first position to account for offset in RF world
if (position_offset.is_zero()) {
position_offset = position;
}
position -= position_offset;
// the accel values given in the state are very strange. Calculate
// it from delta-velocity instead, although this does introduce
// noise
Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds;
accel_ef.z -= GRAVITY_MSS;
accel_body = dcm.transposed() * accel_ef;
accel_body.x = constrain_float(accel_body.x, -16, 16);
accel_body.y = constrain_float(accel_body.y, -16, 16);
accel_body.z = constrain_float(accel_body.z, -16, 16);
airspeed = state.m_airspeed_MPS;
battery_voltage = state.m_batteryVoltage_VOLTS;
battery_current = state.m_batteryCurrentDraw_AMPS;
rpm1 = state.m_propRPM;
rpm2 = state.m_heliMainRotorRPM;
update_position();
time_now_us = (state.m_currentPhysicsTime_SEC - initial_time_s)*1.0e6;
if (frame_counter++ % 1000 == 0) {
if (last_frame_count_s != 0) {
printf("%.2f FPS\n",
1000 / (state.m_currentPhysicsTime_SEC - last_frame_count_s));
printf("(%.3f %.3f %.3f %.3f) (%.3f %.3f %.3f %.3f)\n",
quat.q1, quat.q2, quat.q3, quat.q4,
quat2.q1, quat2.q2, quat2.q3, quat2.q4);
} else {
printf("Initial position %f %f %f\n", position.x, position.y, position.z);
}
last_frame_count_s = state.m_currentPhysicsTime_SEC;
}
last_time_s = state.m_currentPhysicsTime_SEC;
}
} // namespace SITL