aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: use thread for FlightAxis comms

Browse Source 
this allows us to run SITL at a much higher framerate than RF can
provide, and results in the EKF being much happier
This commit is contained in:
Andrew Tridgell 2017-10-21 11:57:59 +11:00
parent 7def86ae96
commit c5cd1b873c
5 changed files with 155 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/SITL/SIM_Aircraft.cpp
View File

@ -32,7 +32,7 @@
#include <DataFlash/DataFlash.h>
#include <AP_Param/AP_Param.h>
namespace SITL {
using namespace SITL;
/*
parent class for all simulator types
@ -680,6 +680,9 @@ void Aircraft::smooth_sensors(void)
return;
}
const float delta_time = (now - smoothing.last_update_us) * 1.0e-6f;
if (delta_time < 0 || delta_time > 0.1) {
return;
}
// calculate required accel to get us to desired position and velocity in the time_constant
const float time_constant = 0.1f;
@ -773,4 +776,24 @@ float Aircraft::filtered_servo_range(const struct sitl_input &input, uint8_t idx
return filtered_idx(v, idx);
}
} // namespace SITL
// extrapolate sensors by a given delta time in seconds
void Aircraft::extrapolate_sensors(float delta_time)
{
Vector3f accel_earth = dcm * accel_body;
accel_earth.z += GRAVITY_MSS;
dcm.rotate(gyro * delta_time);
dcm.normalize();
// work out acceleration as seen by the accelerometers. It sees the kinematic
// acceleration (ie. real movement), plus gravity
accel_body = dcm.transposed() * (accel_earth + Vector3f(0,0,-GRAVITY_MSS));
// new velocity and position vectors
velocity_ef += accel_earth * delta_time;
position += velocity_ef * delta_time;
velocity_air_ef = velocity_ef + wind_ef;
velocity_air_bf = dcm.transposed() * velocity_air_ef;
}

3
libraries/SITL/SIM_Aircraft.h
View File

@ -232,6 +232,9 @@ protected:
float filtered_servo_angle(const struct sitl_input &input, uint8_t idx);
float filtered_servo_range(const struct sitl_input &input, uint8_t idx);
// extrapolate sensors by a given delta time in seconds
void extrapolate_sensors(float delta_time);
private:
uint64_t last_time_us = 0;
uint32_t frame_counter = 0;

120
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -28,10 +28,11 @@
#include <AP_HAL/AP_HAL.h>
#include <DataFlash/DataFlash.h>
#include "pthread.h"
extern const AP_HAL::HAL& hal;
namespace SITL {
using namespace SITL;
// the asprintf() calls are not worth checking for SITL
#pragma GCC diagnostic ignored "-Wunused-result"
@ -72,6 +73,41 @@ FlightAxis::FlightAxis(const char *home_str, const char *frame_str) :
default:
AP_HAL::panic("Unsupported flightaxis rotation %u\n", (unsigned)rotation);
}
/* Create the thread that will be waiting for data from FlightAxis */
mutex = hal.util->new_semaphore();
int ret = pthread_create(&thread, NULL, update_thread, this);
if (ret != 0) {
AP_HAL::panic("SIM_FlightAxis: failed to create thread");
}
}
/*
update thread trampoline
*/
void *FlightAxis::update_thread(void *arg)
{
FlightAxis *flightaxis = (FlightAxis *)arg;
pthread_setname_np(pthread_self(), "ardupilot-flightaxis");
flightaxis->update_loop();
return nullptr;
}
/*
main update loop
*/
void FlightAxis::update_loop(void)
{
while (true) {
struct sitl_input new_input;
mutex->take(HAL_SEMAPHORE_BLOCK_FOREVER);
new_input = last_input;
mutex->give();
exchange_data(new_input);
}
}
/*
@ -267,7 +303,18 @@ void FlightAxis::exchange_data(const struct sitl_input &input)
scaled_servos[7]);
if (reply) {
mutex->take(HAL_SEMAPHORE_BLOCK_FOREVER);
double lastt_s = state.m_currentPhysicsTime_SEC;
parse_reply(reply);
double dt = state.m_currentPhysicsTime_SEC - lastt_s;
if (dt > 0 && dt < 0.1) {
if (average_frame_time_s < 1.0e-6) {
average_frame_time_s = dt;
}
average_frame_time_s = average_frame_time_s * 0.98 + dt * 0.02;
}
socket_frame_counter++;
mutex->give();
free(reply);
}
}
@ -278,21 +325,43 @@ void FlightAxis::exchange_data(const struct sitl_input &input)
*/
void FlightAxis::update(const struct sitl_input &input)
{
exchange_data(input);
mutex->take(HAL_SEMAPHORE_BLOCK_FOREVER);
last_input = input;
double dt_seconds = state.m_currentPhysicsTime_SEC - last_time_s;
if (dt_seconds < 0) {
// cope with restarting RealFlight while connected
initial_time_s = time_now_us * 1.0e-6f;
last_time_s = state.m_currentPhysicsTime_SEC;
position_offset.zero();
mutex->give();
return;
}
if (dt_seconds < 0.0001f) {
// we probably got a repeated frame
time_now_us += 1;
if (dt_seconds < 0.00001f) {
float delta_time = 0.001;
// don't go past the next expected frame
if (delta_time + extrapolated_s > average_frame_time_s) {
delta_time = average_frame_time_s - extrapolated_s;
}
if (delta_time <= 0) {
usleep(1000);
mutex->give();
return;
}
time_now_us += delta_time * 1.0e6;
extrapolate_sensors(delta_time);
update_position();
update_mag_field_bf();
mutex->give();
usleep(delta_time*1.0e6);
extrapolated_s += delta_time;
report_FPS();
return;
}
extrapolated_s = 0;
if (initial_time_s <= 0) {
dt_seconds = 0.001f;
initial_time_s = state.m_currentPhysicsTime_SEC - dt_seconds;
@ -367,16 +436,15 @@ void FlightAxis::update(const struct sitl_input &input)
update_position();
time_advance();
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));
} else {
printf("Initial position %f %f %f\n", position.x, position.y, position.z);
uint64_t new_time_us = (state.m_currentPhysicsTime_SEC - initial_time_s)*1.0e6;
if (new_time_us < time_now_us) {
uint64_t dt_us = time_now_us - new_time_us;
if (dt_us > 500000) {
// time going backwards
time_now_us = new_time_us;
}
last_frame_count_s = state.m_currentPhysicsTime_SEC;
} else {
time_now_us = new_time_us;
}
last_time_s = state.m_currentPhysicsTime_SEC;
@ -385,6 +453,26 @@ void FlightAxis::update(const struct sitl_input &input)
// update magnetic field
update_mag_field_bf();
mutex->give();
report_FPS();
}
} // namespace SITL
/*
report frame rates
*/
void FlightAxis::report_FPS(void)
{
if (frame_counter++ % 1000 == 0) {
if (last_frame_count_s != 0) {
uint64_t frames = socket_frame_counter - last_socket_frame_counter;
last_socket_frame_counter = socket_frame_counter;
double dt = state.m_currentPhysicsTime_SEC - last_frame_count_s;
printf("%.2f/%.2f FPS avg=%.2f\n",
frames / dt, 1000 / dt, 1.0/average_frame_time_s);
} else {
printf("Initial position %f %f %f\n", position.x, position.y, position.z);
}
last_frame_count_s = state.m_currentPhysicsTime_SEC;
}
}

