FT_Error MyFaceRequester(FTC_FaceID face_id, FT_Library library, FT_Pointer request_data, FT_Face *aface)
{
FT_Error result = FT_New_Face(library, face_id, 0, aface);
if(!result) gDebug(LOG_INFO,ROUTINE_NAME,"Font '%s' loaded", (char*)face_id);
else gDebug(LOG_ERR,ROUTINE_NAME,"Font '%s' not loaded", (char*)face_id);
return result;
}
int RenderChar(FT_ULong currentchar, int sx, int sy, int ex, unsigned char *color)
{
int bpp=4;
int row, pitch, bit, x = 0, y = 0;
FT_UInt glyphindex;
FT_Vector kerning;
FT_Error error;
if(!(glyphindex = FT_Get_Char_Index(face, currentchar)))
{
gDebug(LOG_ERR,ROUTINE_NAME,"FT_Get_Char_Index %x \"%c\"", (int)currentchar,(int)currentchar);
return 0;
}
FTC_Node anode;
if((error = FTC_SBitCache_Lookup(cache, &desc, glyphindex, &sbit, &anode)))
{
gDebug(LOG_ERR,ROUTINE_NAME,"FTC_SBitCache_Lookup %x \"%c\"", (int)currentchar,(int)currentchar);
return 0;
}
if(use_kerning)
{
FT_Get_Kerning(face, prev_glyphindex, glyphindex, ft_kerning_default, &kerning);
prev_glyphindex = glyphindex;
kerning.x >>= 6;
}
else
int
gRecvInt()
{
int val;
gDebug( "Receiving int from %s ...\n", who );
gRead( &val, sizeof(val) );
gDebug( " -> %d (%#x)\n", val, val );
return val;
}
char *
gRecvStr()
{
int len;
char *buf;
gDebug( "Receiving string from %s ...\n", who );
buf = igRecvArr( &len );
gDebug( " -> %'.*s\n", len, buf );
return buf;
}
void IMU::Calibrate( float dt, bool all )
{
if ( mCalibrationAccum < 2000 ) {
bool last_pass = ( mCalibrationAccum >= 1999 );
for ( auto dev : Sensor::Devices() ) {
if ( all or dynamic_cast< Gyroscope* >( dev ) != nullptr ) {
dev->Calibrate( dt, last_pass );
}
}
} else if ( all and mCalibrationAccum < 3000 ) {
UpdateSensors( dt );
mGravity += mAcceleration / 1000.0f;
} else {
mState = CalibrationDone;
mAcceleration = Vector3f();
mGyroscope = Vector3f();
mMagnetometer = Vector3f();
mRPY = Vector3f();
mdRPY = Vector3f();
mRate = Vector3f();
gDebug() << "Calibration done !\n";
}
mCalibrationAccum++;
}
std::list< int > I2C::ScanAll()
{
std::list< int > ret;
int fd = open( "/dev/i2c-1", O_RDWR );
int res = 0;
int byte_ = 0;
for ( int i = 0; i < 128; i++ ) {
if ( ioctl( fd, I2C_SLAVE, i ) < 0) {
if ( errno == EBUSY ) {
continue;
} else {
gDebug() << "Error: Could not set address to " << i << " : " << strerror(errno);
exit(0);
}
}
res = read( fd, &byte_, 1 );
/*
if ( ( i >= 0x30 && i <= 0x37 ) || ( i >= 0x50 && i <= 0x5F ) ) {
res = read( fd, &byte_, 1 );
} else {
byte_ = 0x0F;
res = write( fd, &byte_, 1 );
}
*/
if ( res > 0 ) {
ret.push_back( i );
}
}
close( fd );
return ret;
}
GUIStatusBar::GUIStatusBar(const PrivatelyConstruct& dummy,
const String& style, const GUIDimensions& dimensions)
:GUIElementContainer(dimensions, style)
{
mPanel = GUIPanel::create();
mBgPanel = GUIPanel::create(1);
_registerChildElement(mPanel);
_registerChildElement(mBgPanel);
mBackground = GUITexture::create(GUIOptions(GUIOption::flexibleWidth()), getSubStyleName(getGUIBackgroundTypeName()));
mMessage = GUIButton::create(HString(L""), GUIOptions(GUIOption::flexibleWidth()), getSubStyleName(getGUIMessageTypeName()));
mScene = GUILabel::create(HString(L"Scene: Unnamed"), GUIOptions(GUIOption::fixedWidth(150)));
mProject = GUILabel::create(HString(L"Project: None"), GUIOptions(GUIOption::fixedWidth(200)));
mCompiling = GUILabel::create(HString(L"Compiling..."), GUIOptions(GUIOption::fixedWidth(100)));
GUILayoutY* vertLayout = mPanel->addNewElement<GUILayoutY>();
vertLayout->addNewElement<GUIFixedSpace>(3);
GUILayoutX* horzLayout = vertLayout->addNewElement<GUILayoutX>();
horzLayout->addNewElement<GUIFixedSpace>(10);
horzLayout->addElement(mMessage);
horzLayout->addNewElement<GUIFlexibleSpace>();
horzLayout->addElement(mScene);
horzLayout->addNewElement<GUIFixedSpace>(10);
horzLayout->addElement(mProject);
horzLayout->addNewElement<GUIFixedSpace>(10);
horzLayout->addElement(mCompiling);
horzLayout->addNewElement<GUIFixedSpace>(10);
mBgPanel->addElement(mBackground);
mCompiling->setActive(false);
mLogEntryAddedConn = gDebug().onLogModified.connect(std::bind(&GUIStatusBar::logModified, this));
mMessageBtnPressedConn = mMessage->onClick.connect(std::bind(&GUIStatusBar::messageBtnClicked, this));
}
int Socket::Write( const void* buf, uint32_t len, int timeout )
{
if ( !mConnected ) {
return -1;
}
int ret = 0;
if ( mPortType == UDP or mPortType == UDPLite ) {
if ( mBroadcast ) {
mClientSin.sin_family = AF_INET;
mClientSin.sin_port = htons( mPort );
mClientSin.sin_addr.s_addr = inet_addr( "192.168.32.255" );
}
uint32_t sendsize = sizeof( mClientSin );
ret = sendto( mSocket, buf, len, 0, (SOCKADDR *)&mClientSin, sendsize );
} else {
ret = send( mClientSocket, buf, len, 0 );
}
if ( ret <= 0 and ( errno == EAGAIN or errno == -EAGAIN ) ) {
return 0;
}
if ( ret < 0 and mPortType != UDP and mPortType != UDPLite ) {
gDebug() << "TCP disconnected\n";
mConnected = false;
return -1;
}
return ret;
}
void
gSendArr( int len, const char *buf )
{
gDebug( "Sending array %02[:*hhx to %s\n", len, buf, who );
gWrite( &len, sizeof(len) );
gWrite( buf, len );
}
char *
gRecvArr( int *num )
{
char *arr;
gDebug( "Receiving array from %s ...\n", who );
gRead( num, sizeof(*num) );
gDebug( " -> %d bytes\n", *num );
if (!*num)
return (char *)0;
if (!(arr = malloc( *num )))
logPanic( "No memory for read buffer\n" );
gRead( arr, *num );
gDebug( " -> %02[*hhx\n", *num, arr );
return arr;
}
bool Raspicam::LiveThreadRun()
{
if ( !mLink->isConnected() ) {
mLink->Connect();
if ( mLink->isConnected() ) {
gDebug() << "Raspicam connected !\n";
mLink->setBlocking( false );
IL::Camera::SetState( IL::Component::StateExecuting );
mEncoder->SetState( IL::Component::StateExecuting );
mLiveFrameCounter = 0;
}
return true;
} else if ( mRecording and Board::GetTicks() - mLedTick >= 1000 * 500 ) {
GPIO::Write( 32, ( mLedState = !mLedState ) );
mLedTick = Board::GetTicks();
}
uint8_t data[65536] = { 0 };
uint32_t datalen = mEncoder->getOutputData( data );
if ( (int32_t)datalen > 0 ) {
mLiveFrameCounter++;
LiveSend( (char*)data, datalen );
if ( mRecording ) {
RecordWrite( (char*)data, datalen );
}
}
return true;
}
void IMU::RecalibrateAll()
{
mCalibrationAccum = 0;
mState = CalibratingAll;
mRPYOffset = Vector3f();
gDebug() << "Calibrating all sensors...\n";
}
void
gSendStr( const char *buf )
{
int len = buf ? strlen( buf ) + 1 : 0;
gDebug( "Sending string %'s to %s\n", buf, who );
gWrite( &len, sizeof(len) );
gWrite( buf, len );
}
void Frame::CalibrateESCs()
{
fDebug0();
gDebug() << "Disabling ESCs\n";
for ( Motor* m : mMotors ) {
m->Disable();
}
gDebug() << "Waiting 10 seconds...\n";
usleep( 10 * 1000 * 1000 );
gDebug() << "Setting maximal speed\n";
for ( Motor* m : mMotors ) {
m->setSpeed( 1.0f, true );
}
gDebug() << "Waiting 10 seconds...\n";
usleep( 10 * 1000 * 1000 );
gDebug() << "Setting minimal speed\n";
for ( Motor* m : mMotors ) {
m->setSpeed( 0.0f, true );
}
gDebug() << "Waiting 2 seconds...\n";
usleep( 2 * 1000 * 1000 );
gDebug() << "Disarm all ESCs\n";
for ( Motor* m : mMotors ) {
m->Disarm();
}
}
I2C::I2C( int addr )
: mAddr( addr )
{
pthread_mutex_lock( &mMutex );
if ( mFD < 0 ) {
mFD = open( "/dev/i2c-1", O_RDWR );
gDebug() << "fd : " << mFD << "\n";
}
pthread_mutex_unlock( &mMutex );
}
Link* Link::Create( Config* config, const std::string& lua_object )
{
std::string type = config->string( lua_object + ".link_type" );
if ( mKnownLinks.find( type ) != mKnownLinks.end() ) {
return mKnownLinks[ type ]( config, lua_object );
}
gDebug() << "FATAL ERROR : Link type \"" << type << "\" not supported !\n";
return nullptr;
} // -- Socket{ type = "TCP/UDP/UDPLite", port = port_number[, broadcast = true/false] } <= broadcast is false by default
std::string Board::infos()
{
std::string res = "";
res += "Type:" BOARD "\n";
res += "CPU:" + readproc( "/proc/cpuinfo", "model name" ) + std::string( "\n" );
res += "CPU frequency:" + std::to_string( std::atoi( readproc( "/sys/devices/system/cpu/cpu0/cpufreq/scaling_cur_freq" ).c_str() ) / 1000 ) + std::string( "MHz\n" );
res += "CPU cores count:" + std::to_string( sysconf( _SC_NPROCESSORS_ONLN ) ) + std::string( "\n" );
gDebug() << "res : \"" << res << "\"\n";
return res;
}
int Socket::Read( void* buf, uint32_t len, int timeout )
{
if ( !mConnected ) {
return -1;
}
int ret = 0;
memset( buf, 0, len );
// timeout = 500;
if ( timeout > 0 ) {
struct timeval tv;
tv.tv_sec = timeout / 1000;
tv.tv_usec = 1000 * ( timeout % 1000 );
setsockopt( mSocket, SOL_SOCKET, SO_RCVTIMEO, (char*)&tv, sizeof(struct timeval) );
}
if ( mPortType == UDP or mPortType == UDPLite ) {
uint32_t fromsize = sizeof( mClientSin );
ret = recvfrom( mSocket, buf, len, 0, (SOCKADDR *)&mClientSin, &fromsize );
if ( ret <= 0 and errno != EAGAIN ) {
gDebug() << "UDP disconnected ( " << ret << " : " << strerror( errno ) << " )\n";
mConnected = false;
return -1;
}
// return -1;
} else {
ret = recv( mClientSocket, buf, len, MSG_NOSIGNAL );
// if ( ( ret <= 0 and errno != EAGAIN ) or ( errno == EAGAIN and timeout > 0 ) ) {
if ( ret <= 0 ) {
gDebug() << "TCP disconnected ( " << strerror( errno ) << " )\n";
mConnected = false;
return -1;
}
}
return ret;
}
int Raspicam::LiveSend( char* data, int datalen )
{
if ( datalen <= 0 ) {
return datalen;
}
int err = mLink->Write( (uint8_t*)data, datalen, 0 );
if ( err < 0 ) {
gDebug() << "Link->Write() error : " << strerror(errno) << " (" << errno << ")\n";
return -1;
}
return 0;
}
static char *
igRecvArr( int *rlen )
{
int len;
char *buf;
gRead( &len, sizeof(len) );
*rlen = len;
gDebug( " -> %d bytes\n", len );
if (!len)
return (char *)0;
if (!(buf = malloc( len )))
logPanic( "No memory for read buffer\n" );
gRead( buf, len );
return buf;
}
MonoArray* ScriptDebug::internal_getMessages()
{
Vector<LogEntry> entries = gDebug().getLog().getEntries();
UINT32 numEntries = (UINT32)entries.size();
ScriptArray output = ScriptArray::create<ScriptLogEntry>(numEntries);
for (UINT32 i = 0; i < numEntries; i++)
{
MonoString* message = MonoUtil::stringToMono(entries[i].getMessage());
ScriptLogEntryData scriptEntry = { entries[i].getChannel(), message };
output.set(i, scriptEntry);
}
return output.getInternal();
}
uint64_t OpenGL43Instance::ReferenceImage( Image* image )
{
uint32_t glTextureID;
glGenTextures( 1, &glTextureID );
glBindTexture( GL_TEXTURE_2D, glTextureID );
glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA8, image->width(), image->height(), 0, GL_RGBA, GL_UNSIGNED_BYTE, image->data() );
glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT );
glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT );
glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR );
glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
((OpenGL43Instance*)Instance::baseInstance())->AffectVRAM( image->width() * image->height() * sizeof( uint32_t ) );
gDebug() << "New texture referenced ( VRAM usage : " << ( ((OpenGL43Instance*)Instance::baseInstance())->mGpuRamCounter / 1024 ) << " KB )\n";
return glTextureID;
}
bool IMU::SensorsThreadRun()
{
if ( mState == Running ) {
bool rate = ( mMain->stabilizer()->mode() == Stabilizer::Rate );
float dt = ((float)( Board::GetTicks() - mSensorsThreadTick ) ) / 1000000.0f;
UpdateSensors( dt, rate );
mSensorsThreadTickRate = Board::WaitTick( 1000000 / 500, mSensorsThreadTickRate, -200 );
imu_fps++;
if ( Board::GetTicks() - imu_ticks >= 1000 * 1000 * 5 ) {
gDebug() << "Sampling rate : " << ( imu_fps / 5 ) << " Hz\n";
imu_fps = 0;
imu_ticks = Board::GetTicks();
}
} else {
usleep( 1000 * 100 );
}
return true;
}
void IMU::Recalibrate()
{
bool cal_all = false;
for ( Accelerometer* dev : Sensor::Accelerometers() ) {
if ( not dev->calibrated() ) {
cal_all = true;
break;
}
}
if (cal_all ) {
RecalibrateAll();
return;
}
mCalibrationAccum = 0;
mState = Calibrating;
mRPYOffset = Vector3f();
gDebug() << "Calibrating...\n";
}
Link* Socket::Instanciate( Config* config, const std::string& lua_object )
{
PortType type = UDPLite; // Default to UDPLite
std::string stype = config->string( lua_object + ".type" );
if ( stype == "TCP" ) {
type = TCP;
} else if ( stype == "UDP" ) {
type = UDP;
} else if ( stype == "UDPLite" ) {
type = UDPLite;
} else {
gDebug() << "FATAL ERROR : Unsupported Socket type \"" << stype << "\" !\n";
}
int port = config->integer( lua_object + ".port" );
bool broadcast = config->boolean( lua_object + ".broadcast" );
return new Socket( port, type, broadcast );
}
void Config::Reload()
{
luaL_dostring( L, "function Vector( x, y, z, w ) return { x = x, y = y, z = z, w = w } end" );
luaL_dostring( L, "function Socket( params ) params.link_type = \"Socket\" ; return params end" );
luaL_dostring( L, "function RawWifi( params ) params.link_type = \"RawWifi\" ; params.device = \"wlan0\" ; if params.blocking == nil then params.blocking = true end ; if params.retries == nil then params.retries = 2 end ; return params end" );
luaL_dostring( L, "function Voltmeter( params ) params.sensor_type = \"Voltmeter\" ; return params end" );
luaL_dostring( L, ( "board = { type = \"" + std::string( BOARD ) + "\" }" ).c_str() );
luaL_dostring( L, "frame = { motors = { front_left = {}, front_right = {}, rear_left = {}, rear_right = {} } }" );
luaL_dostring( L, "battery = {}" );
luaL_dostring( L, "camera = {}" );
luaL_dostring( L, "controller = {}" );
luaL_dostring( L, "stabilizer = { loop_time = 2500 }" );
luaL_dostring( L, "accelerometers = {}" );
luaL_dostring( L, "gyroscopes = {}" );
luaL_dostring( L, "magnetometers = {}" );
luaL_dostring( L, "altimeters = {}" );
luaL_dostring( L, "GPSes = {}" );
luaL_dostring( L, "user_sensors = {}" );
luaL_dostring( L, "function RegisterSensor( name, params ) user_sensors[name] = params ; return params end" );
luaL_loadfile( L, mFilename.c_str() );
int ret = lua_pcall( L, 0, LUA_MULTRET, 0 );
if ( ret != 0 ) {
gDebug() << "Lua : Error while executing file \"" << mFilename << "\" : \"" << lua_tostring( L, -1 ) << "\"\n";
return;
}
std::list< std::string > user_sensors;
LocateValue( "user_sensors" );
lua_pushnil( L );
while( lua_next( L, -2 ) != 0 ) {
std::string key = lua_tostring( L, -2 );
user_sensors.emplace_back( key );
lua_pop( L, 1 );
}
for ( std::string s : user_sensors ) {
Sensor::RegisterDevice( s, this, "user_sensors." + s );
}
}
File::File( File* side, std::string filename, File::MODE mode )
: mType( FILE )
, mMode( mode )
{
fDebug( side, filename, mode );
std::string path = side->mPath.substr( 0, side->mPath.rfind( "/" ) + 1 ) + filename;
gDebug() << "Resulting path : '" << path << "'\n";
std::ios_base::openmode std_mode = std::ios_base::binary;
if ( mode & READ ) {
std_mode |= std::ios_base::in;
}
if ( mode & WRITE ) {
std_mode |= std::ios_base::out;
}
mPath = path;
#ifdef GE_ANDROID
mIsAsset = false;
mStream = new std::fstream( filename, std_mode );
if ( !mStream->is_open() ) {
mStream = new std::fstream( std::string( BaseWindow::androidState()->activity->internalDataPath ) + "/" + filename, std_mode );
}
if ( !mStream->is_open() ) {
mStream = (std::fstream*)AAssetManager_open( BaseWindow::androidState()->activity->assetManager, filename.c_str(), AASSET_MODE_STREAMING );
mIsAsset = true;
}
#elif GE_IOS
mStream = new std::fstream( filename, std_mode );
if ( !mStream->is_open() ) {
delete mStream;
mStream = _ge_iOSOpen( filename.c_str(), std_mode );
}
#else
mStream = new std::fstream( path, std_mode );
#endif
}
void MPU9150Accel::Calibrate( float dt, bool last_pass )
{
if ( mCalibrated ) {
mCalibrated = false;
mCalibrationAccum = Vector4f();
mOffset = Vector3f();
}
Vector3f accel;
Read( &accel, true );
mCalibrationAccum += Vector4f( accel, 1.0f );
if ( last_pass ) {
mOffset = mCalibrationAccum.xyz() / mCalibrationAccum.w;
mCalibrationAccum = Vector4f();
mCalibrated = true;
gDebug() << "MPU9150 SAVING CALIBRATED OFFSETS !\n";
aDebug( "mOffset", mOffset.x, mOffset.y, mOffset.z );
Board::SaveRegister( "MPU9150:Accelerometer:Offset:X", std::to_string( mOffset.x ) );
Board::SaveRegister( "MPU9150:Accelerometer:Offset:Y", std::to_string( mOffset.y ) );
Board::SaveRegister( "MPU9150:Accelerometer:Offset:Z", std::to_string( mOffset.z ) );
}
}
void
gSendInt( int val )
{
gDebug( "Sending int %d (%#x) to %s\n", val, val, who );
gWrite( &val, sizeof(val) );
}
void GUIStatusBar::logModified()
{
LogEntry entry;
if(!gDebug().getLog().getLastEntry(entry))
{
GUIContent messageContent(HString(L""));
mMessage->setContent(messageContent);
return;
}
HSpriteTexture iconTexture;
Color textColor = COLOR_INFO;
UINT32 logChannel = entry.getChannel();
switch (logChannel)
{
case (UINT32)DebugChannel::Debug:
iconTexture = BuiltinEditorResources::instance().getLogMessageIcon(LogMessageIcon::Info, 16, false);
break;
case (UINT32)DebugChannel::Warning:
case (UINT32)DebugChannel::CompilerWarning:
iconTexture = BuiltinEditorResources::instance().getLogMessageIcon(LogMessageIcon::Warning, 16, false);
textColor = COLOR_WARNING;
break;
case (UINT32)DebugChannel::Error:
case (UINT32)DebugChannel::CompilerError:
iconTexture = BuiltinEditorResources::instance().getLogMessageIcon(LogMessageIcon::Error, 16, false);
textColor = COLOR_ERROR;
break;
}
WString message = toWString(entry.getMessage());
size_t lfPos = message.find_first_of('\n');
size_t crPos = message.find_first_of('\r');
size_t newlinePos;
if (lfPos != WString::npos)
{
if (crPos != WString::npos)
newlinePos = std::min(lfPos, crPos);
else
newlinePos = lfPos;
}
else if (crPos != WString::npos)
newlinePos = crPos;
else
newlinePos = -1;
if (newlinePos == -1)
{
GUIContent messageContent(HString(message), iconTexture);
mMessage->setContent(messageContent);
mMessage->setTint(textColor);
}
else
{
WString firstLine = message.substr(0, newlinePos);
GUIContent messageContent(HString(firstLine), iconTexture);
mMessage->setContent(messageContent);
mMessage->setTint(textColor);
}
}
