bool CDRMUtils::InitDrm()
{
if(m_fd >= 0)
{
/* caps need to be set before allocating connectors, encoders, crtcs, and planes */
auto ret = drmSetClientCap(m_fd, DRM_CLIENT_CAP_UNIVERSAL_PLANES, 1);
if (ret)
{
CLog::Log(LOGERROR, "CDRMUtils::%s - failed to set Universal planes capability: %s", __FUNCTION__, strerror(errno));
return false;
}
if(!GetResources())
{
return false;
}
if(!GetConnector())
{
return false;
}
if(!GetEncoder())
{
return false;
}
if(!GetCrtc())
{
return false;
}
if(!GetPlanes())
{
return false;
}
}
drmModeFreeResources(m_drm_resources);
m_drm_resources = nullptr;
if(m_fd < 0)
{
return false;
}
if(!GetPreferredMode())
{
return false;
}
drmSetMaster(m_fd);
m_orig_crtc = drmModeGetCrtc(m_fd, m_crtc->crtc->crtc_id);
return true;
}
/*
* Document-method: encode
*
* call-seq: encode(obj[, io[, &block]])
*
* +obj+ is the Ruby object to encode to JSON
*
* +io+ is an optional IO used to stream the encoded JSON string to.
* If +io+ isn't specified, this method will return the resulting JSON string. If +io+ is specified, this method returns nil
*
* If an optional block is passed, it's called when encoding is complete and passed the resulting JSON string
*
* It should be noted that you can reuse an instance of this class to continue encoding multiple JSON
* to the same stream. Just continue calling this method, passing it the same IO object with new/different
* ruby objects to encode. This is how streaming is accomplished.
*/
static VALUE rb_yajl_encoder_encode(int argc, VALUE * argv, VALUE self) {
yajl_encoder_wrapper * wrapper;
const unsigned char * buffer;
unsigned int len;
VALUE obj, io, blk, outBuff;
GetEncoder(self, wrapper);
rb_scan_args(argc, argv, "11&", &obj, &io, &blk);
if (blk != Qnil) {
wrapper->on_progress_callback = blk;
}
/* begin encode process */
yajl_encode_part(wrapper, obj, io);
/* just make sure we output the remaining buffer */
yajl_gen_get_buf(wrapper->encoder, &buffer, &len);
outBuff = rb_str_new((const char *)buffer, len);
#ifdef HAVE_RUBY_ENCODING_H
rb_enc_associate(outBuff, utf8Encoding);
#endif
yajl_gen_clear(wrapper->encoder);
if (io != Qnil) {
rb_io_write(io, outBuff);
if (wrapper->terminator != 0 && wrapper->terminator != Qnil) {
rb_io_write(io, wrapper->terminator);
}
return Qnil;
} else if (blk != Qnil) {
rb_funcall(blk, intern_call, 1, outBuff);
if (wrapper->terminator != 0) {
rb_funcall(blk, intern_call, 1, wrapper->terminator);
}
return Qnil;
} else {
if (wrapper->terminator != 0 && wrapper->terminator != Qnil) {
rb_str_concat(outBuff, wrapper->terminator);
}
return outBuff;
}
return Qnil;
}
/*
* Document-method: on_progress
*
* call-seq: on_progress = Proc.new {|str| ...}
*
* This callback setter allows you to pass a Proc/lambda or any other object that responds to #call.
*
* It will pass the caller a chunk of the encode buffer after it's reached it's internal max buffer size (defaults to 8kb).
* For example, encoding a large object that would normally result in 24288 bytes of data will result in 3 calls to this callback (assuming the 8kb default encode buffer).
*/
static VALUE rb_yajl_encoder_set_progress_cb(VALUE self, VALUE callback) {
yajl_encoder_wrapper * wrapper;
GetEncoder(self, wrapper);
wrapper->on_progress_callback = callback;
return Qnil;
}
// submission
// much of this is cut and paste from nsFormFrame::OnSubmit
NS_IMETHODIMP
nsIsIndexFrame::OnSubmit(nsPresContext* aPresContext)
{
if (!mContent || !mInputContent) {
return NS_ERROR_UNEXPECTED;
}
if (mContent->IsEditable()) {
return NS_OK;
}
nsresult result = NS_OK;
// Begin ProcessAsURLEncoded
nsAutoString data;
nsCOMPtr<nsIUnicodeEncoder> encoder;
if(NS_FAILED(GetEncoder(getter_AddRefs(encoder)))) // Non-fatal error
encoder = nsnull;
nsAutoString value;
GetInputValue(value);
URLEncode(value, encoder, data);
// End ProcessAsURLEncoded
// make the url string
nsAutoString href;
// Get the document.
// We'll need it now to form the URL we're submitting to.
// We'll also need it later to get the DOM window when notifying form submit observers (bug 33203)
nsCOMPtr<nsIDocument> document = mContent->GetDocument();
if (!document) return NS_OK; // No doc means don't submit, see Bug 28988
// Resolve url to an absolute url
nsIURI *baseURI = document->GetDocBaseURI();
if (!baseURI) {
NS_ERROR("No Base URL found in Form Submit!");
return NS_OK; // No base URL -> exit early, see Bug 30721
}
// If an action is not specified and we are inside
// a HTML document then reload the URL. This makes us
// compatible with 4.x browsers.
// If we are in some other type of document such as XML or
// XUL, do nothing. This prevents undesirable reloading of
// a document inside XUL.
nsresult rv;
nsCOMPtr<nsIHTMLDocument> htmlDoc;
htmlDoc = do_QueryInterface(document, &rv);
if (NS_FAILED(rv)) {
// Must be a XML, XUL or other non-HTML document type
// so do nothing.
return NS_OK;
}
// Necko's MakeAbsoluteURI doesn't reuse the baseURL's rel path if it is
// passed a zero length rel path.
nsCAutoString relPath;
baseURI->GetSpec(relPath);
if (!relPath.IsEmpty()) {
CopyUTF8toUTF16(relPath, href);
// If re-using the same URL, chop off old query string (bug 25330)
PRInt32 queryStart = href.FindChar('?');
if (kNotFound != queryStart) {
href.Truncate(queryStart);
}
} else {
NS_ERROR("Rel path couldn't be formed in form submit!");
return NS_ERROR_OUT_OF_MEMORY;
}
// Add the URI encoded form values to the URI
// Get the scheme of the URI.
nsCOMPtr<nsIURI> actionURL;
nsXPIDLCString scheme;
PRBool isJSURL = PR_FALSE;
const nsACString &docCharset = document->GetDocumentCharacterSet();
const nsPromiseFlatCString& flatDocCharset = PromiseFlatCString(docCharset);
if (NS_SUCCEEDED(result = NS_NewURI(getter_AddRefs(actionURL), href,
flatDocCharset.get(),
baseURI))) {
result = actionURL->SchemeIs("javascript", &isJSURL);
}
// Append the URI encoded variable/value pairs for GET's
if (!isJSURL) { // Not for JS URIs, see bug 26917
if (href.FindChar('?') == kNotFound) { // Add a ? if needed
href.Append(PRUnichar('?'));
} else { // Adding to existing query string
if (href.Last() != '&' && href.Last() != '?') { // Add a & if needed
href.Append(PRUnichar('&'));
}
}
href.Append(data);
}
nsCOMPtr<nsIURI> uri;
result = NS_NewURI(getter_AddRefs(uri), href,
flatDocCharset.get(), baseURI);
if (NS_FAILED(result)) return result;
// Now pretend we're triggering a link
nsContentUtils::TriggerLink(mContent, aPresContext, uri,
EmptyString(), PR_TRUE, PR_TRUE);
return result;
}
int main(int argc, char **argv)
{
//Register signal and signal handler
signal(SIGINT, signal_callback_handler);
//Init UDP with callbacks and pointer to run status
initUDP( &UDP_Command_Handler, &UDP_Control_Handler, &Running );
print("Eddie starting...\r\n");
initIdentity();
double EncoderPos[2] = {0};
initEncoders( 183, 46, 45, 44 );
print("Encoders activated.\r\n");
imuinit();
print("IMU Started.\r\n");
float kalmanAngle;
InitKalman();
#ifndef DISABLE_MOTORS
print( "Starting motor driver (and resetting wireless) please be patient..\r\n" );
if ( motor_driver_enable() < 1 )
{
print("Startup Failed; Error starting motor driver.\r\n");
motor_driver_disable();
return -1;
}
print("Motor Driver Started.\r\n");
#endif
print("Eddie is starting the UDP network thread..\r\n");
pthread_create( &udplistenerThread, NULL, &udplistener_Thread, NULL );
print( "Eddie is Starting PID controllers\r\n" );
/*Set default PID values and init pitchPID controllers*/
pidP_P_GAIN = PIDP_P_GAIN; pidP_I_GAIN = PIDP_I_GAIN; pidP_D_GAIN = PIDP_D_GAIN; pidP_I_LIMIT = PIDP_I_LIMIT; pidP_EMA_SAMPLES = PIDP_EMA_SAMPLES;
PIDinit( &pitchPID[0], &pidP_P_GAIN, &pidP_I_GAIN, &pidP_D_GAIN, &pidP_I_LIMIT, &pidP_EMA_SAMPLES );
PIDinit( &pitchPID[1], &pidP_P_GAIN, &pidP_I_GAIN, &pidP_D_GAIN, &pidP_I_LIMIT, &pidP_EMA_SAMPLES );
/*Set default values and init speedPID controllers*/
pidS_P_GAIN = PIDS_P_GAIN; pidS_I_GAIN = PIDS_I_GAIN; pidS_D_GAIN = PIDS_D_GAIN; pidS_I_LIMIT = PIDS_I_LIMIT; pidS_EMA_SAMPLES = PIDS_EMA_SAMPLES;
PIDinit( &speedPID[0], &pidS_P_GAIN, &pidS_I_GAIN, &pidS_D_GAIN, &pidS_I_LIMIT, &pidS_EMA_SAMPLES );
PIDinit( &speedPID[1], &pidS_P_GAIN, &pidS_I_GAIN, &pidS_D_GAIN, &pidS_I_LIMIT, &pidS_EMA_SAMPLES );
//Get estimate of starting angle and specify complementary filter and kalman filter start angles
getOrientation();
kalmanAngle = filteredPitch = i2cPitch;
setkalmanangle( filteredPitch );
filteredRoll = i2cRoll;
print( "Eddie startup complete. Hold me upright to begin\r\n" );
double gy_scale = 0.01;
last_PID_ms = last_gy_ms = current_milliseconds();
while(Running)
{
GetEncoders( EncoderPos );
if( fabs(GetEncoder()) > 2000 && !inRunAwayState )
{
print( "Help! I'm running and not moving.\r\n");
ResetEncoders();
inRunAwayState=1;
}
/*Read IMU and calculate rough angle estimates*/
getOrientation();
/*Calculate time since last IMU reading and determine gyro scale (dt)*/
gy_scale = ( current_milliseconds() - last_gy_ms ) / 1000.0f;
last_gy_ms = current_milliseconds();
/*Complementary filters to smooth rough pitch and roll estimates*/
filteredPitch = 0.995 * ( filteredPitch + ( gy * gy_scale ) ) + ( 0.005 * i2cPitch );
filteredRoll = 0.98 * ( filteredRoll + ( gx * gy_scale ) ) + ( 0.02 * i2cRoll );
/*Kalman filter for most accurate pitch estimates*/
kalmanAngle = -getkalmanangle(filteredPitch, gy, gy_scale /*dt*/);
/* Monitor angles to determine if Eddie has fallen too far... or if Eddie has been returned upright*/
if ( ( inRunAwayState || ( fabs( kalmanAngle ) > 50 || fabs( filteredRoll ) > 45 ) ) && !inFalloverState )
{
#ifndef DISABLE_MOTORS
motor_driver_standby(1);
#endif
inFalloverState = 1;
print( "Help! I've fallen over and I can't get up =)\r\n");
}
else if ( fabs( kalmanAngle ) < 10 && inFalloverState && fabs( filteredRoll ) < 20 )
{
if ( ++inSteadyState == 100 )
{
inRunAwayState = 0;
inSteadyState = 0;
#ifndef DISABLE_MOTORS
motor_driver_standby(0);
#endif
inFalloverState = 0;
print( "Thank you!\r\n" );
}
}
else
{
inSteadyState = 0;
}
if ( !inFalloverState )
{
/* Drive operations */
smoothedDriveTrim = ( 0.99 * smoothedDriveTrim ) + ( 0.01 * driveTrim );
if( smoothedDriveTrim != 0 )
{
EncoderAddPos(smoothedDriveTrim); //Alter encoder position to generate movement
}
/* Turn operations */
if( turnTrim != 0 )
{
EncoderAddPos2( turnTrim, -turnTrim ); //Alter encoder positions to turn
}
double timenow = current_milliseconds();
speedPIDoutput[0] = PIDUpdate( 0, EncoderPos[0], timenow - last_PID_ms, &speedPID[0] );//Wheel Speed PIDs
speedPIDoutput[1] = PIDUpdate( 0, EncoderPos[1], timenow - last_PID_ms, &speedPID[1] );//Wheel Speed PIDs
pitchPIDoutput[0] = PIDUpdate( speedPIDoutput[0], kalmanAngle, timenow - last_PID_ms, &pitchPID[0] );//Pitch Angle PIDs
pitchPIDoutput[1] = PIDUpdate( speedPIDoutput[1], kalmanAngle, timenow - last_PID_ms, &pitchPID[1] );//Pitch Angle PIDs
last_PID_ms = timenow;
//Limit PID output to +/-100 to match 100% motor throttle
if ( pitchPIDoutput[0] > 100.0 ) pitchPIDoutput[0] = 100.0;
if ( pitchPIDoutput[1] > 100.0 ) pitchPIDoutput[1] = 100.0;
if ( pitchPIDoutput[0] < -100.0 ) pitchPIDoutput[0] = -100.0;
if ( pitchPIDoutput[1] < -100.0 ) pitchPIDoutput[1] = -100.0;
}
else //We are inFalloverState
{
ResetEncoders();
pitchPID[0].accumulatedError = 0;
pitchPID[1].accumulatedError = 0;
speedPID[0].accumulatedError = 0;
speedPID[1].accumulatedError = 0;
driveTrim = 0;
turnTrim = 0;
}
#ifndef DISABLE_MOTORS
set_motor_speed_right( pitchPIDoutput[0] );
set_motor_speed_left( pitchPIDoutput[1] );
#endif
if ( (!inFalloverState || outputto == UDP) && StreamData )
{
print( "PIDout: %0.2f,%0.2f\tcompPitch: %6.2f kalPitch: %6.2f\tPe: %0.3f\tIe: %0.3f\tDe: %0.3f\tPe: %0.3f\tIe: %0.3f\tDe: %0.3f\r\n",
speedPIDoutput[0],
pitchPIDoutput[0],
filteredPitch,
kalmanAngle,
pitchPID[0].error,
pitchPID[0].accumulatedError,
pitchPID[0].differentialError,
speedPID[0].error,
speedPID[0].accumulatedError,
speedPID[0].differentialError
);
}
} //--while(Running)
print( "Eddie is cleaning up...\r\n" );
CloseEncoder();
pthread_join(udplistenerThread, NULL);
print( "UDP Thread Joined..\r\n" );
#ifndef DISABLE_MOTORS
motor_driver_disable();
print( "Motor Driver Disabled..\r\n" );
#endif
print( "Eddie cleanup complete. Good Bye!\r\n" );
return 0;
}
