void f3kdb_avisynth::mt_proc(void)
{
volatile mt_info* info = _mt_info;
assert(info);
while (!info->exit)
{
MemoryBarrier();
{
auto info_nv = const_cast<mt_info*>(info);
process_plane(info_nv->n, info_nv->src, info_nv->dst, info_nv->dstp_u, PLANAR_U, info_nv->env);
process_plane(info_nv->n, info_nv->src, info_nv->dst, info_nv->dstp_v, PLANAR_V, info_nv->env);
}
MemoryBarrier();
mt_info_reset_pointers(info);
SetEvent(info->work_complete_event);
WaitForSingleObject(info->work_event, INFINITE);
}
}
bool LogReader::update(uint8_t &type)
{
uint8_t hdr[3];
if (::read(fd, hdr, 3) != 3) {
return false;
}
if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
printf("bad log header\n");
return false;
}
if (hdr[2] == LOG_FORMAT_MSG) {
struct log_Format &f = formats[num_formats];
memcpy(&f, hdr, 3);
if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) {
return false;
}
if (num_formats < LOGREADER_MAX_FORMATS-1) {
num_formats++;
}
type = f.type;
return true;
}
uint8_t i;
for (i=0; i<num_formats; i++) {
if (formats[i].type == hdr[2]) break;
}
if (i == num_formats) {
return false;
}
const struct log_Format &f = formats[i];
uint8_t data[f.length];
memcpy(data, hdr, 3);
if (::read(fd, &data[3], f.length-3) != f.length-3) {
return false;
}
switch (f.type) {
case LOG_MESSAGE_MSG: {
struct log_Message msg;
if(sizeof(msg) != f.length) {
printf("Bad MESSAGE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
if (strncmp(msg.msg, "ArduPlane", strlen("ArduPlane")) == 0) {
vehicle = VEHICLE_PLANE;
::printf("Detected Plane\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
ahrs.set_fly_forward(true);
} else if (strncmp(msg.msg, "ArduCopter", strlen("ArduCopter")) == 0) {
vehicle = VEHICLE_COPTER;
::printf("Detected Copter\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
ahrs.set_fly_forward(false);
} else if (strncmp(msg.msg, "ArduRover", strlen("ArduRover")) == 0) {
vehicle = VEHICLE_ROVER;
::printf("Detected Rover\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
ahrs.set_fly_forward(true);
}
dataflash.Log_Write_Message(msg.msg);
break;
}
case LOG_IMU_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
if (gyro_mask & 1) {
ins.set_gyro(0, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 1) {
ins.set_accel(0, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
dataflash.Log_Write_IMU(ins);
break;
}
case LOG_IMU2_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU2 length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
if (gyro_mask & 2) {
ins.set_gyro(1, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 2) {
ins.set_accel(1, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
dataflash.Log_Write_IMU(ins);
break;
}
case LOG_IMU3_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU3 length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
if (gyro_mask & 4) {
ins.set_gyro(2, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 4) {
ins.set_accel(2, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
dataflash.Log_Write_IMU(ins);
break;
}
case LOG_GPS_MSG: {
struct log_GPS msg;
if(sizeof(msg) != f.length) {
printf("Bad GPS length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.apm_time);
Location loc;
loc.lat = msg.latitude;
loc.lng = msg.longitude;
loc.alt = msg.altitude;
loc.options = 0;
Vector3f vel(msg.ground_speed*0.01f*cosf(radians(msg.ground_course*0.01f)),
msg.ground_speed*0.01f*sinf(radians(msg.ground_course*0.01f)),
msg.vel_z);
gps.setHIL(0, (AP_GPS::GPS_Status)msg.status,
msg.apm_time,
loc,
vel,
msg.num_sats,
msg.hdop,
msg.vel_z != 0);
if (msg.status == 3 && ground_alt_cm == 0) {
ground_alt_cm = msg.altitude;
}
rel_altitude = msg.rel_altitude*0.01f;
dataflash.Log_Write_GPS(gps, 0, rel_altitude);
break;
}
case LOG_GPS2_MSG: {
struct log_GPS2 msg;
if(sizeof(msg) != f.length) {
printf("Bad GPS2 length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.apm_time);
Location loc;
loc.lat = msg.latitude;
loc.lng = msg.longitude;
loc.alt = msg.altitude;
loc.options = 0;
Vector3f vel(msg.ground_speed*0.01f*cosf(radians(msg.ground_course*0.01f)),
msg.ground_speed*0.01f*sinf(radians(msg.ground_course*0.01f)),
msg.vel_z);
gps.setHIL(1, (AP_GPS::GPS_Status)msg.status,
msg.apm_time,
loc,
vel,
msg.num_sats,
msg.hdop,
msg.vel_z != 0);
if (msg.status == 3 && ground_alt_cm == 0) {
ground_alt_cm = msg.altitude;
}
dataflash.Log_Write_GPS(gps, 1, rel_altitude);
break;
}
case LOG_SIMSTATE_MSG: {
struct log_AHRS msg;
if(sizeof(msg) != f.length) {
printf("Bad SIMSTATE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.time_ms);
sim_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
break;
}
case LOG_BARO_MSG: {
struct log_BARO msg;
if (sizeof(msg) == f.length+sizeof(float)) {
// old style, without climbrate
memset(&msg, 0, sizeof(msg));
memcpy(&msg, data, f.length);
} else if (sizeof(msg) == f.length) {
memcpy(&msg, data, sizeof(msg));
} else {
printf("Bad LOG_BARO length %u expected %u\n",
(unsigned)f.length, (unsigned)sizeof(msg));
exit(1);
}
wait_timestamp(msg.timestamp);
baro.setHIL(0, msg.pressure, msg.temperature*0.01f);
dataflash.Log_Write_Baro(baro);
break;
}
case LOG_PARAMETER_MSG: {
struct log_Parameter msg;
if(sizeof(msg) != f.length) {
printf("Bad LOG_PARAMETER length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
set_parameter(msg.name, msg.value);
break;
}
case LOG_AHR2_MSG: {
struct log_AHRS msg;
if(sizeof(msg) != f.length) {
printf("Bad AHR2 length %u should be %u\n", (unsigned)f.length, (unsigned)sizeof(msg));
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.time_ms);
ahr2_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
break;
}
default:
if (vehicle == VEHICLE_PLANE) {
process_plane(f.type, data, f.length);
} else if (vehicle == VEHICLE_COPTER) {
process_copter(f.type, data, f.length);
} else if (vehicle == VEHICLE_ROVER) {
process_rover(f.type, data, f.length);
}
break;
}
type = f.type;
return true;
}
bool LogReader::update(uint8_t &type)
{
uint8_t hdr[3];
if (::read(fd, hdr, 3) != 3) {
return false;
}
if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
printf("bad log header\n");
return false;
}
if (hdr[2] == LOG_FORMAT_MSG) {
struct log_Format &f = formats[num_formats];
memcpy(&f, hdr, 3);
if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) {
return false;
}
num_formats++;
type = f.type;
return true;
}
uint8_t i;
for (i=0; i<num_formats; i++) {
if (formats[i].type == hdr[2]) break;
}
if (i == num_formats) {
return false;
}
const struct log_Format &f = formats[i];
uint8_t data[f.length];
memcpy(data, hdr, 3);
if (::read(fd, &data[3], f.length-3) != f.length-3) {
return false;
}
switch (f.type) {
case LOG_MESSAGE_MSG: {
struct log_Message msg;
if(sizeof(msg) != f.length) {
printf("Bad MESSAGE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
if (strncmp(msg.msg, "ArduPlane", strlen("ArduPlane")) == 0) {
vehicle = VEHICLE_PLANE;
::printf("Detected Plane\n");
} else if (strncmp(msg.msg, "ArduCopter", strlen("ArduCopter")) == 0) {
vehicle = VEHICLE_COPTER;
::printf("Detected Copter\n");
} else if (strncmp(msg.msg, "ArduRover", strlen("ArduRover")) == 0) {
vehicle = VEHICLE_ROVER;
::printf("Detected Rover\n");
}
break;
}
case LOG_IMU_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
if (gyro_mask & 1) {
ins.set_gyro(0, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 1) {
ins.set_accel(0, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
break;
}
case LOG_IMU2_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU2 length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
if (gyro_mask & 2) {
ins.set_gyro(1, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 2) {
ins.set_accel(1, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
break;
}
case LOG_GPS_MSG: {
struct log_GPS msg;
if(sizeof(msg) != f.length) {
printf("Bad GPS length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.apm_time);
gps->setHIL(msg.status==3?GPS::FIX_3D:GPS::FIX_NONE,
msg.apm_time,
msg.latitude*1.0e-7f,
msg.longitude*1.0e-7f,
msg.altitude*1.0e-2f,
msg.ground_speed*1.0e-2f,
msg.ground_course*1.0e-2f,
0,
msg.num_sats);
if (msg.status == 3 && ground_alt_cm == 0) {
ground_alt_cm = msg.altitude;
}
rel_altitude = msg.rel_altitude*0.01f;
break;
}
case LOG_SIMSTATE_MSG: {
struct log_AHRS msg;
if(sizeof(msg) != f.length) {
printf("Bad SIMSTATE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.time_ms);
sim_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
break;
}
case LOG_BARO_MSG: {
struct log_BARO msg;
if(sizeof(msg) != f.length) {
printf("Bad LOG_BARO length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
baro.setHIL(msg.pressure, msg.temperature*0.01f);
break;
}
case LOG_PARAMETER_MSG: {
struct log_Parameter msg;
if(sizeof(msg) != f.length) {
printf("Bad LOG_PARAMETER length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
set_parameter(msg.name, msg.value);
break;
}
default:
if (vehicle == VEHICLE_PLANE) {
process_plane(f.type, data, f.length);
} else if (vehicle == VEHICLE_COPTER) {
process_copter(f.type, data, f.length);
} else if (vehicle == VEHICLE_ROVER) {
process_rover(f.type, data, f.length);
}
break;
}
type = f.type;
return true;
}
PVideoFrame __stdcall f3kdb_avisynth::GetFrame(int n, IScriptEnvironment* env)
{
PVideoFrame src = child->GetFrame(n, env);
// interleaved 16bit output needs extra alignment
PVideoFrame dst = env->NewVideoFrame(vi, PLANE_ALIGNMENT * 2);
if (vi.IsPlanar() && !vi.IsY8())
{
if (!_mt)
{
process_plane(n, src, dst, dst->GetWritePtr(PLANAR_Y), PLANAR_Y, env);
process_plane(n, src, dst, dst->GetWritePtr(PLANAR_U), PLANAR_U, env);
process_plane(n, src, dst, dst->GetWritePtr(PLANAR_V), PLANAR_V, env);
} else {
bool new_thread = _mt_info == NULL;
if (new_thread)
{
_mt_info = mt_info_create();
if (!_mt_info) {
env->ThrowError("f3kdb_avisynth: Failed to allocate mt_info.");
return NULL;
}
}
// we must get write pointer before copying the frame pointer
// otherwise NULL will be returned
unsigned char* dstp_y = dst->GetWritePtr(PLANAR_Y);
_mt_info->n = n;
_mt_info->dstp_u = dst->GetWritePtr(PLANAR_U);
_mt_info->dstp_v = dst->GetWritePtr(PLANAR_V);
_mt_info->env = env;
MemoryBarrier();
{
auto info_nv = const_cast<mt_info*>(_mt_info);
info_nv->dst = dst;
info_nv->src = src;
}
MemoryBarrier();
if (!new_thread)
{
SetEvent(_mt_info->work_event);
}
else
{
_mt_info->thread_handle = (HANDLE)_beginthreadex(NULL, 0, mt_proc_wrapper, this, 0, NULL);
if (!_mt_info->thread_handle) {
int err = errno;
mt_info_destroy(_mt_info);
_mt_info = NULL;
env->ThrowError("f3kdb_avisynth: Failed to create worker thread, code = %d.", err);
return NULL;
}
}
process_plane(n, src, dst, dstp_y, PLANAR_Y, env);
WaitForSingleObject(_mt_info->work_complete_event, INFINITE);
}
} else {
// Y8
process_plane(n, src, dst, dst->GetWritePtr(), PLANAR_Y, env);
}
return dst;
}