29
libraries/SITL/SIM_FlightAxis.h
View File

@ -156,14 +156,24 @@ private:
void exchange_data(const struct sitl_input &input);
void parse_reply(const char *reply);
double initial_time_s = 0;
double last_time_s = 0;
bool heli_demix = false;
bool rev4_servos = false;
bool controller_started = false;
uint64_t frame_counter = 0;
uint64_t activation_frame_counter = 0;
double last_frame_count_s = 0;
static void *update_thread(void *arg);
void update_loop(void);
void report_FPS(void);
struct sitl_input last_input;
double average_frame_time_s;
double extrapolated_s;
double initial_time_s;
double last_time_s;
bool heli_demix;
bool rev4_servos;
bool controller_started;
uint64_t frame_counter;
uint64_t activation_frame_counter;
uint64_t socket_frame_counter;
uint64_t last_socket_frame_counter;
double last_frame_count_s;
Vector3f position_offset;
Vector3f last_velocity_ef;
Matrix3f att_rotation;
@ -171,6 +181,9 @@ private:
const char *controller_ip = "127.0.0.1";
uint16_t controller_port = 18083;
pthread_t thread;
AP_HAL::Semaphore *mutex;
};

17
libraries/SITL/SIM_XPlane.cpp
View File

@ -336,26 +336,13 @@ failed:
}
// advance time by 1ms
Vector3f rot_accel;
frame_time_us = 1000;
float delta_time = frame_time_us * 1e-6f;
time_now_us += frame_time_us;
// extrapolate sensors
dcm.rotate(gyro * delta_time);
dcm.normalize();
// work out acceleration as seen by the accelerometers. It sees the kinematic
// acceleration (ie. real movement), plus gravity
accel_body = dcm.transposed() * (accel_earth + Vector3f(0,0,-GRAVITY_MSS));
// new velocity and position vectors
velocity_ef += accel_earth * delta_time;
position += velocity_ef * delta_time;
velocity_air_ef = velocity_ef + wind_ef;
velocity_air_bf = dcm.transposed() * velocity_air_ef;
extrapolate_sensors(delta_time);
update_position();
time_advance();
update_mag_field_bf();