Пример #1
0
void gGauge::draw(float value, String sValue) {
	
	float z = ( 7.0+6.0*(value-_valueMin)/(_valueMax-_valueMin) )*PI/4.0;
	if ( z<PI* 7.0/4.0 )   z = PI* 7.0/4.0;
	if ( z>PI*13.0/4.0 )   z = PI*13.0/4.0;
	float k = min(_radius*0.8, _radius-8-3);
	float l = 0;
	uint16_t vX  = _x0-k*cos(z);
	uint16_t vY  = _y0-k*sin(z);
	uint16_t _vX = _x0-k*cos(_oldZ);
	uint16_t _vY = _y0-k*sin(_oldZ);
	
	_oldZ = z;
	if ( (vX==_vX) && (vY==_vY) ) return;
	
	// value
	_pscreen->line(_x0-1, _y0-1, vX-1, vY-1, _valueColour);
	_pscreen->line(_x0-1, _y0,   vX-1, vY,   _valueColour);
	_pscreen->line(_x0-1, _y0+1, vX-1, vY+1, _valueColour);
	//    _pscreen->line(_x0,   _y0-1, vX,   vY-1, _valueColour);
	//    _pscreen->line(_x0,   _y0,   vX,   vY,   _valueColour);
	//    _pscreen->line(_x0,   _y0+1, vX,   vY+1, _valueColour);
	_pscreen->line(_x0+1, _y0-1, vX+1, vY-1, _valueColour);
	_pscreen->line(_x0+1, _y0,   vX+1, vY,   _valueColour);
	_pscreen->line(_x0+1, _y0+1, vX+1, vY+1, _valueColour);
	
	_pscreen->line(_x0-1, _y0-1, _vX-1, _vY-1, _backColour);
	_pscreen->line(_x0-1, _y0,   _vX-1, _vY,   _backColour);
	_pscreen->line(_x0-1, _y0+1, _vX-1, _vY+1, _backColour);
	//    _pscreen->line(_x0,   _y0-1, _vX,   _vY-1, _backColour);
	//    _pscreen->line(_x0,   _y0,   _vX,   _vY,   _backColour);
	//    _pscreen->line(_x0,   _y0+1, _vX,   _vY+1, _backColour);
	_pscreen->line(_x0+1, _y0-1, _vX+1, _vY-1, _backColour);
	_pscreen->line(_x0+1, _y0,   _vX+1, _vY,   _backColour);
	_pscreen->line(_x0+1, _y0+1, _vX+1, _vY+1, _backColour);
	
	_pscreen->line(_x0-1, _y0-1, vX-1, vY-1, _valueColour);
	_pscreen->line(_x0-1, _y0,   vX-1, vY,   _valueColour);
	_pscreen->line(_x0-1, _y0+1, vX-1, vY+1, _valueColour);
	//    _pscreen->line(_x0,   _y0-1, vX,   vY-1, _valueColour);
	//    _pscreen->line(_x0,   _y0,   vX,   vY,   _valueColour);
	//    _pscreen->line(_x0,   _y0+1, vX,   vY+1, _valueColour);
	_pscreen->line(_x0+1, _y0-1, vX+1, vY-1, _valueColour);
	_pscreen->line(_x0+1, _y0,   vX+1, vY,   _valueColour);
	_pscreen->line(_x0+1, _y0+1, vX+1, vY+1, _valueColour);
	
	// min and max memory
	if (_memory>0) {
		_pscreen->setPenSolid(true);
		// first time
		if (_n==0) {
			_max = z;
			_min = z;
			_n++;
		}
		
		boolean fMax = false;
		boolean fMin = false;
		
		// max - coordinates in normal scale
		if (z>=_max) {
			_pscreen->circle(_x0-(_radius-8)*cos(_max), _y0-(_radius-8)*sin(_max), 2, _backColour);
			_max = z;
			_amnesiaMax = _memory;
		} 
		else if (_amnesiaMax>1) {
			fMax = true;
			_amnesiaMax--;
		} 
		else {
			_pscreen->circle(_x0-(_radius-8)*cos(_max), _y0-(_radius-8)*sin(_max), 2, _backColour);
			_max = z;
		} // max
		
		// min - coordinates in normal scale
		if (z<=_min) {
			_pscreen->circle(_x0-(_radius-8)*cos(_min), _y0-(_radius-8)*sin(_min), 2, _backColour);
			_min = z;
			_amnesiaMin = _memory;
		} 
		else if (_amnesiaMin>0) {
			fMin = true;
			_amnesiaMin--;
		} 
		else {
			_pscreen->circle(_x0-(_radius-8)*cos(_min), _y0-(_radius-8)*sin(_min), 2, _backColour);
			_min = z;
		} // min
		
		if (fMin)       _pscreen->circle(_x0-(_radius-8)*cos(_min), _y0-(_radius-8)*sin(_min), 2, _minColour);
		if (fMax)       _pscreen->circle(_x0-(_radius-8)*cos(_max), _y0-(_radius-8)*sin(_max), 2, _maxColour);
	} // min and max memory
	
	// min and max values
	_pscreen->setFont(0);
	_pscreen->setFontSolid(true);
	_pscreen->gText(_x0-_radius, _y0+_radius-_pscreen->fontY(), ftoa(_valueMin, 1, 0), _frontColour);
	_pscreen->gText(_x0+_radius-_pscreen->fontX()*ftoa(_valueMax, 1, 0).length(), _y0+_radius-_pscreen->fontY(), ftoa(_valueMax, 1, 0), _frontColour);
	
	// string display
	if (sValue!="") {
		_pscreen->setPenSolid(false);
		_pscreen->setFontSolid(true);
		
		uint8_t i=4;
		do {
			i--;
			_pscreen->setFont(i);
		} 
		while ( (sValue.length()*_pscreen->fontX() > (2*_radius*0.80)) && (i>0) );
		
		String _s = sValue.substring(0, min(sValue.length(), (2*_radius*0.80) / _pscreen->fontX()));
		k = sValue.length()*_pscreen->fontX()/2.0;
		l = _radius*0.90-_pscreen->fontY();
		
		_pscreen->dRectangle(_x0-k-1, _y0+l-2, 2*k+3, _pscreen->fontY()+3, _gridColour);
		_pscreen->gText(_x0-k, _y0+l, _s, _frontColour);
	} // string display
	
	_pscreen->setPenSolid(true);
	_pscreen->circle(_x0, _y0, 3, _valueColour); 
	
	//// debug
	//  Serial.print(value, 2);
	//  Serial.print("\t>");
	//  Serial.print(z, 2);
	//  Serial.print("\t min:");
	//  Serial.print(_min, 2);
	//  Serial.print("\t(");
	//  Serial.print(_amnesiaMin, DEC);
	//  Serial.print(")\t max:");
	//  Serial.print(_max, 2);
	//  Serial.print("\t(");
	//  Serial.print(_amnesiaMax, DEC);
	//  Serial.print(")\n");
}
Пример #2
0
HTREEITEM DebugTree::insertVar( void *var,Decl *d,const string &name,HTREEITEM it,HTREEITEM parent ){

	string s=name;

	ConstType *ct=d->type->constType();
	StructType *st=d->type->structType();
	VectorType *vt=d->type->vectorType();

	if( ct ){
		Type *t=ct->valueType;
		s+=typeTag(t);
		if( t->intType() ){
			s+="="+itoa( ct->intValue );
		}else if( t->floatType() ){
			s+="="+ftoa( ct->floatValue );
		}else if( t->stringType() ){
			s+="=\""+ct->stringValue+'\"';
		}
	}else if( var ){
		Type *t=d->type;
		s+=typeTag( t );
		if( t->intType() ){
			s+="="+itoa( *(int*)var );
		}else if( t->floatType() ){
			s+="="+ftoa( *(float*)var );
		}else if( t->stringType() ){
			BBStr *str=*(BBStr**)var;
			if( str ) s+="=\""+*str+'\"';
			else s+="=\"\"";
		}else if( st ){
			var=*(void**)var;
			if( var ) var=*(void**)var;
			if( !var ) s+=" (Null)";
		}
	}

	if( it ){
		if( GetItemText( it )!=s.c_str() ){
			SetItemText( it,s.c_str() );
		}
	}else{
		it=InsertItem( s.c_str(),parent );
	}

	++st_nest;
	if( st ){
		if( var ){
			if( st_nest<4 ){
				HTREEITEM st_it=GetChildItem( it );
				for( int k=0;k<st->fields->size();++k ){
					Decl *st_d=st->fields->decls[k];
					void *st_var=(char*)var+st_d->offset;

					char name[256];
					st_d->getName( name );

					st_it=insertVar( st_var,st_d,name,st_it,it );
				}
			}
		}else{
			while( HTREEITEM t=GetChildItem( it ) ){
				DeleteItem( t );
			}
		}
	}
	--st_nest;

	return it ? GetNextSiblingItem( it ) : 0;
}
Пример #3
0
void reload(GLOBAL *g, struct payments_module *p)
{
	QueryHandle *res, *result;
	char *insert, *description, *invoiceid, *value, *taxid, *currtime;
	char *d_period, *w_period, *m_period, *q_period, *y_period, *h_period;
	int i, imonth, imday, today, n=2, k=2, m=2, o=2, pl=0;
	int docid=0, last_cid=0, last_paytype=0, last_plan=0, exec=0, suspended=0, itemid=0;

	time_t t;
	struct tm tt;
	char monthday[3], month[3], year[5], quarterday[4], weekday[2], yearday[4], halfday[4];
	char monthname[20], nextmon[8];

	char *nets = strdup(" AND EXISTS (SELECT 1 FROM nodes, networks n "
				"WHERE ownerid = a.customerid "
				    "AND (%nets) "
	                "AND ((ipaddr > address AND ipaddr < ("BROADCAST")) "
				        "OR (ipaddr_pub > address AND ipaddr_pub < ("BROADCAST"))) )");

	char *netnames = strdup(p->networks);
	char *netname = strdup(netnames);
	char *netsql = strdup("");

	char *enets = strdup(" AND NOT EXISTS (SELECT 1 FROM nodes, networks n "
				"WHERE ownerid = a.customerid "
				    "AND (%enets) "
	                "AND ((ipaddr > address AND ipaddr < ("BROADCAST")) "
				        "OR (ipaddr_pub > address AND ipaddr_pub < ("BROADCAST"))) )");

	char *enetnames = strdup(p->excluded_networks);
	char *enetname = strdup(enetnames);
	char *enetsql = strdup("");

	char *groups = strdup(" AND EXISTS (SELECT 1 FROM customergroups g, customerassignments ca "
				"WHERE ca.customerid = a.customerid "
				    "AND g.id = ca.customergroupid "
				    "AND (%groups)) ");

	char *groupnames = strdup(p->customergroups);
	char *groupname = strdup(groupnames);
	char *groupsql = strdup("");

	char *egroups = strdup(" AND NOT EXISTS (SELECT 1 FROM customergroups g, customerassignments ca "
				"WHERE ca.customerid = a.customerid "
				    "AND g.id = ca.customergroupid "
				    "AND (%egroups)) ");

	char *egroupnames = strdup(p->excluded_customergroups);
	char *egroupname = strdup(egroupnames);
	char *egroupsql = strdup("");

	while( n>1 )
	{
    		n = sscanf(netnames, "%s %[._a-zA-Z0-9- ]", netname, netnames);

		if( strlen(netname) )
		{
			netsql = realloc(netsql, sizeof(char *) * (strlen(netsql) + strlen(netname) + 30));
			if(strlen(netsql))
				strcat(netsql, " OR UPPER(n.name) = UPPER('");
			else
				strcat(netsql, "UPPER(n.name) = UPPER('");

			strcat(netsql, netname);
			strcat(netsql, "')");
		}
	}
	free(netname); free(netnames);

	if(strlen(netsql))
		g->str_replace(&nets, "%nets", netsql);

	while( o>1 )
	{
    		o = sscanf(enetnames, "%s %[._a-zA-Z0-9- ]", enetname, enetnames);

		if( strlen(enetname) )
		{
			enetsql = realloc(enetsql, sizeof(char *) * (strlen(enetsql) + strlen(enetname) + 30));
			if(strlen(enetsql))
				strcat(enetsql, " OR UPPER(n.name) = UPPER('");
			else
				strcat(enetsql, "UPPER(n.name) = UPPER('");

			strcat(enetsql, enetname);
			strcat(enetsql, "')");
		}
	}
	free(enetname); free(enetnames);

	if(strlen(enetsql))
		g->str_replace(&enets, "%enets", enetsql);

	while( k>1 )
	{
		k = sscanf(groupnames, "%s %[._a-zA-Z0-9- ]", groupname, groupnames);

		if( strlen(groupname) )
		{
			groupsql = realloc(groupsql, sizeof(char *) * (strlen(groupsql) + strlen(groupname) + 30));
			if(strlen(groupsql))
				strcat(groupsql, " OR UPPER(g.name) = UPPER('");
			else
				strcat(groupsql, "UPPER(g.name) = UPPER('");

			strcat(groupsql, groupname);
			strcat(groupsql, "')");
		}
	}
	free(groupname); free(groupnames);

	if(strlen(groupsql))
		g->str_replace(&groups, "%groups", groupsql);

	while( m>1 )
	{
		m = sscanf(egroupnames, "%s %[._a-zA-Z0-9- ]", egroupname, egroupnames);

		if( strlen(egroupname) )
		{
			egroupsql = realloc(egroupsql, sizeof(char *) * (strlen(egroupsql) + strlen(egroupname) + 30));
			if(strlen(egroupsql))
				strcat(egroupsql, " OR UPPER(g.name) = UPPER('");
			else
				strcat(egroupsql, "UPPER(g.name) = UPPER('");

			strcat(egroupsql, egroupname);
			strcat(egroupsql, "')");
		}
	}
	free(egroupname); free(egroupnames);

	if(strlen(egroupsql))
		g->str_replace(&egroups, "%egroups", egroupsql);

	// get current date
	t = time(NULL);
	memcpy(&tt, localtime(&t), sizeof(tt));
	strftime(monthday, 	sizeof(monthday), 	"%d", &tt);
	strftime(weekday, 	sizeof(weekday), 	"%u", &tt);
	strftime(monthname, 	sizeof(monthname), 	"%B", &tt);
	strftime(month, 	sizeof(month), 		"%m", &tt);
	strftime(year, 		sizeof(year), 		"%Y", &tt);

	currtime = strdup(itoa(t));
	imday = tt.tm_mday;
	imonth = tt.tm_mon+1;

	// leap year fix
	if(is_leap_year(tt.tm_year+1900) && tt.tm_yday+1 > 31+28)
		strncpy(yearday, itoa(tt.tm_yday), sizeof(yearday));
	else
		strncpy(yearday, itoa(tt.tm_yday+1), sizeof(yearday));

	// halfyear
	if(imonth > 6)
		strncpy(halfday, itoa(imday + (imonth - 7) * 100), sizeof(halfday));
	else
		strncpy(halfday, itoa(imday + (imonth - 1) * 100), sizeof(halfday));

	switch(imonth) {
		case 1:
		case 4:
		case 7:
		case 10:
			sprintf(quarterday, "%d", imday);
			break;
		case 2:
		case 5:
		case 8:
		case 11:
			sprintf(quarterday, "%d", imday+100);
			break;
		default:
			sprintf(quarterday, "%d", imday+200);
			break;
	}

	// next month in YYYY/MM format
	if (imonth == 12)
		snprintf(nextmon, sizeof(nextmon), "%04d/%02d", tt.tm_year+1901, 1);
	else
		snprintf(nextmon, sizeof(nextmon), "%04d/%02d", tt.tm_year+1900, imonth+1);

	// time periods
	y_period = get_period(&tt, YEARLY, p->up_payments);
	h_period = get_period(&tt, HALFYEARLY, p->up_payments);
	q_period = get_period(&tt, QUARTERLY, p->up_payments);
	m_period = get_period(&tt, MONTHLY, p->up_payments);
	w_period = get_period(&tt, WEEKLY, p->up_payments);
	d_period = get_period(&tt, DAILY, p->up_payments);

	// today (for disposable liabilities)
	tt.tm_sec = 0;
	tt.tm_min = 0;
	tt.tm_hour = 0;
	today = mktime(&tt);

	/****** main payments *******/
	if( (res = g->db_pquery(g->conn, "SELECT * FROM payments "
		"WHERE value <> 0 AND (period="_DAILY_" OR (period="_WEEKLY_" AND at=?) "
			"OR (period="_MONTHLY_" AND at=?) "
			"OR (period="_QUARTERLY_" AND at=?) "
			"OR (period="_HALFYEARLY_" AND at=?) "
			"OR (period="_YEARLY_" AND at=?))",
			weekday, monthday, quarterday, halfday, yearday))!= NULL )
	{
		for(i=0; i<g->db_nrows(res); i++) 
		{
			exec = (g->db_pexec(g->conn, "INSERT INTO cash (time, type, value, customerid, comment, docid) "
				"VALUES (?, 1, ? * -1, 0, '? / ?', 0)",
					currtime,
					g->db_get_data(res,i,"value"),
					g->db_get_data(res,i,"name"),
					g->db_get_data(res,i,"creditor")
				) ? 1 : exec);
		}
		g->db_free(&res);
#ifdef DEBUG1
		syslog(LOG_INFO, "DEBUG: [%s/payments] Main payments reloaded", p->base.instance);
#endif
	}
	else 
		syslog(LOG_ERR, "[%s/payments] Unable to read 'payments' table", p->base.instance);

	/****** customer payments *******/
	// let's create main query
	char *query = strdup("SELECT a.tariffid, a.customerid, a.period, t.period AS t_period, "
	    "a.at, a.suspended, a.invoice, a.id AS assignmentid, a.settlement, a.datefrom, a.pdiscount, a.vdiscount, "
		"c.paytype, a.paytype AS a_paytype, a.numberplanid, d.inv_paytype AS d_paytype, "
		"UPPER(c.lastname) AS lastname, c.name AS custname, c.address, c.zip, c.city, c.ten, c.ssn, "
		"c.countryid, c.divisionid, c.paytime, "
		"(CASE a.liabilityid WHEN 0 THEN t.type ELSE -1 END) AS tarifftype, "
		"(CASE a.liabilityid WHEN 0 THEN t.name ELSE li.name END) AS name, "
		"(CASE a.liabilityid WHEN 0 THEN t.taxid ELSE li.taxid END) AS taxid, "
		"(CASE a.liabilityid WHEN 0 THEN t.prodid ELSE li.prodid END) AS prodid, "
		"(CASE a.liabilityid WHEN 0 THEN "
		    "ROUND((t.value - t.value * a.pdiscount / 100) - a.vdiscount, 2) "
			"ELSE ROUND((li.value - li.value * a.pdiscount / 100) - a.vdiscount, 2) "
			"END) AS value "
		"FROM assignments a "
		"JOIN customers c ON (a.customerid = c.id) "
		"LEFT JOIN tariffs t ON (a.tariffid = t.id) "
		"LEFT JOIN liabilities li ON (a.liabilityid = li.id) "
		"LEFT JOIN divisions d ON (d.id = c.divisionid) "
		"WHERE c.status = 3 AND c.deleted = 0 "
		    "AND ("
		        "(a.period="_DISPOSABLE_" AND at=?) "
		        "OR (("
		            "(a.period="_DAILY_") "
			        "OR (a.period="_WEEKLY_" AND at=?) "
			        "OR (a.period="_MONTHLY_" AND at=?) "
			        "OR (a.period="_QUARTERLY_" AND at=?) "
			        "OR (a.period="_HALFYEARLY_" AND at=?) "
			        "OR (a.period="_YEARLY_" AND at=?) "
			        ") "
			        "AND (a.datefrom <= ? OR a.datefrom = 0) "
			        "AND (a.dateto >= ? OR a.dateto = 0)"
			    ")"
			")"
		    "%nets"
		    "%enets"
		    "%groups"
		    "%egroups"
		" ORDER BY a.customerid, a.invoice DESC, a.paytype, a.numberplanid, value DESC");

	g->str_replace(&query, "%nets", strlen(netsql) ? nets : "");
	g->str_replace(&query, "%enets", strlen(enetsql) ? enets : "");
	g->str_replace(&query, "%groups", strlen(groupsql) ? groups : "");
	g->str_replace(&query, "%egroups", strlen(egroupsql) ? egroups : "");

	if( (res = g->db_pquery(g->conn, query,
		itoa(today), weekday, monthday, quarterday, halfday, yearday,  currtime, currtime)) != NULL)
	{
		struct plan *plans = (struct plan *) malloc(sizeof(struct plan));
		int invoice_number = 0;

		if( g->db_nrows(res) )
		{
			if (!p->numberplanid)
			{
				// get numbering plans for all divisions
				result = g->db_query(g->conn, "SELECT n.id, n.period, COALESCE(a.divisionid, 0) AS divid, isdefault "
					"FROM numberplans n "
					"LEFT JOIN numberplanassignments a ON (a.planid = n.id) "
					"WHERE doctype = 1");

				for(i=0; i<g->db_nrows(result); i++) 
				{
					plans = (struct plan *) realloc(plans, (sizeof(struct plan) * (pl+1)));
					plans[pl].plan = atoi(g->db_get_data(result, i, "id"));
					plans[pl].period = atoi(g->db_get_data(result, i, "period"));
					plans[pl].division = atoi(g->db_get_data(result, i, "divid"));
					plans[pl].isdefault = atoi(g->db_get_data(result, i, "isdefault"));
					plans[pl].number = 0;
					pl++;
				}
				g->db_free(&result);
			}
		}
#ifdef DEBUG1
		else
			syslog(LOG_INFO, "DEBUG: [%s/payments] Customer assignments not found", p->base.instance);
#endif
		// payments accounting and invoices writing
		for(i=0; i<g->db_nrows(res); i++)
		{
			char *cid_c         = g->db_get_data(res,i,"customerid");
			char *pdiscount     = g->db_get_data(res,i,"pdiscount");
			char *vdiscount     = g->db_get_data(res,i,"vdiscount");
			int cid             = atoi(cid_c);
			int s_state         = atoi(g->db_get_data(res,i,"suspended"));
			int period          = atoi(g->db_get_data(res,i,"period"));
			int settlement      = atoi(g->db_get_data(res,i,"settlement"));
			int datefrom        = atoi(g->db_get_data(res,i,"datefrom"));
			int t_period        = atoi(g->db_get_data(res,i,"t_period"));
			double val          = atof(g->db_get_data(res,i,"value"));
			int tarifftype_int  = atoi(g->db_get_data(res, i, "tarifftype"));
			char *tarifftype    = (tarifftype_int == -1 ? "" : get_tarifftype_str(p, tarifftype_int));

			if( !val ) continue;

			// assignments suspending check
			if( last_cid != cid )
			{
				result = g->db_pquery(g->conn, "SELECT 1 FROM assignments "
					"WHERE customerid = ? AND tariffid = 0 AND liabilityid = 0 "
					    "AND (datefrom <= ? OR datefrom = 0) AND (dateto >= ? OR dateto = 0)",
					cid_c, currtime, currtime);

				suspended = g->db_nrows(result) ? 1 : 0;
				g->db_free(&result);
			}

			if( suspended || s_state )
				val = val * p->suspension_percentage / 100;

			if( !val ) continue;

			// calculate assignment value according to tariff's period
			if (t_period && period != DISPOSABLE && t_period != period) {
				if (t_period == YEARLY)
					val = val / 12.0;
				else if (t_period == HALFYEARLY)
					val = val / 6.0;
				else if (t_period == QUARTERLY)
					val = val / 3.0;

				if (period == YEARLY)
					val = val * 12.0;
				else if (period == HALFYEARLY)
					val = val * 6.0;
				else if (period == QUARTERLY)
					val = val * 3.0;
				else if (period == WEEKLY)
					val = val / 4.0;
				else if (period == DAILY)
					val = val / 30.0;
			}

			value = ftoa(val);
			taxid = g->db_get_data(res,i,"taxid");

			// prepare insert to 'cash' table
			insert = strdup("INSERT INTO cash (time, value, taxid, customerid, comment, docid, itemid) "
				"VALUES (?, %value * -1, %taxid, %customerid, '?', %docid, %itemid)");
			g->str_replace(&insert, "%customerid", cid_c);
			g->str_replace(&insert, "%value", value);
			g->str_replace(&insert, "%taxid", taxid);

			if( period == DISPOSABLE )
				description = strdup(g->db_get_data(res,i,"name"));
			else
				description = strdup(p->comment);

			switch( period )
			{
				case DAILY: g->str_replace(&description, "%period", d_period); break;
				case WEEKLY: g->str_replace(&description, "%period", w_period); break;
				case MONTHLY: g->str_replace(&description, "%period", m_period); break;
				case QUARTERLY: g->str_replace(&description, "%period", q_period); break;
				case HALFYEARLY: g->str_replace(&description, "%period", h_period); break;
				case YEARLY: g->str_replace(&description, "%period", y_period); break;
			}
			g->str_replace(&description, "%type", tarifftype);
			g->str_replace(&description, "%tariff", g->db_get_data(res,i,"name"));
			g->str_replace(&description, "%next_mon", nextmon);
			g->str_replace(&description, "%month", monthname);
			g->str_replace(&description, "%currentm", month);
			g->str_replace(&description, "%year", year);
			g->str_replace(&description, "%currenty", year);

			if( atoi(g->db_get_data(res,i,"invoice")) )
			{
				char *divisionid = g->db_get_data(res,i,"divisionid");
				int a_numberplan = atoi(g->db_get_data(res,i,"numberplanid"));
				int a_paytype = atoi(g->db_get_data(res,i,"a_paytype")); // assignment
				int c_paytype = atoi(g->db_get_data(res,i,"paytype"));   // customer
				int d_paytype = atoi(g->db_get_data(res,i,"d_paytype")); // division
				int paytype, numberplan, divid = atoi(divisionid);

				// paytype (by priority)
				if (a_paytype)
					paytype = a_paytype;
				else if (c_paytype)
					paytype = c_paytype;
				else if (d_paytype)
					paytype = d_paytype;
				else
					paytype = p->paytype;

				// select numberplan
				if (a_numberplan) {
					for (n=0; n<pl; n++)
						if (plans[n].plan == a_numberplan)
							break;
				} else {
					for (n=0; n<pl; n++)
						if (plans[n].division == divid && plans[n].isdefault)
							break;
				}

				numberplan = (n < pl) ? n : -1;

				if ( last_cid != cid || last_paytype != paytype || last_plan != numberplan)
				{
					char *countryid = g->db_get_data(res,i,"countryid");
					char *numberplanid, *paytime, *paytype_str = strdup(itoa(paytype));
					int period, number = 0;

					last_paytype = paytype;
					last_plan = numberplan;

					// numberplan found
					if (numberplan >= 0)
					{
						numberplanid = strdup(itoa(plans[numberplan].plan));
						period = plans[numberplan].period;
						number = plans[numberplan].number;
					}
					else // not found, use default/shared plan
					{
						numberplanid = strdup(itoa(p->numberplanid));
						period = p->num_period;
						number = invoice_number;
					}

					if(!number)
					{
						char *start = get_num_period_start(&tt, period);
						char *end = get_num_period_end(&tt, period);

						// set invoice number
						result = g->db_pquery(g->conn, "SELECT MAX(number) AS number FROM documents "
							"WHERE cdate >= ? AND cdate < ? AND numberplanid = ? AND type = 1", 
							start, end, numberplanid); 

						if( g->db_nrows(result) )
							number = atoi(g->db_get_data(result,0,"number"));
						g->db_free(&result);
					}

					++number;

					if(n < pl)
					{
						// update number
						for(m=0; m<pl; m++)
							if(plans[m].plan == plans[n].plan)
								plans[m].number = number;
					}
					else
						invoice_number = number;

					// deadline
					if(atoi(g->db_get_data(res,i,"paytime")) < 0)
						paytime = p->deadline;
					else
						paytime = g->db_get_data(res,i,"paytime");

					// prepare insert to 'invoices' table
					g->db_pexec(g->conn, "INSERT INTO documents (number, numberplanid, type, countryid, divisionid, "
						"customerid, name, address, zip, city, ten, ssn, cdate, sdate, paytime, paytype) "
						"VALUES (?, ?, 1, ?, ?, ?, '? ?', '?', '?', '?', '?', '?', ?, ?, ?, ?)",
						itoa(number),
						numberplanid,
						countryid,
						divisionid,
						cid_c,
						g->db_get_data(res,i,"lastname"),
						g->db_get_data(res,i,"custname"),
						g->db_get_data(res,i,"address"),
						g->db_get_data(res,i,"zip"),
						g->db_get_data(res,i,"city"),
						g->db_get_data(res,i,"ten"),
						g->db_get_data(res,i,"ssn"),
						currtime,
						currtime,
						paytime,
						paytype_str
					);

					docid = g->db_last_insert_id(g->conn, "documents");
					itemid = 0;

					free(numberplanid);
					free(paytype_str);
				}

				invoiceid = strdup(itoa(docid));

				result = g->db_pquery(g->conn, "SELECT itemid FROM invoicecontents "
				    "WHERE tariffid = ? AND docid = ? AND description = '?' AND value = ? AND pdiscount = ? AND vdiscount = ?",
				    g->db_get_data(res,i,"tariffid"), invoiceid, description, value, pdiscount, vdiscount);

				if( g->db_nrows(result) ) 
				{
					g->db_pexec(g->conn, "UPDATE invoicecontents SET count = count+1 WHERE docid = ? AND itemid = ?",
						invoiceid,
						g->db_get_data(result,0,"itemid")
						);

					exec = g->db_pexec(g->conn, "UPDATE cash SET value = value + (? * -1) "
					    "WHERE docid = ? AND itemid = ?",
					    value, invoiceid, g->db_get_data(result,0,"itemid"));
				}
				else if (docid)
				{
					itemid++;

					g->db_pexec(g->conn,"INSERT INTO invoicecontents (docid, itemid, value, "
					        "taxid, prodid, content, count, description, tariffid, pdiscount, vdiscount) "
					        "VALUES (?, ?, ?, ?, '?', 'szt.', 1, '?', ?, ?, ?)",
						invoiceid,
						itoa(itemid),
						value,
						taxid,
						g->db_get_data(res,i,"prodid"),
						description,
						g->db_get_data(res,i,"tariffid"),
						pdiscount,
						vdiscount
						);

					g->str_replace(&insert, "%docid", invoiceid);
					g->str_replace(&insert, "%itemid", itoa(itemid));
					exec = g->db_pexec(g->conn, insert, currtime, description);
				}

				g->db_free(&result);
				free(invoiceid);
			}
			else
			{
				g->str_replace(&insert, "%docid", "0");
				g->str_replace(&insert, "%itemid", "0");
				exec = g->db_pexec(g->conn, insert, currtime, description) ? 1 : exec;
			}

			free(insert);
			free(description);

			// settlements accounting has sense only for up payments
			if( settlement && datefrom && p->up_payments)
			{
				int alldays = 1;
				int diffdays = (int) ((today - datefrom)/86400);

				switch( period )
				{
					// there are no disposable or daily liabilities with settlement
					case WEEKLY: 	alldays = 7; break;
					case MONTHLY: 	alldays = 30; break;
					case QUARTERLY: alldays = 90; break;
					case HALFYEARLY: alldays = 182; break;
					case YEARLY: 	alldays = 365; break;
				}

				value = ftoa(diffdays * val/alldays);

				description = strdup(p->s_comment);
				g->str_replace(&description, "%period", get_diff_period(datefrom, today-86400));
				g->str_replace(&description, "%type", tarifftype);
				g->str_replace(&description, "%tariff", g->db_get_data(res,i,"name"));
				g->str_replace(&description, "%month", monthname);
				g->str_replace(&description, "%year", year);

				// prepare insert to 'cash' table
				insert = strdup("INSERT INTO cash (time, value, taxid, customerid, comment, docid, itemid) "
					"VALUES (?, %value * -1, %taxid, %customerid, '?', %docid, %itemid)");

				g->str_replace(&insert, "%customerid", cid_c);
				g->str_replace(&insert, "%value", value);
				g->str_replace(&insert, "%taxid", taxid);

				// we're using transaction to not disable settlement flag
				// when something will goes wrong
				g->db_begin(g->conn);

				if( atoi(g->db_get_data(res,i,"invoice")) )
				{
					// oh, now we've got invoice id
					invoiceid = strdup(itoa(docid));

					result = g->db_pquery(g->conn, "SELECT itemid FROM invoicecontents "
					    "WHERE tariffid = ? AND docid = ? AND description = '?' AND value = ?",
					    g->db_get_data(res,i,"tariffid"), invoiceid, description, value);

					if( g->db_nrows(result) )
					{
						g->db_pexec(g->conn, "UPDATE invoicecontents SET count = count+1 WHERE docid = ? AND itemid = ?",
							invoiceid,
							g->db_get_data(result,0,"itemid")
						);

						exec = g->db_pexec(g->conn, "UPDATE cash SET value = value + (? * -1) "
						    "WHERE docid = ? AND itemid = ?",
						    value, invoiceid, g->db_get_data(result,0,"itemid"));
					}
					else if (docid)
					{
						itemid++;

						g->db_pexec(g->conn,"INSERT INTO invoicecontents (docid, itemid, value, taxid, prodid, "
						        "content, count, description, tariffid, pdiscount, vdiscount) "
						        "VALUES (?, ?, ?, ?, '?', 'szt.', 1, '?', ?, ?, ?)",
							invoiceid,
							itoa(itemid),
							value,
							taxid,
							g->db_get_data(res,i,"prodid"),
							description,
							g->db_get_data(res,i,"tariffid"),
							pdiscount,
							vdiscount
							);

						g->str_replace(&insert, "%docid", invoiceid);
						g->str_replace(&insert, "%itemid", itoa(itemid));
						g->str_replace(&insert, "%value", value);
						exec = g->db_pexec(g->conn, insert, currtime, description);
					}

					g->db_free(&result);
					free(invoiceid);
				}
				else
				{
					g->str_replace(&insert, "%docid", "0");
					g->str_replace(&insert, "%itemid", "0");
					g->str_replace(&insert, "%value", value);
					g->db_pexec(g->conn, insert, currtime, description) ? 1 : exec;
				}

				// uncheck settlement flag
				g->db_pexec(g->conn, "UPDATE assignments SET settlement = 0 WHERE id = ?", g->db_get_data(res,i,"assignmentid"));

				g->db_commit(g->conn);

				free(insert);
				free(description);
			}

			last_cid = cid;
		}

		g->db_free(&res);
		free(y_period);
		free(h_period);
		free(q_period);
		free(m_period);
		free(w_period);
		free(d_period);

		free(plans);
#ifdef DEBUG1
		syslog(LOG_INFO, "DEBUG: [%s/payments] Customer payments reloaded", p->base.instance);
#endif
	}
	else 
		syslog(LOG_ERR, "[%s/payments] Unable to read tariff assignments", p->base.instance);

	free(query);

	/****** invoices checking *******/
	if (p->check_invoices)
	{
		char *query = strdup(
			"UPDATE documents SET closed = 1 "
			"WHERE customerid IN ( "
				"SELECT a.customerid "
				"FROM cash a "
				"WHERE a.time <= %NOW% "
				"   %nets%enets%groups%egroups "
				"GROUP BY a.customerid "
				"HAVING SUM(a.value) >= 0) "
			"AND type IN (1, 3, 5) "
			"AND cdate <= %NOW% "
			"AND closed = 0");

		g->str_replace(&query, "%nets", strlen(netsql) ? nets : "");
		g->str_replace(&query, "%enets", strlen(enetsql) ? enets : "");
		g->str_replace(&query, "%groups", strlen(groupsql) ? groups : "");
		g->str_replace(&query, "%egroups", strlen(egroupsql) ? egroups : "");

		g->db_pexec(g->conn, query);

		free(query);
	}

	// remove old assignments
	if(p->expiry_days<0) p->expiry_days *= -1; // number of expiry days can't be negative

    char *exp_days = strdup(itoa(p->expiry_days));

	g->db_pexec(g->conn, "DELETE FROM liabilities "
	    "WHERE id IN ("
		    "SELECT liabilityid FROM assignments "
	        "WHERE dateto < ? - 86400 * ? AND dateto != 0 AND at < ? - 86400 * ? "
		        "AND liabilityid != 0)",
	    currtime, exp_days, itoa(today), exp_days);
	g->db_pexec(g->conn, "DELETE FROM assignments "
	    "WHERE dateto < ? - 86400 * ? AND dateto != 0 AND at < ? - 86400 * ?",
	    currtime, exp_days, itoa(today), exp_days);

	// clean up
	free(exp_days);
	free(currtime);
	free(nets); free(enets);
	free(groups); free(egroups);
	free(netsql); free(enetsql);
	free(groupsql);	free(egroupsql);
	free(p->comment);
	free(p->s_comment);
	free(p->deadline);
	free(p->networks);
	free(p->customergroups);
	free(p->excluded_networks);
	free(p->excluded_customergroups);
}
Пример #4
0
string FloatConstNode::stringValue(){
	return ftoa( value );
}
Пример #5
0
static void cliCMix(char *cmdline)
{
    int i, check = 0;
    int num_motors = 0;
    uint8_t len;
    char buf[16];
    float mixsum[3];
    char *ptr;

    len = strlen(cmdline);

    if (len == 0) {
        cliPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n");
        for (i = 0; i < MAX_MOTORS; i++) {
            if (mcfg.customMixer[i].throttle == 0.0f)
                break;
            num_motors++;
            printf("#%d:\t", i + 1);
            printf("%s\t", ftoa(mcfg.customMixer[i].throttle, buf));
            printf("%s\t", ftoa(mcfg.customMixer[i].roll, buf));
            printf("%s\t", ftoa(mcfg.customMixer[i].pitch, buf));
            printf("%s\r\n", ftoa(mcfg.customMixer[i].yaw, buf));
        }
        mixsum[0] = mixsum[1] = mixsum[2] = 0.0f;
        for (i = 0; i < num_motors; i++) {
            mixsum[0] += mcfg.customMixer[i].roll;
            mixsum[1] += mcfg.customMixer[i].pitch;
            mixsum[2] += mcfg.customMixer[i].yaw;
        }
        cliPrint("Sanity check:\t");
        for (i = 0; i < 3; i++)
            cliPrint(fabsf(mixsum[i]) > 0.01f ? "NG\t" : "OK\t");
        cliPrint("\r\n");
        return;
    } else if (strncasecmp(cmdline, "reset", 5) == 0) {
        // erase custom mixer
        for (i = 0; i < MAX_MOTORS; i++)
            mcfg.customMixer[i].throttle = 0.0f;
    } else if (strncasecmp(cmdline, "load", 4) == 0) {
        ptr = strchr(cmdline, ' ');
        if (ptr) {
            len = strlen(++ptr);
            for (i = 0; ; i++) {
                if (mixerNames[i] == NULL) {
                    cliPrint("Invalid mixer type...\r\n");
                    break;
                }
                if (strncasecmp(ptr, mixerNames[i], len) == 0) {
                    mixerLoadMix(i);
                    printf("Loaded %s mix...\r\n", mixerNames[i]);
                    cliCMix("");
                    break;
                }
            }
        }
    } else {
        ptr = cmdline;
        i = atoi(ptr); // get motor number
        if (--i < MAX_MOTORS) {
            ptr = strchr(ptr, ' ');
            if (ptr) {
                mcfg.customMixer[i].throttle = _atof(++ptr);
                check++;
            }
            ptr = strchr(ptr, ' ');
            if (ptr) {
                mcfg.customMixer[i].roll = _atof(++ptr);
                check++;
            }
            ptr = strchr(ptr, ' ');
            if (ptr) {
                mcfg.customMixer[i].pitch = _atof(++ptr);
                check++;
            }
            ptr = strchr(ptr, ' ');
            if (ptr) {
                mcfg.customMixer[i].yaw = _atof(++ptr);
                check++;
            }
            if (check != 4) {
                cliPrint("Wrong number of arguments, needs idx thr roll pitch yaw\r\n");
            } else {
                cliCMix("");
            }
        } else {
            printf("Motor number must be between 1 and %d\r\n", MAX_MOTORS);
        }
    }
}
Пример #6
0
static double eval_formula(char *formula, int *is_bad_formula, stack_t **cir_ref, int id)
{
    char val_str[TMP_STR_SIZE];
    char tmp_id_str[TMP_STR_SIZE];
    size_t size = TMP_STR_SIZE;
    double val = 0.0;
    double result = 0.0;
    *is_bad_formula = 0;
    cell_type_e cell_type;
    char *arg;

    if(itoa(id, tmp_id_str, size) == NULL)
        goto error;

    push_copy(cir_ref, tmp_id_str, cgc_strlen(tmp_id_str) + 1);
    queue_t *rpn = infixtorpn(formula, cgc_strlen(formula) + 1);

    queue_t *args = NULL;
    stack_t *values = NULL;

    stack_t *tmp = NULL;
    operator_t *op = NULL;

    while (rpn != NULL) {
        arg = dequeue_copy(&rpn);
        cell_type = parsearg(arg);
        switch(cell_type) {
            case DOUBLE:
                push(&values, arg);
                break;
            case FUNCTION:
                op = get_op(arg);
                if (eval_function(op, &values, val_str, size) == -1) {
                    goto error;
                }

                push_copy(&values, val_str, size);
                break;
            case CELL_ID:
                tmp = *cir_ref;
                cell_t *cell = get_cell(arg);
                if(cell == NULL)
                    goto error;

                while (tmp != NULL) {
                    if(itoa(cell->id, tmp_id_str, size) == NULL)
                        goto error;

                    if (memcmp(tmp->data, tmp_id_str, cgc_strlen(tmp->data) + 1) == 0)
                        goto error; //Circular reference
                    tmp = tmp->next;
                }

                if (cell->cell_type == UNUSED) {
                    push_copy(&values, "0", sizeof("0"));
                } else if (cell->cell_type == DOUBLE) {
                    push_copy(&values, cell->str, cgc_strlen(cell->str) + 1);
                } else if(cell->cell_type == FORMULA) {
                    val = eval_formula(cell->formula, is_bad_formula, cir_ref, cell->id);
                    if(*is_bad_formula)
                        goto error;

                    ftoa(val, val_str, size);
                    push_copy(&values, val_str, size);
                } else {
                    goto error;
                }

                break;
            default:
                goto error;
        }
    }

    char *result_str = pop_copy(&values);
    if (values != NULL)
        goto error;

    result = atof(result_str, size, is_bad_formula);
    if (*is_bad_formula)
        goto error;

    goto cleanup;

error:
    *is_bad_formula = 1;
    val = 0.0;
    clear_queue(&rpn);
    clear_queue(&args);
    clear_stack(&values);
cleanup:
    free(pop_copy(cir_ref));

    return result;
}
Пример #7
0
void doSweepLoop()
{
    sei();

    servo_data_t *servo = create_jim_servo();

    ir_init(ADC_ONE_64TH, ADC_AREF, 2);
    timer_prescaler_t prescaler = TIMER_ONE_1024TH;
    ping_init(prescaler);
    oi_t *oiSensor = malloc(sizeof(oi_t));
    oi_tare_encoders(&(oiSensor->left_encoder), &(oiSensor->right_encoder));
    int velocity = 0;
    int radius = 0;
    int leftWheelVelocity = 0;
    int rightWheelVelocity = 0;
    oi_full_mode();

    ir_enable_continous_mode();
    sendPing();

    printf0("creating stored ir sensor lookup table...\r\n");
    list_t *lookup_table = create_jims_ir_sensor_lookup_table();
    printf0("done...\r\n");
    while (1)
    {
        char c = '\0';

        char ping_available = 0;
        unsigned p_cm;
        if (volatile_ping_capture_complete)
        {
            ping_available = 1;
            unsigned long end_capture_count = tmr1_read_input_capture_count();
            unsigned long end_time_cap = (volatile_timer1_overflows << 16) | end_capture_count;
            unsigned long delta = end_time_cap - volatile_ping_send_pulse_start_time;
            p_cm = ping_count_to_cm(prescaler, delta);

            //send again
            sendPing();
        }

        int requestIrCalibration = 0;
        handleInput(servo, &leftWheelVelocity, &rightWheelVelocity, &requestIrCalibration);

        if (requestIrCalibration)
        {
            lfreefree(lookup_table);
            cli();
            lookup_table = create_ir_lookup_table_from_ping(servo, prescaler);
            sei();
        }

        unsigned voltage = ir_read_voltage_avg(1);
        unsigned calculatedDist = ir_lookup_distance(lookup_table, voltage);
        double ir_cm = calculatedDist * 0.254;
        ir_cm = calculatedDist / 10;
        char buff[200];
        ftoa(buff, ir_cm);

        //unsigned p_cm; //= ping_cm_busy_wait(prescaler);
        //p_cm = ping_count_to_cm(prescaler, volatile_timer1_capture_count);

        unsigned curDeg = servo_calculate_position_deg(servo);

        //printf0("%d volts \t\t calculatedDist= %u \r\n", voltage, calculatedDist);
        printf0("%uº \t %s ir_cm [%u v]\t", curDeg, buff, voltage);
        printf0(" pw=%u\t", servo->cur_pulse_width);
        if (ping_available)
            printf0("%u p_cm\t", p_cm);
        printf0("\r\n");
    }
}
Пример #8
0
/* Returns bytes written, negative on error */
int vfprintf(rd_t rd, char *fmt, va_list ap, int (*puts_fn)(rd_t,char*), int (*putc_fn)(rd_t,char)) {
    int total = 0;

    /* We buffer data here as long as possible, because the actual puts/putc functions tend to be slow
     * and the fewer calls we can make, the better. */
    char holding[32];
    int hold_count = 0;
    const int hold_len = sizeof(holding)/sizeof(holding[0]);

    holding[0] = '\0';

    int ret;

    while (*fmt) {
        if (*fmt == '%') {
            switch (*(++fmt)) {
                case 'x': {
                    /* Hex */
                    uint32_t hex = va_arg(ap, uint32_t);
                    char buf[12];

                    uitoa(hex, buf, 9, 16);

                    ret = holding_flush(holding, &hold_count, rd, puts_fn);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }

                    ret = puts_fn(rd, buf);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }

                    break;
                }
                case 'i': case 'd': {
                    int num = va_arg(ap, int);
                    char buf[9];    /* 7 digits in INT_MAX + '-' and '\0' */

                    itoa(num, buf, 12, 10);

                    ret = holding_flush(holding, &hold_count, rd, puts_fn);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }

                    ret = puts_fn(rd, buf);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }
                    break;
                }
                case 'u': {
                    uint32_t num = va_arg(ap, uint32_t);
                    char buf[9];    /* 7 digits in INT_MAX + '-' and '\0' */

                    uitoa(num, buf, 12, 10);

                    ret = holding_flush(holding, &hold_count, rd, puts_fn);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }

                    ret = puts_fn(rd, buf);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }
                    break;
                }
                case 'f': {
                    float num = (float) va_arg(ap, double);
                    char buf[20];

                    ftoa(num, 0.0001f, buf, 20);

                    ret = holding_flush(holding, &hold_count, rd, puts_fn);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }

                    ret = puts_fn(rd, buf);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }
                    break;
                }
                case 'c': {
                    char letter = (char) va_arg(ap, uint32_t);

                    ret = holding_push(letter, holding, hold_len, &hold_count, rd, puts_fn);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }

                    break;
                }
                case 's': {
                    char *s = va_arg(ap, char*);

                    ret = holding_flush(holding, &hold_count, rd, puts_fn);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }

                    ret = puts_fn(rd, s);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }

                    break;
                }
                case '%': { /* Just print a % */
                    ret = holding_push('%', holding, hold_len, &hold_count, rd, puts_fn);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }

                    break;
                }
                default: {
                    ret = holding_push('%', holding, hold_len, &hold_count, rd, puts_fn);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }

                    ret = holding_push(*fmt, holding, hold_len, &hold_count, rd, puts_fn);
                    if (ret >= 0) {
                        total += ret;
                    }
                    else {
                        return ret;
                    }
                }
            }

            fmt++;
        }
        else {
            ret = holding_push(*fmt++, holding, hold_len, &hold_count, rd, puts_fn);
            if (ret >= 0) {
                total += ret;
            }
            else {
                return ret;
            }
        }
    }
Пример #9
0
void lcd_putint(float x){
	char res[64];
	ftoa(x,res,4);
	
	lcd_printf(res);
}
Пример #10
0
static void cliDump(char *cmdline)
{
    int i;
    char buf[16];
    float thr, roll, pitch, yaw;
    uint32_t mask;
    const clivalue_t *setval;

    printf("Current Config: Copy everything below here...\r\n");

    // print out aux switches
    cliAux("");

    // print out current motor mix
    printf("mixer %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]);

    // print custom mix if exists
    if (mcfg.customMixer[0].throttle != 0.0f) {
        for (i = 0; i < MAX_MOTORS; i++) {
            if (mcfg.customMixer[i].throttle == 0.0f)
                break;
            thr = mcfg.customMixer[i].throttle;
            roll = mcfg.customMixer[i].roll;
            pitch = mcfg.customMixer[i].pitch;
            yaw = mcfg.customMixer[i].yaw;
            printf("cmix %d", i + 1);
            if (thr < 0)
                printf(" ");
            printf("%s", ftoa(thr, buf));
            if (roll < 0)
                printf(" ");
            printf("%s", ftoa(roll, buf));
            if (pitch < 0)
                printf(" ");
            printf("%s", ftoa(pitch, buf));
            if (yaw < 0)
                printf(" ");
            printf("%s\r\n", ftoa(yaw, buf));
        }
        printf("cmix %d 0 0 0 0\r\n", i + 1);
    }

    // print enabled features
    mask = featureMask();
    for (i = 0; ; i++) { // disable all feature first
        if (featureNames[i] == NULL)
            break;
        printf("feature -%s\r\n", featureNames[i]);
    }
    for (i = 0; ; i++) {  // reenable what we want.
        if (featureNames[i] == NULL)
            break;
        if (mask & (1 << i))
            printf("feature %s\r\n", featureNames[i]);
    }

    // print RC MAPPING
    for (i = 0; i < 8; i++)
        buf[mcfg.rcmap[i]] = rcChannelLetters[i];
    buf[i] = '\0';
    printf("map %s\r\n", buf);

    // print settings
    for (i = 0; i < VALUE_COUNT; i++) {
        setval = &valueTable[i];
        printf("set %s = ", valueTable[i].name);
        cliPrintVar(setval, 0);
        cliPrint("\r\n");
    }
}
Пример #11
0
int main(int argc, char ** argv) {
    struct SCRAMNET * scram;
    scramtime_st scramtime;
    char    name[256];
    int     i;
    bool    got_pnt, got_agc, got_alfashm;
    time_t  time_pnt, time_agc, time_if1, time_if2, time_tt, time_alfashm, time_fix;
    double Enc2Deg = 1./ ( (4096. * 210. / 5.0) / (360.) );
    const double D2R=(M_PI/180.0);

    char strbuf[1024];
    redisContext *c;
    redisReply *reply;
    const char *hostname = "127.0.0.1";
    int port = 6379;

    char *infilename;
    char *outfilename;
    char *rotatefilename;
    FILE *scramfp;
   
    const char *usage = "Usage: s6_observatory [-test] [-stdout] [-nodb] [-nottl] [-hostname hostname] [-port port]\n                      [-infile scram_input_file] [-outfile scram_output_file] [-rotate seconds]\n  -test: don't read scram, put in dummy values\n  -stdout: output packets to stdout (normally quiet)\n  -nodb: don't update redis db\n  -nottl: don't expire any of the scram keys in the redis db\n  hostname/port: for redis database (default 127.0.0.1:6379)\n  -infile: name of file to read scram packets from\n  -outfile: name of file to write scram packets to\n     (can't use both infile and outfile simultaneously)\n  -rotate: for output files, rotate every seconds seconds (default MAXINT)\n\n";

    bool dotest = false;
    bool dostdout = false;
    bool nodb = false;
    bool nottl = false;
    bool useinfile = false;
    bool useoutfile = false;
   
    int rotatesecs = 2147483647;
    time_t lastrotate;

    double RA, Dec, MJD, azfix, zafix; // PNT vars
    int mlasttck, Az, ZA, agctime; double Azdeg, ZAdeg, timesecs, Azerrdeg, ZAerrdeg; // AGC vars
    int synIDB_0, fltrbank; double synIHz_0, rfFreq, FrqMhz; // IF1 vars
    bool useAlfa; int sigSrc; // IF2 vars
    int encoder; double degrees; // TT vars
    int fstbias, sndbias; double motorpos; // ALFASHM vars
    double synIHz_1, lo2Hz;

    double beamAz, beamZA; // ra/dec conversion per beam vars
    double coord_unixtime;
    double fixedRA[7]; // for fixed values
    double fixedDec[7];

    // totally annoying: string buffers for doubles above, since hiredis/hmset demands we need these
    char RAbuf[24];
    char Decbuf[24];
    char MJDbuf[24];
    char azfixbuf[24];
    char zafixbuf[24];
    char Azdegbuf[24];
    char ZAdegbuf[24];
    char synIHz_0buf[24];
    char synIHz_1buf[24];
    char rfFreqbuf[24];
    char FrqMhzbuf[24];
    char degreesbuf[24];
    char motorposbuf[24];
    char *fixedRAbuf[7];
    char *fixedDecbuf[7];

    for (i = 0; i < 7; i++) { 
        fixedRAbuf[i] = malloc(24); 
        fixedDecbuf[i] = malloc(24); 
    }

    infilename     = malloc(1024);
    outfilename    = malloc(1024);
    rotatefilename = malloc(1024);

    for (i = 1; i < argc; i++) {
        if (strcmp(argv[i],"-test") == 0) { 
            // do not read scram - use hard coded fake data instead
            dotest = true; 
        }
        else if (strcmp(argv[i],"-infile") == 0) { 
            // do not read scram from network - read scram blocks 
            // from a file instead
            useinfile = true; infilename = argv[++i];   
        }
        else if (strcmp(argv[i],"-nodb") == 0) { 
            // do not update the redis DB
            nodb = true;    
        }
        else if (strcmp(argv[i],"-stdout") == 0) { 
            // output values to stdout.  Can be used with or without
            // -nodb but is nearly always specified when -nodb is used.
            dostdout = true;    
        }
        else if (strcmp(argv[i],"-nottl") == 0) { 
            // do not set time-to-live for redis entries.
            // TODO - redis TTL not yet implemented.
            nottl = true;   
        }
        else if (strcmp(argv[i],"-hostname") == 0) { 
            // the host on which the redis DB resides
            hostname = argv[++i];   
        }
        else if (strcmp(argv[i],"-port") == 0) { 
            // the port on which the redis DB server is listening
            port = atoi(argv[++i]);     
        }
        else if (strcmp(argv[i],"-outfile") == 0) { 
            // dump all scram block to a file
            useoutfile = true; outfilename = argv[++i];     
        }
        else if (strcmp(argv[i],"-rotate") == 0) { 
            // how often to rotate the outfile 
            rotatesecs = atoi(argv[++i]);   
        }
        else { 
            fprintf(stderr,"%s",usage); exit (1);   
        }
    }

    if (useinfile && useoutfile) {
        fprintf(stderr,"can't use both -infile and -outfile\n\n%s",usage); exit (1);
    }

    if (useinfile) {
        if ((scramfp = fopen(infilename,"rb")) == NULL) {
            fprintf(stderr,"cannot open file for reading: %s\n",infilename); exit (1);
        }
    }
    if (useoutfile) {
        lastrotate = time(NULL);
        if ((scramfp = fopen(outfilename,"wb")) == NULL) {
            fprintf(stderr,"cannot open file for writing: %s\n",outfilename); exit (1);
        }
    }

    if (!nodb) {
        struct timeval timeout = { 1, 500000 }; // 1.5 seconds
        c = redisConnectWithTimeout(hostname, port, timeout);
        if (c == NULL || c->err) {
            if (c) {
                printf("Connection error: %s\n", c->errstr);
                redisFree(c);
            } else {
                printf("Connection error: can't allocate redis context\n");
            }
            exit(1);
        }
    }

    if (!dotest) scram = init_scramread(&scramtime.scram);

    got_pnt = got_agc = got_alfashm = false;
    time_pnt = time_agc = time_if1 = time_if2 = time_tt = time_alfashm = time_fix = 0;

    // main loop
    while (1) {

        if (!dotest) {

            if (useinfile) {
                if (fread(&scramtime,sizeof(scramtime),1,scramfp) != 1) {
                    fprintf(stderr,"end of file (reading scram)\n");
                    fclose(scramfp);
                    exit(0);
                }
            } else { 
                if (read_scram(scram) == -1) {
                    fprintf(stderr, "GetScramData : bad scram read\n");
                    exit(1);
                }
                if (useoutfile) {
                    scramtime.time = time(NULL);
                    if (fwrite(&scramtime,sizeof(scramtime),1,scramfp) != 1) {
                        fprintf(stderr,"problem writing to scram file\n");
                        fclose(scramfp);
                        exit(1);
                    }
                    if ((time(NULL)-lastrotate)>=rotatesecs) {
                        sprintf(rotatefilename,"%s.%d",outfilename,(int)time(NULL));
                        // fprintf(stderr,"rotating: %s to %s...\n",outfilename,rotatefilename);
                        fclose(scramfp);
                        rename(outfilename,rotatefilename);
                        if ((scramfp = fopen(outfilename,"wb")) == NULL) {
                            fprintf(stderr,"cannot open file for writing: %s\n",outfilename); exit (1);
                        }
                        lastrotate = time(NULL);
                    }
                } 
            }

            getnameinfo((const struct sockaddr *)&scram->from, sizeof(struct sockaddr_in), name, 256, NULL, 0, 0);
            
            if (strcmp(scram->in.magic, "PNT") == 0) {
                got_pnt = true;
                time_pnt = time(NULL); 
                if (useinfile) {
                    time_pnt = scramtime.time;
                }
                RA  = scram->pntData.st.x.pl.curP.raJ;
                Dec = scram->pntData.st.x.pl.curP.decJ;
                RA  *= 24.0 / C_2PI;
                Dec *= 360.0 / C_2PI;
                MJD  = scram->pntData.st.x.pl.tm.mjd + scram->pntData.st.x.pl.tm.ut1Frac;
                azfix = scram->pntData.st.x.modelCorEncAzZa[0] / D2R;   // go to degrees
                zafix = scram->pntData.st.x.modelCorEncAzZa[1] / D2R;

                ftoa(RA,RAbuf); ftoa(Dec,Decbuf); ftoa(MJD,MJDbuf); ftoa(azfix,azfixbuf); ftoa(zafix,zafixbuf);
                sprintf(strbuf,"SCRAM:PNT PNTSTIME %ld PNTRA %0.10lf PNTDEC %0.10lf PNTMJD %0.10lf PNTAZCOR %0.10lf PNTZACOR %0.10lf",time_pnt,RA,Dec,MJD,azfix,zafix);
                if (dostdout) {
                    fprintf(stderr,"%s\n",strbuf);
                }
            if (!nodb) { 
                reply = redisCommand(c,"HMSET %s %s %d %s %s %s %s %s %s %s %s %s %s",
                                   "SCRAM:PNT","PNTSTIME",time_pnt,"PNTRA",RAbuf,"PNTDEC",Decbuf,"PNTMJD",MJDbuf,"PNTAZCOR",azfixbuf,"PNTZACOR",zafixbuf); 
                freeReplyObject(reply); 
            }
            if (dostdout) {
                fprintf(stderr,"%s\n",strbuf);
            }
            
            } else if (strcmp(scram->in.magic, "AGC") == 0) {
                got_agc = true;
                time_agc = time(NULL); 
                if (useinfile) {
                    time_agc = scramtime.time;
                }
                mlasttck = scram->agcData.st.secMLastTick;
                Az = scram->agcData.st.cblkMCur.dat.posAz;
                Azdeg = scram->agcData.st.cblkMCur.dat.posAz * 0.0001;
                ZA = scram->agcData.st.cblkMCur.dat.posGr; 
                ZAdeg = scram->agcData.st.cblkMCur.dat.posGr * 0.0001;
                agctime = scram->agcData.st.cblkMCur.dat.timeMs;
                timesecs = scram->agcData.st.cblkMCur.dat.timeMs * 0.001; 
                Azerrdeg = scram->agcData.st.posErr.reqPosDifRd[0];
                ZAerrdeg = scram->agcData.st.posErr.reqPosDifRd[1];
                ftoa(Azdeg,Azdegbuf); ftoa(ZAdeg,ZAdegbuf);
                sprintf(strbuf,"SCRAM:AGC AGCSTIME %ld AGCTIME %d AGCAZ %0.10lf AGCZA %0.10lf",time_agc,agctime,Azdeg,ZAdeg);
                if (!nodb) { 
                    reply = redisCommand(c,"HMSET %s %s %d %s %d %s %s %s %s",
                                     "SCRAM:AGC","AGCSTIME",time_agc,"AGCTIME",agctime,"AGCAZ",Azdegbuf,"AGCZA",ZAdegbuf); 
                    freeReplyObject(reply); 
                }
                if (dostdout) {
                    fprintf(stderr,"%s\n",strbuf);
                }

            } else if (strcmp(scram->in.magic, "IF1") == 0) {
                time_if1 = time(NULL); 
                if (useinfile) {
                    time_if1 = scramtime.time;
                }
                synIHz_0 = scram->if1Data.st.synI.freqHz[0];        // TODO label/name as 1st LO, right?
                synIDB_0 = scram->if1Data.st.synI.ampDb[0];
                rfFreq = scram->if1Data.st.rfFreq;
                FrqMhz = scram->if1Data.st.if1FrqMhz;
                fltrbank = scram->if1Data.st.stat2.alfaFb;
                ftoa(synIHz_0,synIHz_0buf); ftoa(rfFreq,rfFreqbuf); ftoa(FrqMhz,FrqMhzbuf);
                sprintf(strbuf,"SCRAM:IF1 IF1STIME %ld IF1SYNHZ %0.10lf IF1SYNDB %d IF1RFFRQ %0.10lf IF1IFFRQ %0.10lf IF1ALFFB %d",
                               time_if1,synIHz_0,synIDB_0,rfFreq,FrqMhz,fltrbank);
                if (!nodb) { 
                    reply = redisCommand(c,"HMSET %s %s %d %s %s %s %d %s %s %s %s %s %d",
                                         "SCRAM:IF1","IF1STIME",time_if1,"IF1SYNHZ",synIHz_0buf,"IF1SYNDB",synIDB_0,"IF1RFFRQ",rfFreqbuf,
                                         "IF1IFFRQ",FrqMhzbuf,"IF1ALFFB",fltrbank); 
                    freeReplyObject(reply); 
                }
                if (dostdout) {
                    fprintf(stderr,"%s\n",strbuf);
                }

            } else if (strcmp(scram->in.magic, "IF2") == 0) {
                time_if2 = time(NULL); 
                if (useinfile) {
                    time_if2 = scramtime.time;
                }
                if(scram->if2Data.st.stat1.useAlfa) { useAlfa = true; } else { useAlfa = false; }
                synIHz_1 = scram->if2Data.st.synI.freqHz[4];        // TODO label/name as 2nd LO, right?  AO Phil says to use [4]
                ftoa(synIHz_1,synIHz_1buf);
                sigSrc = scram->if2Data.st.stat1.sigSrc;
                sprintf(strbuf,"SCRAM:IF2 IF2STIME %ld IF2SYNHZ %s IF2ALFON %d IF2SIGSR %d",time_if2,synIHz_1buf,useAlfa,sigSrc);
                if (!nodb) { 
                reply = redisCommand(c,"HMSET %s %s %d %s %s %s %d %s %d",
                                     "SCRAM:IF2","IF2STIME",time_if2, "IF2SYNHZ", synIHz_1buf, "IF2ALFON",useAlfa, "IF2SIGSR",sigSrc); 
                freeReplyObject(reply); 
                }
                if (dostdout) {
                    fprintf(stderr,"%s\n",strbuf);
                }

            } else if (strcmp(scram->in.magic, "TT") == 0) {
                time_tt = time(NULL); 
                if (useinfile) {
                    time_tt = scramtime.time;
                }
                encoder = scram->ttData.st.slv[0].inpMsg.position;
                degrees = (double)(scram->ttData.st.slv[0].inpMsg.position) * Enc2Deg;
double turDeg=scram->ttData.st.slv[0].tickMsg.position/ TUR_DEG_TO_ENC_UNITS;

                ftoa(degrees,degreesbuf);
                sprintf(strbuf,"SCRAM:TT TTSTIME %ld TTTURENC %d TTTURDEG %0.10lf",time_tt,encoder,degrees);
                if (!nodb) { 
                    reply = redisCommand(c,"HMSET %s %s %d %s %d %s %s",
                                         "SCRAM:TT","TTSTIME",time_tt,"TTTURENC",encoder,"TTTURDEG",degreesbuf); 
                    freeReplyObject(reply); 
                }
                if (dostdout) {
                    fprintf(stderr,"%s\n",strbuf);
                }

            } else if (strcmp(scram->in.magic, "ALFASHM") == 0) {
                got_alfashm = true; 
                time_alfashm = time(NULL); 
                if (useinfile) {
                    time_alfashm = scramtime.time;
                }
                fstbias = (int)scram->alfa.first_bias;
                sndbias = (int)scram->alfa.second_bias;
                motorpos = scram->alfa.motor_position; 
                ftoa(motorpos,motorposbuf)
                sprintf(strbuf,"SCRAM:ALFASHM ALFSTIME %ld ALFBIAS1 %d ALFBIAS2 %d ALFMOPOS %0.10lf",
                    time_alfashm,fstbias,sndbias,motorpos);
                if (!nodb) { 
                    reply = redisCommand(c,"HMSET %s %s %d %s %d %s %d %s %s",
                                         "SCRAM:ALFASHM","ALFSTIME",time_alfashm,"ALFBIAS1",fstbias,"ALFBIAS2",sndbias,"ALFMOPOS",motorposbuf); 
                    freeReplyObject(reply); 
                }
                if (dostdout) {
                    fprintf(stderr,"%s\n",strbuf);
                }
            } else {
            // fprintf(stderr, "UNKNOWN SCRAM: %ld %s from %s\n", current, scram->in.magic, name);
            }

            // Calculate derived values.
            if (got_alfashm && got_agc && got_pnt) {
                time_fix = time_alfashm;
                if (time_agc > time_fix) {
                    time_fix = time_agc;
                }
                if (time_pnt > time_fix) {
                    time_fix = time_pnt;
                }
                coord_unixtime = s6_seti_ao_timeMS2unixtime(agctime,time_fix);
                for (i=0;i<7;i++) {
                    beamAz = Azdeg; beamZA = ZAdeg; // in degrees
                    beamAz -= azfix; beamZA -= zafix; // fix also in degrees
                    s6_BeamOffset(&beamAz, &beamZA, i, motorpos); // i is beam
                    s6_AzZaToRaDec(beamAz, beamZA, coord_unixtime, &fixedRA[i], &fixedDec[i]);
                    ftoa(fixedRA[i],fixedRAbuf[i]); 
                    ftoa(fixedDec[i],fixedDecbuf[i]); 
                }
                sprintf(strbuf,"SCRAM:DERIVED DERTIME %ld RA0 %0.10lf DEC0 %0.10lf RA1 %0.10lf DEC1 %0.10lf RA2 %0.10lf DEC2 %0.10lf RA3 %0.10lf DEC3 %0.10lf RA4 %0.10lf DEC4 %0.10lf RA5 %0.10lf DEC5 %0.10lf RA6 %0.10lf DEC6 %0.10lf",
                  time_fix,fixedRA[0],fixedDec[0],fixedRA[1],fixedDec[1],fixedRA[2],fixedDec[2],fixedRA[3],fixedDec[3],
                  fixedRA[4],fixedDec[4],fixedRA[5],fixedDec[5],fixedRA[6],fixedDec[6]);
                if (!nodb) { 
                    reply = redisCommand(c,"HMSET %s %s %d %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s",
                                         "SCRAM:DERIVED","DERTIME",time_fix,"RA0",fixedRAbuf[0],"DEC0",fixedDecbuf[0],"RA1",fixedRAbuf[1],
                                         "DEC1", fixedDecbuf[1],"RA2",fixedRAbuf[2],"DEC2",fixedDecbuf[2],"RA3",fixedRAbuf[3],"DEC3",fixedDecbuf[3],
                                         "RA4",fixedRAbuf[4],"DEC4",fixedDecbuf[4],"RA5",fixedRAbuf[5],"DEC5",fixedDecbuf[5],"RA6",fixedRAbuf[6],
                                         "DEC6",fixedDecbuf[6]); freeReplyObject(reply); 
                }
                if (dostdout) {
                    fprintf(stderr,"%s\n",strbuf);
                }
            }
        } // end if !nodotest

        else { // test mode
            fprintf(stderr, "..test mode..\n");  // indicate that we are running

            sprintf(strbuf,"SCRAM:PNT PNTSTIME %ld PNTRA 1.1 PNTDEC 2.2 PNTMJD 3.3 PNTAZCOR 4.4 PNTZACOR 5.5",time(NULL));
            if (dostdout) {
                fprintf(stderr,"%s\n",strbuf);
            }
            if (!nodb) { 
                reply = redisCommand(c,"HMSET %s %s %d %s %s %s %s %s %s %s %s %s %s",
                                     "SCRAM:PNT","PNTSTIME",time(NULL),"PNTRA","1.1","PNTDEC","2.2","PNTMJD","3.3",
                                     "PNTAZCOR","4.4","PNTZACOR","5.5"); 
                if (reply->type == REDIS_REPLY_ERROR) {
                    fprintf(stderr, "Error: %s\n", reply->str);
                }
                freeReplyObject(reply);
            }

            sprintf(strbuf,"SCRAM:AGC AGCSTIME %ld AGCTIME 1 AGCAZ 2.2 AGCZA 3.3",time(NULL));
            if (dostdout) {
                fprintf(stderr,"%s\n",strbuf);
            }
            if (!nodb) { 
                reply = redisCommand(c,"HMSET %s %s %d %s %d %s %s %s %s","SCRAM:AGC","AGCSTIME",time(NULL),"AGCTIME",1,
                                     "AGCAZ","2.2","AGCZA","3.3"); 
                if (reply->type == REDIS_REPLY_ERROR) { 
                    fprintf(stderr, "Error: %s\n", reply->str); 
                }
                freeReplyObject(reply);
            }
            sprintf(strbuf,"SCRAM:IF1 IF1STIME %ld IF1SYNHZ 1.1 IF1SYNDB 2 IF1RFFRQ 3.3 IF1IFFRQ 4.4 IF1ALFFB 5",time(NULL));
            if (dostdout) { 
                fprintf(stderr,"%s\n",strbuf);
            }
            if (!nodb) { 
                reply = redisCommand(c,"HMSET %s %s %d %s %s %s %d %s %s %s %s %s %d",
                                     "SCRAM:IF1","IF1STIME",time(NULL),"IF1SYNHZ","1.1","IF1SYNDB",2,"IF1RFFRQ","3.3",
                                     "IF1IFFRQ","4.4","IF1ALFFB",5); 
                if (reply->type == REDIS_REPLY_ERROR) { 
                    fprintf(stderr, "Error: %s\n", reply->str); 
                }
                freeReplyObject(reply);
            }

            sprintf(strbuf,"SCRAM:IF2 IF2STIME %ld IF2SYNHZ 0.0 IF2ALFON 1",time(NULL));
            if (dostdout) {
                fprintf(stderr,"%s\n",strbuf);
            }
            if (!nodb) { 
                reply = redisCommand(c,"HMSET %s %s %d %s %s %s %d","SCRAM:IF2","IF2STIME",time(NULL),"IF2SYNHZ","0.0","IF2ALFON",1); 
                if (reply->type == REDIS_REPLY_ERROR) { 
                    fprintf(stderr, "Error: %s\n", reply->str); 
                }
                freeReplyObject(reply);
            }

            sprintf(strbuf,"SCRAM:TT TTSTIME %ld TTTURENC 1 TTTURDEG 2.2",time(NULL));
            if (dostdout) {
                fprintf(stderr,"%s\n",strbuf);
            }
            if (!nodb) { 
                reply = redisCommand(c,"HMSET %s %s %d %s %d %s %s","SCRAM:TT","TTSTIME",time(NULL),"TTTURENC",1,"TTTURDEG","2.2"); 
                if (reply->type == REDIS_REPLY_ERROR) { 
                    fprintf(stderr, "Error: %s\n", reply->str); 
                }
                freeReplyObject(reply);
            }

            sprintf(strbuf,"SCRAM:ALFASHM ALFSTIME %ld ALFBIAS1 1 ALFBIAS2 2 ALFMOPOS 3.3",time(NULL));
            if (dostdout) {
                fprintf(stderr,"%s\n",strbuf);
            }
            if (!nodb) { 
                reply = redisCommand(c,"HMSET %s %s %d %s %d %s %d %s %s","SCRAM:ALFASHM","ALFSTIME",time(NULL),"ALFBIAS1",1,"ALFBIAS2",2,"ALFMOPOS","3.3"); 
                if (reply->type == REDIS_REPLY_ERROR) { 
                    fprintf(stderr, "Error: %s\n", reply->str); 
                }
                freeReplyObject(reply);
            }

            sprintf(strbuf,"SCRAM:DERIVED DERTIME %ld RA0 0.1 DEC0 0.1 RA1 1.2 DEC1 1.2 RA2 2.3 DEC2 2.3 RA3 3.4 DEC3 3.4 RA4 4.4 DEC4 4.5 RA5 5.6 DEC5 5.6 RA6 6.7 DEC6 6.7",time(NULL));
            if (dostdout) {
                fprintf(stderr,"%s\n",strbuf);
            }
            if (!nodb) { 
                reply = redisCommand(c,"HMSET %s %s %d %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s","SCRAM:DERIVED","DERTIME",time(NULL),"RA0","0.1","DEC0","0.1","RA1","1.2","DEC1","1.2","RA2","2.3","DEC2","2.3","RA3","3.4","DEC3","3.4","RA4","4.5","DEC4","4.5","RA5","5.6","DEC5","5.6","RA6","6.7","DEC6","6.7");
                if (reply->type == REDIS_REPLY_ERROR) { 
                    fprintf(stderr, "Error: %s\n", reply->str);
                }
                freeReplyObject(reply);
            }
        
            sleep(1); // just so we make it look more like scram and don't hammer redis db

        } // end if dotest

    } // end main loop

    exit(0);
}
Пример #12
0
string CWFixedPhase::description()
//=======================================================================================
   {
   unsigned pad = max(0, 23 - (int)name().size());
   return "         tt_"+name()+string(pad, ' ') + " = "+ftoa(target, "7.0")+ " (dd)\n";
   }
Пример #13
0
/******************************************************
函数:计算强度值
参数:Modval:回弹值得平均值
      Cardepth:炭化深度平均值
	  Pumpflag:泵送非泵送
      ht_jd0:角度修正
	  ht_mian0:测试面修正
返回值:修正后的推定值
***********************************************************/
float GetRegStrg(float Cardepth, float Modval, char Pumpflag,char ht_jd0,char ht_mian0)
{
	float fVal,fR2,fR,fCor,fR1,fC;
	
	int nC,i,nR;
	
	fVal = 0.F;
	fR =Modval;// GetAvarage();
	fR2 = GetFloatRound(fR, 1);//先四舍五入
	
	
	fCor = GetAngleCor(fR2,ht_jd0);//角度修正
	fR += fCor;
	
	fR2 = GetFloatRound(fR, 1);//四舍五入
	fCor = GetSurfCor(fR2,ht_mian0);//测试面修正
	fR += fCor;
	ftoa(fR,1,str0);
	
	if(fR-MAX_RM>0.00001F)//如果大于60.0,表示推定值大于60.1
	{
		fVal=60.1f;//MORE_THEN_SIXTY;
		return fVal;
	}
	if(fR<MIN_RM-0.0001F) //如果小余10.0,表示小余9.9
	{
		fVal=9.9f;//LESS_THEN_TEN;
		return fVal;
	}
	if(Pumpflag==1)//表示非泵送
	{
		fC=Cardepth;
		//根据平均碳化深度值求出列数
		nC = 0;
		for(i=1; i<=12; i++)
		{
			if(fC+0.25 >= i*0.5F)
				nC ++;
			else
				break;
		}
		//根据修正后的回弹值求出行数

		nR = 0;
		for (i = 1; i < CURVELINE - 1; i++)
		{
			if(fR >= MIN_RM + (float)i*0.2F)
				nR ++;
			else
				break;
		}
		fR1 = MIN_RM+(float)nR*0.2F;
		//查强度换算表,获取换算强度值
		if((Data[nR][nC] < 0.00000001F) | ((Data[nR+1][nC]<0.00000001F)))//强度换算表中的空值
		{
			if(fR<24.0f)
				fVal=9.9f;//LESS_THEN_TEN;
			else
				fVal=60.1f;//MORE_THEN_SIXTY;
		}
		else
		{
			fVal = (float)(Data[nR][nC]+((fR-fR1)/0.2F)*(Data[nR+1][nC]-(Data[nR][nC])));
		}
		fVal=GetFloatRound(fVal,1);	
	}
	else//泵送
	{
		if(fR-52.8>0.00001F)
		{
			fVal=60.1f;//MORE_THEN_SIXTY;
			return fVal;
		}
		if(fR<18.6-0.0001F) 
		{
			fVal=9.9f;//LESS_THEN_TEN;
			return fVal;
		}
		fC=Cardepth;
		//根据平均碳化深度值求出列数
		nC = 0;
		for(i=1; i<=12; i++)
		{
			if(fC+0.25 >= i*0.5F)
				nC ++;
			else
				break;
		}
		//根据修正后的回弹值求出行数
		nR = 0;
		for(i=1; i<CURVELINE-1; i++)
		{
			if(fR > 18.6+i*0.2F)
				nR ++;
			else
				break;
		}
		fR1 = 18.6+nR*0.2F;
		//查强度换算表,获取换算强度值
		if((bengsong[nR][nC] < 0.00000001F) | (bengsong[nR+1][nC]<0.00000001F))//强度换算表中的空值
		{
			if(fR<21.0f)
				fVal=9.9f;//LESS_THEN_TEN;
			else
				fVal=60.1f;//MORE_THEN_SIXTY;
		}
		else
		{
			fVal = (float)(bengsong[nR][nC]+((fR-fR1)/0.2F)*(bengsong[nR+1][nC]-bengsong[nR][nC]));
		}
		fVal=GetFloatRound(fVal, 1);			
	}
	
	return fVal;
}
Пример #14
0
Файл: vga.c Проект: abdias9/kal
void vga_terminal_putfloat(double f) {
	char* s = ftoa(f, 2);
	printf("%s\n", s);
	free(s);
}
Пример #15
0
int main(void)
{
	uint32_t currentTime;

    // High Speed Telemetry Test Code Begin
    char numberString[12];
    // High Speed Telemetry Test Code End
    RCC_GetClocksFreq(&clocks);
    USB_Interrupts_Config();
    Set_USBClock();
    USB_Init();
    
    // Wait until device configured
    //while(bDeviceState != CONFIGURED);

    testInit();
    
    LED0_ON;
    systemReady = true;
    
    //nrf_tx_mode_no_aa(addr,5,40);
    
    nrf_rx_mode_dual(addr,5,40);
    {
        uint8_t status = nrf_read_reg(NRF_STATUS);
        nrf_write_reg(NRF_WRITE_REG|NRF_STATUS,status); // clear IRQ flags
        nrf_write_reg(NRF_FLUSH_RX, 0xff);
        nrf_write_reg(NRF_FLUSH_TX, 0xff);
    }
    while (1)
    {
        uint8_t buf[64];
        static uint8_t last_tx_done = 0;
        if(ring_buf_pop(nrf_rx_buffer,buf,32)){
            // get data from the adapter
            switch(buf[0]){
                case SET_ATT:
                    break;
                case SET_MOTOR:
                    break;
                case SET_MODE:
                    report_mode = buf[1];
                    break;
            }
            last_tx_done = 1;
        }
        
        if(tx_done){
            tx_done = 0;
            // report ACK success
            last_tx_done = 1;
        }
        
        if(ring_buf_pop(nrf_tx_buffer,buf,32)){
            if(last_tx_done){
                last_tx_done = 0;
                nrf_ack_packet(0, buf, 32);
            }
        }
        
        if (frame_50Hz)
        {
            int16_t motor_val[4];
        	frame_50Hz = false;
        	currentTime      = micros();
			deltaTime50Hz    = currentTime - previous50HzTime;
			previous50HzTime = currentTime;
            //memcpy(buf, accelSummedSamples100Hz, 12);
            //memcpy(buf+12, gyroSummedSamples100Hz, 12);
            //memcpy(buf+24, magSumed, 6);
            if(report_mode == DT_ATT){
                buf[0] = DT_ATT;
                memcpy(buf + 1, &sensors.attitude200Hz[0], 12);
                memcpy(buf + 13, &executionTime200Hz, 4);
                motor_val[0] = motor[0];
                motor_val[1] = motor[1];
                motor_val[2] = motor[2];
                motor_val[3] = motor[3];
                memcpy(buf + 17, motor_val, 8);
                usb_send_data(buf , 64);
                executionTime50Hz = micros() - currentTime;
            }else if(report_mode == DT_SENSOR){
                buf[0] = DT_SENSOR;
                memcpy(buf + 1, gyroSummedSamples100Hz, 12);
                memcpy(buf + 13, accelSummedSamples100Hz, 12);
                memcpy(buf + 25, magSumed, 6);
            }
            //nrf_tx_packet(buf,16);
            //if(nrf_rx_packet(buf,16) == NRF_RX_OK)
            //{
            //    LED0_TOGGLE;
            //}
            ring_buf_push(nrf_tx_buffer, buf, 32);
        }
        
        if(frame_10Hz)
        {
            frame_10Hz = false;
            magSumed[XAXIS] = magSum[XAXIS];
            magSumed[YAXIS] = magSum[YAXIS];
            magSumed[ZAXIS] = magSum[ZAXIS];
            magSum[XAXIS] = 0;
			magSum[YAXIS] = 0;
			magSum[ZAXIS] = 0;
            newMagData = true;
        }
        
        if (frame_100Hz)
        {
            frame_100Hz = false;
            computeAxisCommands(dt100Hz);
            mixTable();
            writeServos();
            writeMotors();
        }
        
        if (frame_200Hz)
        {
        	frame_200Hz = false;

       	    currentTime       = micros();
       	    deltaTime200Hz    = currentTime - previous200HzTime;
       	    previous200HzTime = currentTime;

       	    dt200Hz = (float)deltaTime200Hz * 0.000001f;  // For integrations in 200 Hz loop

            #if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
                sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
			    sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
			    sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];

                sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
                sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
                sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);

                computeGyroTCBias();
                sensors.gyro200Hz[ROLL ] =  ((float)gyroSummedSamples200Hz[ROLL]  / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
			    sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
                sensors.gyro200Hz[YAW  ] = -((float)gyroSummedSamples200Hz[YAW]   / 5.0f - gyroRTBias[YAW  ] - gyroTCBias[YAW  ]) * GYRO_SCALE_FACTOR;
            #endif

            #if defined(USE_MADGWICK_AHRS)
                MadgwickAHRSupdate( sensors.gyro200Hz[ROLL],   sensors.gyro200Hz[PITCH],  sensors.gyro200Hz[YAW],
                                    sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
                                    sensors.mag10Hz[XAXIS],    sensors.mag10Hz[YAXIS],    sensors.mag10Hz[ZAXIS],
                                    sensorConfig.accelCutoff,
                                    newMagData,
                                    dt200Hz );

                newMagData = false;

		        q0q0 = q0 * q0;
		        q1q1 = q1 * q1;
		        q2q2 = q2 * q2;
		        q3q3 = q3 * q3;

    	        sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
    	        sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
    	        sensors.attitude200Hz[YAW  ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
            #endif

            #if defined(USE_MARG_AHRS)
                MargAHRSupdate( sensors.gyro200Hz[ROLL],   sensors.gyro200Hz[PITCH],  sensors.gyro200Hz[YAW],
                                sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
                                sensors.mag10Hz[XAXIS],    sensors.mag10Hz[YAXIS],    sensors.mag10Hz[ZAXIS],
                                sensorConfig.accelCutoff,
                                newMagData,
                                dt200Hz );

                newMagData = false;

		        q0q0 = q0 * q0;
		        q1q1 = q1 * q1;
		        q2q2 = q2 * q2;
		        q3q3 = q3 * q3;

    	        sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
    	        sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
    	        sensors.attitude200Hz[YAW  ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
            #endif

            executionTime200Hz = micros() - currentTime;
        }
    }
    systemInit();
    systemReady = true;
    while (1)
    {
    	///////////////////////////////

        if (frame_50Hz)
        {
        	frame_50Hz = false;

        	currentTime      = micros();
			deltaTime50Hz    = currentTime - previous50HzTime;
			previous50HzTime = currentTime;

			processFlightCommands();

			executionTime50Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_10Hz)
        {
            LED0_TOGGLE;
        	frame_10Hz = false;

        	currentTime      = micros();
			deltaTime10Hz    = currentTime - previous10HzTime;
			previous10HzTime = currentTime;

			sensors.mag10Hz[XAXIS] = -((float)magSum[XAXIS] / 5.0f * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]);
			sensors.mag10Hz[YAXIS] =   (float)magSum[YAXIS] / 5.0f * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS];
			sensors.mag10Hz[ZAXIS] = -((float)magSum[ZAXIS] / 5.0f * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]);

			magSum[XAXIS] = 0;
			magSum[YAXIS] = 0;
			magSum[ZAXIS] = 0;

			newMagData = true;

        	pressureAverage = pressureSum / 10;
        	pressureSum = 0;
        	calculateTemperature();
        	calculatePressureAltitude();
        	sensors.pressureAltitude10Hz = pressureAlt;

        	serialCom();

        	if ( EKF_Initialized == false ) EKF_Init( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
                                                      sensors.mag10Hz[XAXIS],    sensors.mag10Hz[YAXIS],    sensors.mag10Hz[ZAXIS] );

            executionTime10Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_500Hz)
        {
			frame_500Hz = false;

       	    currentTime       = micros();
       	    deltaTime500Hz    = currentTime - previous500HzTime;
       	    previous500HzTime = currentTime;

       	    dt500Hz = (float)deltaTime500Hz * 0.000001f;  // For integrations in 500 Hz loop

            computeGyroTCBias();
            sensors.gyro500Hz[ROLL ] =  ((float)gyroSummedSamples500Hz[ROLL]  / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
			sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
            sensors.gyro500Hz[YAW  ] = -((float)gyroSummedSamples500Hz[YAW]   / 2.0f - gyroRTBias[YAW  ] - gyroTCBias[YAW  ]) * GYRO_SCALE_FACTOR;

            #if defined(USE_CHR6DM_AHRS)
                if ( EKF_Initialized == true ) EKF_Predict( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
                                                            dt500Hz );

                sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
                sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
                sensors.attitude200Hz[YAW  ] = gEstimatedStates.psi;
            #endif

       	    executionTime500Hz = micros() - currentTime;
		}

        ///////////////////////////////

        if (frame_200Hz)
        {
        	frame_200Hz = false;

       	    currentTime       = micros();
       	    deltaTime200Hz    = currentTime - previous200HzTime;
       	    previous200HzTime = currentTime;

       	    dt200Hz = (float)deltaTime200Hz * 0.000001f;  // For integrations in 200 Hz loop

            #if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
                sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
			    sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
			    sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];

                sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
                sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
                sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);

                computeGyroTCBias();
                sensors.gyro200Hz[ROLL ] =  ((float)gyroSummedSamples200Hz[ROLL]  / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
			    sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
                sensors.gyro200Hz[YAW  ] = -((float)gyroSummedSamples200Hz[YAW]   / 5.0f - gyroRTBias[YAW  ] - gyroTCBias[YAW  ]) * GYRO_SCALE_FACTOR;
            #endif

            #if defined(USE_MADGWICK_AHRS)
                MadgwickAHRSupdate( sensors.gyro200Hz[ROLL],   sensors.gyro200Hz[PITCH],  sensors.gyro200Hz[YAW],
                                    sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
                                    sensors.mag10Hz[XAXIS],    sensors.mag10Hz[YAXIS],    sensors.mag10Hz[ZAXIS],
                                    sensorConfig.accelCutoff,
                                    newMagData,
                                    dt200Hz );

                newMagData = false;

		        q0q0 = q0 * q0;
		        q1q1 = q1 * q1;
		        q2q2 = q2 * q2;
		        q3q3 = q3 * q3;

    	        sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
    	        sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
    	        sensors.attitude200Hz[YAW  ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
            #endif

            #if defined(USE_MARG_AHRS)
                MargAHRSupdate( sensors.gyro200Hz[ROLL],   sensors.gyro200Hz[PITCH],  sensors.gyro200Hz[YAW],
                                sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
                                sensors.mag10Hz[XAXIS],    sensors.mag10Hz[YAXIS],    sensors.mag10Hz[ZAXIS],
                                sensorConfig.accelCutoff,
                                newMagData,
                                dt200Hz );

                newMagData = false;

		        q0q0 = q0 * q0;
		        q1q1 = q1 * q1;
		        q2q2 = q2 * q2;
		        q3q3 = q3 * q3;

    	        sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
    	        sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
    	        sensors.attitude200Hz[YAW  ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
            #endif

            executionTime200Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_100Hz)
        {
        	frame_100Hz = false;

        	currentTime       = micros();
			deltaTime100Hz    = currentTime - previous100HzTime;
			previous100HzTime = currentTime;

			dt100Hz = (float)deltaTime100Hz * 0.000001f;  // For integrations in 100 Hz loop

            sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
			sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
			sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];

        	sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]);
            sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]);
            sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]);

            computeGyroTCBias();
            sensors.gyro100Hz[ROLL ] =  ((float)gyroSummedSamples100Hz[ROLL]  / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
			sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
            sensors.gyro100Hz[YAW  ] = -((float)gyroSummedSamples100Hz[YAW]   / 10.0f - gyroRTBias[YAW  ] - gyroTCBias[YAW  ]) * GYRO_SCALE_FACTOR;

            #if defined(USE_CHR6DM_AHRS)
                if ( EKF_Initialized == true ) EKF_Update( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
					                                       sensors.mag10Hz[XAXIS],    sensors.mag10Hz[YAXIS],    sensors.mag10Hz[ZAXIS],
					                                       sensorConfig.accelCutoff,
                                                           newMagData );
                newMagData = false;

		        sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
    	        sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
    	        sensors.attitude200Hz[YAW  ] = gEstimatedStates.psi;
            #endif

            computeAxisCommands(dt100Hz);
            mixTable();
            writeServos();
            writeMotors();

            // High Speed Telemetry Test Code Begin
            if ( highSpeedAccelTelemEnabled == true )
            {
            	// 100 Hz Accels
            	ftoa(sensors.accel100Hz[XAXIS], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(sensors.accel100Hz[YAXIS], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(sensors.accel100Hz[ZAXIS], numberString); uartPrint(numberString); uartPrint("\n");
            }

            if ( highSpeedGyroTelemEnabled == true )
            {
            	// 100 Hz Gyros
            	ftoa(sensors.gyro100Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(sensors.gyro100Hz[YAW  ], numberString); uartPrint(numberString); uartPrint("\n");
            }

            if ( highSpeedRollRateTelemEnabled == true )
            {
            	// Roll Rate, Roll Rate Command
            	ftoa(sensors.gyro100Hz[ROLL], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(rxCommand[ROLL],         numberString); uartPrint(numberString); uartPrint("\n");
            }

            if ( highSpeedPitchRateTelemEnabled == true )
            {
            	// Pitch Rate, Pitch Rate Command
            	ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(rxCommand[PITCH],         numberString); uartPrint(numberString); uartPrint("\n");
            }

            if ( highSpeedYawRateTelemEnabled == true )
            {
            	// Yaw Rate, Yaw Rate Command
            	ftoa(sensors.gyro100Hz[YAW], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(rxCommand[YAW],         numberString); uartPrint(numberString); uartPrint("\n");
            }

            if ( highSpeedAttitudeTelemEnabled == true )
            {
            	// 200 Hz Attitudes
            	ftoa(sensors.attitude200Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(sensors.attitude200Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(sensors.attitude200Hz[YAW  ], numberString); uartPrint(numberString); uartPrint("\n");
            }
            // High Speed Telemetry Test Code End
            executionTime100Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_5Hz)
        {
        	frame_5Hz = false;

        	currentTime     = micros();
			deltaTime5Hz    = currentTime - previous5HzTime;
			previous5HzTime = currentTime;

        	executionTime5Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_1Hz)
        {
        	frame_1Hz = false;

        	currentTime     = micros();
			deltaTime1Hz    = currentTime - previous1HzTime;
			previous1HzTime = currentTime;

        	executionTime1Hz = micros() - currentTime;
        }

        ////////////////////////////////
    }
}
Пример #16
0
/*
**	transfer copies data from "*in" to
**	Outbuf doing conversions whenever
**	necessary
*/
int
transfer(ANYTYPE *in, char sf, int sl, char df, int dl, int doff)
/* in - pointer to input chars */
/* sf - source format */
/* sl - source length */
/* df - destination format */
/* dl - destination length */
/* doff - destination offset */
{
	register char		*outp;
	register ANYTYPE	*inp;
	register int		i;
	int			j;
	short			smalli;
	char			temp[MAX_FIELD_SIZE];	/* holds char during conversions to ascii */
	float			f;
	double			d;
	long			l;


	outp = &Outbuf[doff];
	inp = in;

	if (sf == DUMMY)
		/* if source format is a dummy fields then
		   nothing else need be done */
		return (0);

	if (df == DUMMY) {
		/* fill field with dummy domain character */
		i = dl;	/* i equals the number of chars */
		while (i--)
			*outp++ = sf;	/* sf holds dummy char */
		return (0);
	}

	if (sf != CHAR_CONST) {
		if (df == CHAR_CONST) {
			/* numeric to char conversion */
			switch (sl) {
			  /* int of size 1 or 2 */
			  case 1:
				itoa(inp->i1type, temp);
				break;

			  case 2:
				itoa(inp->i2type, temp);	/* convert to ascii */
				break;

			  /* int or float of size 4 */
			  case 4:
				if (sf == INT_CONST) {
					smove(locv(inp->i4type), temp);	/* convert and copy */
				} else {
					ftoa(inp->f4type, temp, dl, Out_arg.f4prec, Out_arg.f4style);
				}
				break;

			  /* float of size 8 */
			  case 8:
				ftoa(inp->f8type, temp, dl, Out_arg.f8prec, Out_arg.f8style);
				break;

			  /* there is no possible default */
			  default:
				syserr("bad domain length %d",sl);
			}

			j = strlen(temp);
			if ((i = dl - j) < 0)
				return (5808);	/* field won't fit */

			/* blank pad from left. Number will be right justified */
			while (i--)
				*outp++ = ' ';

			bmove(temp, outp, j);
			return (0);
		}

		if (convert(inp, outp, sf, sl, df, dl))	/* numeric to numeric transfer */
			return (DOMTOOSMALL);	/* numeric truncation error */
		return (0);
	}

	/* character to numeric conversion */
	/* and character to character conversion */
	switch (df) {

	  case CHAR_CONST:
		i = sl;
		if (!i) {
			i = strlen(inp->c0type);
		}
		if (i > dl)
			i = dl;
		if (charmove(inp->c0type, outp, i))
			Baddoms++;
		for (outp += i; i<dl; i++)
			*outp++ = ' ';
		return (0);

	  case FLOAT_CONST:
		if (ingres_atof(inp->c0type, &d))
			return (BADINPUT);	/* bad conversion to numeric */
		if (dl == 8) {
			bmove(&d, outp, dl);
		} else {
			f = d;	/* f8 to f4 conversion */
			bmove(&f, outp, dl);
		}
		return (0);

	  case INT_CONST:
		if (dl == 4) {
			if (ingres_atol(inp->c0type, &l))
				return (5809);
			bmove(&l, outp, sizeof(l));
			return (0);
		}
		smalli = atoi(inp->c0type);
                if (dl == 1) {
                        if ((smalli < -128) || (smalli > 127))
                                return (5809);
                        df = smalli;
                        bmove(&df, outp, dl);
                } else
                        bmove(&smalli, outp, dl);
		return (0);
	}
	return(0);
}
Пример #17
0
static char* distance(void* void_self, SlimList *args) {
	TreadmillDistance* self = (TreadmillDistance*)void_self;
  double d = Api_DistanceTravelled(self->api);
	ftoa(self->result, d, 1);
	return self->result;
}
Пример #18
0
String String::ValueOf(float f, int digits)
{
    return ftoa(f, digits);
}
Пример #19
0
void OLED::print(float f, int precision) {
  char ret[10];
  ftoa(f, ret, precision);
  print(ret);
}
Пример #20
0
void reload(GLOBAL *g, struct notify_module *n)
{
	QueryHandle *res, *result;
	char *mailfile = 0;
	char *command;
	int i, j; 
	double balance;

	res = g->db->query(g->db->conn, "SELECT customers.id AS id, pin, name, lastname, "
		"SUM(cash.value) AS balance, "
		"(SELECT contact FROM customercontacts WHERE customerid = customers.id AND type = 8"
		"	ORDER BY id LIMIT 1) AS email "
		"FROM customers "
		"LEFT JOIN cash ON customers.id = cash.customerid "
		"WHERE deleted = 0 AND email IS NOT NULL "
		"GROUP BY customers.id, customer.name, lastname, email, pin");
	
	if( g->db->nrows(res) )
	{
		for(i=0; i<g->db->nrows(res); i++) 
		{
			balance = atof(g->db->get_data(res,i,"balance"));
			
			if( balance < n->limit ) 
			{
				command = strdup(n->command);
				mailfile = load_file(n->mailtemplate);
			
				if( mailfile ) 
				{
					if( strstr(mailfile, "%last_10_in_a_table") )
					{
						char *date, *value, *comment, *temp, *temp2;
						char *last_ten = strdup("");
							
						result = g->db->pquery(g->db->conn, "SELECT comment, time, value FROM cash WHERE customerid = ? ORDER BY time DESC LIMIT 10", g->db->get_data(res,i,"id"));
						
						for(j=0; j<g->db->nrows(result); j++) 
						{
							date = utoc(atof(g->db->get_data(result,j,"time")));
							value = g->db->get_data(result,j,"value");
							comment = g->db->get_data(result,j,"comment");
						
							temp = (char *) malloc(strlen(date)+strlen(value)+strlen(comment)+12);	
							sprintf(temp, "%s\t | %s\t\t | %s\n", date, value, comment);
						
							temp2 = g->str_concat(last_ten, temp);
							free(last_ten);
							last_ten = strdup(temp2);
							free(temp2);
							free(temp);
							free(date);
						}
															
						g->str_replace(&mailfile, "%last_10_in_a_table", last_ten);
						
						g->db->free(&result);
						free(last_ten);
					}
					
					g->str_replace(&mailfile, "%saldo", g->db->get_data(res,i,"balance"));
					g->str_replace(&mailfile, "%B", g->db->get_data(res,i,"balance"));
					g->str_replace(&mailfile, "%b", balance < 0 ? ftoa(balance * -1) : g->db->get_data(res,i,"balance"));
					g->str_replace(&mailfile, "%pin", g->db->get_data(res,i,"pin"));
					g->str_replace(&mailfile, "%name", g->db->get_data(res,i,"name"));
					g->str_replace(&mailfile, "%lastname", g->db->get_data(res,i,"lastname"));
				
					if( write_file(n->file, mailfile) < 0 )
						syslog(LOG_ERR, "[%s/notify] Unable to write temporary file '%s' for message", n->base.instance, n->file);
					free(mailfile);
				
					if( strlen(n->debugmail) < 1 )
						g->str_replace(&command, "%address", g->db->get_data(res,i,"email"));
					else
						g->str_replace(&command, "%address", n->debugmail);
					system(command); 
				}
				free(command);
			}
		}
#ifdef DEBUG1
		syslog(LOG_INFO, "DEBUG: [%s/notify] reloaded",n->base.instance);
#endif
	}
	else
		syslog(LOG_ERR, "[%s/notify] Unable to read database", n->base.instance);

	g->db->free(&res);
	free(n->command);
	free(n->file);
	free(n->mailtemplate);
	free(n->debugmail);
}
Пример #21
0
int8_t configFormatParam(char *buf, int n) {
    char str[16];

    ftoa(str, p[n], 10);
    return sprintf(buf, "%-17s\t\t%s\n", configParameterStrings[n], str);
}
Пример #22
0
int main(void)
{
	uint32_t currentTime;

    // High Speed Telemetry Test Code Begin
    char numberString[12];
    // High Speed Telemetry Test Code End

    systemInit();

    systemReady = true;

    while (1)
    {
    	///////////////////////////////

        if (frame_50Hz)
        {
        	frame_50Hz = false;

        	currentTime      = micros();
			deltaTime50Hz    = currentTime - previous50HzTime;
			previous50HzTime = currentTime;

			processFlightCommands();

			executionTime50Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_10Hz)
        {
        	frame_10Hz = false;

        	currentTime      = micros();
			deltaTime10Hz    = currentTime - previous10HzTime;
			previous10HzTime = currentTime;

			sensors.mag10Hz[XAXIS] = -((float)magSum[XAXIS] / 5.0f * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]);
			sensors.mag10Hz[YAXIS] =   (float)magSum[YAXIS] / 5.0f * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS];
			sensors.mag10Hz[ZAXIS] = -((float)magSum[ZAXIS] / 5.0f * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]);

			magSum[XAXIS] = 0;
			magSum[YAXIS] = 0;
			magSum[ZAXIS] = 0;

			newMagData = true;

        	pressureAverage = pressureSum / 10;
        	pressureSum = 0;
        	calculateTemperature();
        	calculatePressureAltitude();
        	sensors.pressureAltitude10Hz = pressureAlt;

        	serialCom();

        	if ( EKF_Initialized == false ) EKF_Init( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
                                                      sensors.mag10Hz[XAXIS],    sensors.mag10Hz[YAXIS],    sensors.mag10Hz[ZAXIS] );

            executionTime10Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_500Hz)
        {
			frame_500Hz = false;

       	    currentTime       = micros();
       	    deltaTime500Hz    = currentTime - previous500HzTime;
       	    previous500HzTime = currentTime;

       	    dt500Hz = (float)deltaTime500Hz * 0.000001f;  // For integrations in 500 Hz loop

            computeGyroTCBias();
            sensors.gyro500Hz[ROLL ] =  ((float)gyroSummedSamples500Hz[ROLL]  / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
			sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
            sensors.gyro500Hz[YAW  ] = -((float)gyroSummedSamples500Hz[YAW]   / 2.0f - gyroRTBias[YAW  ] - gyroTCBias[YAW  ]) * GYRO_SCALE_FACTOR;

            #if defined(USE_CHR6DM_AHRS)
                if ( EKF_Initialized == true ) EKF_Predict( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
                                                            dt500Hz );

                sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
                sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
                sensors.attitude200Hz[YAW  ] = gEstimatedStates.psi;
            #endif

       	    executionTime500Hz = micros() - currentTime;
		}

        ///////////////////////////////

        if (frame_200Hz)
        {
        	frame_200Hz = false;

       	    currentTime       = micros();
       	    deltaTime200Hz    = currentTime - previous200HzTime;
       	    previous200HzTime = currentTime;

       	    dt200Hz = (float)deltaTime200Hz * 0.000001f;  // For integrations in 200 Hz loop

            #if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
                sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
			    sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
			    sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];

                sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
                sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
                sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);

                computeGyroTCBias();
                sensors.gyro200Hz[ROLL ] =  ((float)gyroSummedSamples200Hz[ROLL]  / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
			    sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
                sensors.gyro200Hz[YAW  ] = -((float)gyroSummedSamples200Hz[YAW]   / 5.0f - gyroRTBias[YAW  ] - gyroTCBias[YAW  ]) * GYRO_SCALE_FACTOR;
            #endif

            #if defined(USE_MADGWICK_AHRS)
                MadgwickAHRSupdate( sensors.gyro200Hz[ROLL],   sensors.gyro200Hz[PITCH],  sensors.gyro200Hz[YAW],
                                    sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
                                    sensors.mag10Hz[XAXIS],    sensors.mag10Hz[YAXIS],    sensors.mag10Hz[ZAXIS],
                                    sensorConfig.accelCutoff,
                                    newMagData,
                                    dt200Hz );

                newMagData = false;

		        q0q0 = q0 * q0;
		        q1q1 = q1 * q1;
		        q2q2 = q2 * q2;
		        q3q3 = q3 * q3;

    	        sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
    	        sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
    	        sensors.attitude200Hz[YAW  ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
            #endif

            #if defined(USE_MARG_AHRS)
                MargAHRSupdate( sensors.gyro200Hz[ROLL],   sensors.gyro200Hz[PITCH],  sensors.gyro200Hz[YAW],
                                sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
                                sensors.mag10Hz[XAXIS],    sensors.mag10Hz[YAXIS],    sensors.mag10Hz[ZAXIS],
                                sensorConfig.accelCutoff,
                                newMagData,
                                dt200Hz );

                newMagData = false;

		        q0q0 = q0 * q0;
		        q1q1 = q1 * q1;
		        q2q2 = q2 * q2;
		        q3q3 = q3 * q3;

    	        sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
    	        sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
    	        sensors.attitude200Hz[YAW  ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
            #endif

            executionTime200Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_100Hz)
        {
        	frame_100Hz = false;

        	currentTime       = micros();
			deltaTime100Hz    = currentTime - previous100HzTime;
			previous100HzTime = currentTime;

			dt100Hz = (float)deltaTime100Hz * 0.000001f;  // For integrations in 100 Hz loop

            sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
			sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
			sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];

        	sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]);
            sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]);
            sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]);

            computeGyroTCBias();
            sensors.gyro100Hz[ROLL ] =  ((float)gyroSummedSamples100Hz[ROLL]  / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
			sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
            sensors.gyro100Hz[YAW  ] = -((float)gyroSummedSamples100Hz[YAW]   / 10.0f - gyroRTBias[YAW  ] - gyroTCBias[YAW  ]) * GYRO_SCALE_FACTOR;

            #if defined(USE_CHR6DM_AHRS)
                if ( EKF_Initialized == true ) EKF_Update( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
					                                       sensors.mag10Hz[XAXIS],    sensors.mag10Hz[YAXIS],    sensors.mag10Hz[ZAXIS],
					                                       sensorConfig.accelCutoff,
                                                           newMagData );
                newMagData = false;

		        sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
    	        sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
    	        sensors.attitude200Hz[YAW  ] = gEstimatedStates.psi;
            #endif

            computeAxisCommands(dt100Hz);
            mixTable();
            writeServos();
            writeMotors();

            // High Speed Telemetry Test Code Begin
            if ( highSpeedAccelTelemEnabled == true )
            {
            	// 100 Hz Accels
            	ftoa(sensors.accel100Hz[XAXIS], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(sensors.accel100Hz[YAXIS], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(sensors.accel100Hz[ZAXIS], numberString); uartPrint(numberString); uartPrint("\n");
            }

            if ( highSpeedGyroTelemEnabled == true )
            {
            	// 100 Hz Gyros
            	ftoa(sensors.gyro100Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(sensors.gyro100Hz[YAW  ], numberString); uartPrint(numberString); uartPrint("\n");
            }

            if ( highSpeedRollRateTelemEnabled == true )
            {
            	// Roll Rate, Roll Rate Command
            	ftoa(sensors.gyro100Hz[ROLL], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(rxCommand[ROLL],         numberString); uartPrint(numberString); uartPrint("\n");
            }

            if ( highSpeedPitchRateTelemEnabled == true )
            {
            	// Pitch Rate, Pitch Rate Command
            	ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(rxCommand[PITCH],         numberString); uartPrint(numberString); uartPrint("\n");
            }

            if ( highSpeedYawRateTelemEnabled == true )
            {
            	// Yaw Rate, Yaw Rate Command
            	ftoa(sensors.gyro100Hz[YAW], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(rxCommand[YAW],         numberString); uartPrint(numberString); uartPrint("\n");
            }

            if ( highSpeedAttitudeTelemEnabled == true )
            {
            	// 200 Hz Attitudes
            	ftoa(sensors.attitude200Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(sensors.attitude200Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
            	ftoa(sensors.attitude200Hz[YAW  ], numberString); uartPrint(numberString); uartPrint("\n");
            }
            // High Speed Telemetry Test Code End
            executionTime100Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_5Hz)
        {
        	frame_5Hz = false;

        	currentTime     = micros();
			deltaTime5Hz    = currentTime - previous5HzTime;
			previous5HzTime = currentTime;

        	executionTime5Hz = micros() - currentTime;
        }

        ///////////////////////////////

        if (frame_1Hz)
        {
        	frame_1Hz = false;

        	currentTime     = micros();
			deltaTime1Hz    = currentTime - previous1HzTime;
			previous1HzTime = currentTime;

        	executionTime1Hz = micros() - currentTime;
        }

        ////////////////////////////////
    }
}
Пример #23
0
PROCESS_THREAD(ahrs_process,ev,data)
{
  static hrclock_t timestamp;
  static float gx,gy,gz,ax,ay,az,mx,my,mz,td;
  static struct sensors_sensor *gyr, *acc, *mag;

  if(ev == PROCESS_EVENT_EXIT) { /* switch off sampling */
    if (gyr!=NULL) gyr->configure(SENSORS_ACTIVE,0);
    if (acc!=NULL) acc->configure(SENSORS_ACTIVE,0);
    if (mag!=NULL) mag->configure(SENSORS_ACTIVE,0);
  } else if (ev == sensors_event) {
    if (data==acc) {
      ax = acc->value(ACC_VALUE_X);
      ay = acc->value(ACC_VALUE_Y);
      az = acc->value(ACC_VALUE_Z);
    } else if (data==mag) {
      mx = magcal(mag,MAG_VALUE_X);
      my = magcal(mag,MAG_VALUE_Y);
      mz = magcal(mag,MAG_VALUE_Z);
    } else if (data==gyr) {
      gx = gyro2rad(gyr->value(GYRO_VALUE_X));
      gy = gyro2rad(gyr->value(GYRO_VALUE_Y));
      gz = gyro2rad(gyr->value(GYRO_VALUE_Z));
    }
  }

  PROCESS_BEGIN();

  /* initialize sensors */
  gyr = sensors_find(GYRO_SENSOR);
  acc = sensors_find(ACC_SENSOR);
  mag = sensors_find(MAG_SENSOR);

  if (gyr==NULL) {
    strncpy(msg, "gyro not found\n",sizeof(msg));
    process_exit(&ahrs_process);
  } else if (acc==NULL) {
    strncpy(msg, "accelerometer not found\n",sizeof(msg));
    process_exit(&ahrs_process);
  } else if (mag==NULL) {
    strncpy(msg, "compass not found\n",sizeof(msg));
    process_exit(&ahrs_process);
  } else {
    gyr->configure(SENSORS_ACTIVE,1);
    acc->configure(SENSORS_ACTIVE,1);
    mag->configure(SENSORS_ACTIVE,1);
  }

  /* configure sensors, gyro to 2000dps@200Hz with bandpass at 15-70Hz, bandpass
   * for zero-rate removal!  acc to 100Hz and 1Hz high-pass */
  gyr->configure(GYRO_L3G_RANGE,GYRO_L3GVAL_2000DPS);
  gyr->configure(GYRO_L3G_DRATE,GYRO_L3GVAL_200_70HZ);
  gyr->configure(GYRO_L3G_HIGHPASS,GYRO_L3GVAL_HP0); // 15Hz@200Hz with ref
  gyr->configure(GYRO_L3G_FILTER,GYRO_L3GVAL_BOTH);
  acc->configure(ACC_LSM303_RANGE,ACC_LSM303VAL_2G);
  acc->configure(ACC_LSM303_DRATE,ACC_LSM303VAL_100HZ);
  acc->configure(ACC_LSM303_HIGHPASS,ACC_LSM303VAL_HP1);
  acc->configure(ACC_LSM303_FILTER,ACC_LSM303VAL_BOTH);

  /* XXX: ignore first measurements */
  while (gx==0 || mx==0 || ax==0)
    PROCESS_YIELD_UNTIL(ev==sensors_event);

  /* reset quaternion and compass calibration */
  q0=1; q1=q2=q3=exInt=eyInt=ezInt=0;
  magcal(NULL,0);

  while (1) { /* estimate continously and write into msg */
    timestamp = clock_hrtime();
    PROCESS_YIELD_UNTIL(ev==sensors_event);

    /* calculate sampling time */
    td = (clock_hrtime()-timestamp) / 1.e6;

    /* update estimate */
    AHRSupdate(gx,gy,gz,ax,ay,az,mx,my,mz,td);
    //IMUupdate(gx,gy,gz,ax,ay,az,td);

    /* rebuild msg string */
    {
      char *s = msg;
      s   += ftoa(td,s);
      *s++ = ' ';
      s   += ftoa(q0,s);
      *s++ = ' ';
      s   += ftoa(q1,s);
      *s++ = ' ';
      s   += ftoa(q2,s);
      *s++ = ' ';
      s   += ftoa(q3,s);
      *s++ = ' ';
      s   += ftoa(exInt,s);
      *s++ = ' ';
      s   += ftoa(eyInt,s);
      *s++ = ' ';
      s   += ftoa(ezInt,s);
      *s++ = '\n';
      *s = '\0';
    }
  }

  PROCESS_END();
}
Пример #24
0
void EditorToolBar::update(float deltaTime)
{
	ASSERT(world!=0,                 "world was null!  Call setWorld first!");
	ASSERT(toolBarTools!=0,          "toolBarTools was null");
	ASSERT(toolBarMisc!=0,           "toolBarMisc was null");
	ASSERT(toolBarZone!=0,           "toolBarZone was null");
	ASSERT(mousePosLabel!=0,         "mousePosLabel was null");
	ASSERT(objectPalette!=0,         "objectPalette was null");
	ASSERT(tileTypeSelector!=0,      "tileTypeSelector was null");
	ASSERT(texturePalette_Wall!=0,   "texturePalette_Wall was null");
	ASSERT(texturePalette_Floor!=0,  "texturePalette_Floor was null");
	ASSERT(tileHeightSelector!=0,    "tileHeightSelector was null");




	if(g_Application.getState() == GAME_STATE_EDITOR)
	{
		toolBarTools->m_bVisible = true;
		toolBarMisc->m_bVisible = true;
		toolBarZone->m_bVisible = true;
		mousePosLabel->m_bVisible = true;



		// Hide the object palette (overridden by create tool)
		objectPalette->m_bVisible = false;

		// Hide the tile properties panel (overridden in map editor mode)
		toolBarTilePropreties->m_bVisible = false;


		// Get the point under the mouse cursor
		mousePos = getGroundPickPos(0.0f);

		// update the mouse cursor to show this position
		mousePosLabel->setLabel(string("Mouse: (") + ftoa(mousePos.x) +
								string(", ") + ftoa(mousePos.y) +
								string(", ") + ftoa(mousePos.z) +
								string(")"));

        if(g_Keys.isKeyDown(KEY_TEST))
        {
            world->getMap().removeAllMaterials();
        }


		// Update the tool bar widgets
		switch(toolBarTools->getTool())
		{
		case ToolBarForEditorTools::EDITOR_CREATE_TOOL:
			// Show the object palette
			objectPalette->m_bVisible = true;

			// Decide the selected object
			{
				string o = chooseNextObject();
				if(!o.empty())
				{
					nextObject = o;
				}
			}
			break;

		case ToolBarForEditorTools::EDITOR_TILE_PENCIL_TOOL:
			toolBarTilePropreties->m_bVisible = true;

			// Update tile properties from the selectors
			tileEditor_wallTextureFile = chooseTileWallTexture();
			tileEditor_floorTextureFile = chooseTileFloorTexture();
			tileEditor_type = getTileType();

			break;

		case ToolBarForEditorTools::EDITOR_TILE_BLOCK_TOOL:
			toolBarTilePropreties->m_bVisible = true;

			// Update tile properties from the selectors
			tileEditor_wallTextureFile = chooseTileWallTexture();
			tileEditor_floorTextureFile = chooseTileFloorTexture();
			tileEditor_type = getTileType();

			// Has a drag stopped?
			if(!g_Input.MouseLeft)
			{
				// drag exitted
				if(drag)
				{
					// get a reference to the map
					Map &map = world->getMap();

					// Get the position on the ground plane that the mouse was over
					vec3 groundPos = getGroundPickPos(0.0f);

					// Fill a block of tiles
					if(map.onATile(groundPos.x, groundPos.z) && map.onATile(mouseDownPos.x, mouseDownPos.z))
					{
						map.fillBlock(groundPos.x, groundPos.z,
									  mouseDownPos.x, mouseDownPos.z,
							          tileEditor_type,
									  tileEditor_properties,
									  tileEditor_floorTextureFile,
									  tileEditor_wallTextureFile,
							          tileEditor_height);

						// Rebuild the map display list
						map.reaquire();

						g_SoundSystem.play("data/sound/activate.wav");
					}
				}

				drag = false;
			}

			break;

		case ToolBarForEditorTools::EDITOR_DESTROY_TOOL:
			// (This tool doesn't require updating every tick)
			break;

		case ToolBarForEditorTools::EDITOR_ROTATE_TOOL:
			// (This tool doesn't require updating every tick)
			break;

		case ToolBarForEditorTools::EDITOR_ROTATE_X_TOOL:
			// (This tool doesn't require updating every tick)
			break;

		case ToolBarForEditorTools::EDITOR_ROTATE_Z_TOOL:
			// (This tool doesn't require updating every tick)
			break;

		case ToolBarForEditorTools::EDITOR_MOVE_TOOL:
			// (This tool doesn't require updating every tick)
			break;

		case ToolBarForEditorTools::EDITOR_SELECT_TOOL:
			// (This tool doesn't require updating every tick)
			break;
		};




		// The tile selectors cannot be opened if the tile properties dialog is not
		if(toolBarTilePropreties->m_bVisible)
		{
			switch(textureSelectorState)
			{
			case TEXTURE_SELECTOR_HIDE:
				texturePalette_Wall->m_bVisible = false;
				texturePalette_Floor->m_bVisible = false;
				break;

			case TEXTURE_SELECTOR_FLOOR:
				texturePalette_Wall->m_bVisible = false;
				texturePalette_Floor->m_bVisible = true;
				break;

			case TEXTURE_SELECTOR_WALL:
				texturePalette_Wall->m_bVisible = true;
				texturePalette_Floor->m_bVisible = false;
				break;
			};
		}
		else
		{
			tileTypeSelector->m_bVisible = false;
			tileHeightSelector->m_bVisible = false;
			textureSelectorState = TEXTURE_SELECTOR_HIDE;
			texturePalette_Wall->m_bVisible = false;
			texturePalette_Floor->m_bVisible = false;
		}




		// In editor mode, only update the selected object
		if(selected) selected->updateForEditor(deltaTime);




		// Call the event handler for left-click
		if(g_Input.MouseLeft)
		{
			onLeftMouseDown();
		}
		else
		{
			leftClickDebounce=false;
		}

		// Save / Load the game
		if(shouldSave)
		{
			world->saveToFile();
		}
		else if(shouldLoad)
		{
			world->loadFromFile();
		}
		else if(shouldNew)
		{
			createNewMap();
		}
	}
	else
	{// None of these should be visible in game mode

		toolBarTools->m_bVisible = false;
		toolBarMisc->m_bVisible = false;
		mousePosLabel->m_bVisible = false;

		objectPalette->m_bVisible = false;
		toolBarZone->m_bVisible = false;

		// Hide tile properties dialogs
		toolBarTilePropreties->m_bVisible = false;
		tileTypeSelector->m_bVisible = false;
		tileHeightSelector->m_bVisible = false;
		texturePalette_Wall->m_bVisible = false;
		texturePalette_Floor->m_bVisible = false;
		tileHeightSelector->m_bVisible = false;
	}

	// reset each tick
	shouldSave = false;
	shouldLoad = false;
	shouldNew = false;
}
Пример #25
0
static void cliDump(char *cmdline)
{
    (void)cmdline;
    unsigned int i, channel;
    char buf[16];
    float thr, roll, pitch, yaw;
    uint32_t mask;
    const clivalue_t *setval;

    cliVersion(NULL);
    printf("Current Config: Copy everything below here...\r\n");

    // print out aux switches
    cliAux("");

    // print out current motor mix
    printf("mixer %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]);

    // print custom mix if exists
    if (mcfg.customMixer[0].throttle != 0.0f) {
        for (i = 0; i < MAX_MOTORS; i++) {
            if (mcfg.customMixer[i].throttle == 0.0f)
                break;
            thr = mcfg.customMixer[i].throttle;
            roll = mcfg.customMixer[i].roll;
            pitch = mcfg.customMixer[i].pitch;
            yaw = mcfg.customMixer[i].yaw;
            printf("cmix %d", i + 1);
            if (thr < 0)
                printf(" ");
            printf("%s", ftoa(thr, buf));
            if (roll < 0)
                printf(" ");
            printf("%s", ftoa(roll, buf));
            if (pitch < 0)
                printf(" ");
            printf("%s", ftoa(pitch, buf));
            if (yaw < 0)
                printf(" ");
            printf("%s\r\n", ftoa(yaw, buf));
        }
        printf("cmix %d 0 0 0 0\r\n", i + 1);
    }

    // print custom servo mixer if exists
    if (mcfg.customServoMixer[0].rate != 0) {
        for (i = 0; i < MAX_SERVO_RULES; i++) {
            if (mcfg.customServoMixer[i].rate == 0)
                break;
            printf("smix %d ", i + 1);
            printf("%d ", mcfg.customServoMixer[i].targetChannel + 1);
            printf("%d ", mcfg.customServoMixer[i].fromChannel + 1);
            printf("%d ", mcfg.customServoMixer[i].rate);
            printf("%d ", mcfg.customServoMixer[i].speed);
            printf("%d ", mcfg.customServoMixer[i].min);
            printf("%d ", mcfg.customServoMixer[i].max);
            printf("%d\r\n", mcfg.customServoMixer[i].box);
        }
        printf("smix %d 0 0 0 0\r\n", i + 1);
    }

    // print servo directions
    for (i = 0; i < 8; i++)
        for (channel = 0; channel < INPUT_ITEMS; channel++)
            if (cfg.servoConf[i].direction & (1 << channel))
                printf("smix direction %d %d -1\r\n", i + 1 , channel + 1);

    // print enabled features
    mask = featureMask();
    for (i = 0; ; i++) { // disable all feature first
        if (featureNames[i] == NULL)
            break;
        printf("feature -%s\r\n", featureNames[i]);
    }
    for (i = 0; ; i++) {  // reenable what we want.
        if (featureNames[i] == NULL)
            break;
        if (mask & (1 << i))
            printf("feature %s\r\n", featureNames[i]);
    }

    // print RC MAPPING
    for (i = 0; i < 8; i++)
        buf[mcfg.rcmap[i]] = rcChannelLetters[i];
    buf[i] = '\0';
    printf("map %s\r\n", buf);

    // print settings
    for (i = 0; i < VALUE_COUNT; i++) {
        setval = &valueTable[i];
        printf("set %s = ", valueTable[i].name);
        cliPrintVar(setval, 0);
        cliPrint("\r\n");
    }
}
Пример #26
0
//--- DEBUG
void BaseRadio::CheckStatus ( PlayerBase player )
{
	string message = "Status: Is broadcasting? [" + ftoa ( this.IsBroadcasting() ) + "] --- Is receiving? [" + ftoa ( this.IsReceiving() ) + "]";
	player.MessageAction( message );	
}
Пример #27
0
void printflt(float s){ //print a float
	char buffer[20]; //need room for two longs and the .
	printstr(ftoa(s,buffer,3));
}
Пример #28
0
	WString ftow(double number)
	{
		return atow(ftoa(number));
	}
Пример #29
0
void showSensorsPage(void)
{
    uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
    static const char *format = "%s %5d %5d %5d";

    i2c_OLED_set_line(rowIndex++);
    i2c_OLED_send_string("        X     Y     Z");

    if (sensors(SENSOR_ACC)) {
        tfp_sprintf(lineBuffer, format, "ACC", accSmooth[X], accSmooth[Y], accSmooth[Z]);
        padLineBuffer();
        i2c_OLED_set_line(rowIndex++);
        i2c_OLED_send_string(lineBuffer);
    }

    if (sensors(SENSOR_GYRO)) {
        tfp_sprintf(lineBuffer, format, "GYR", gyroADC[X], gyroADC[Y], gyroADC[Z]);
        padLineBuffer();
        i2c_OLED_set_line(rowIndex++);
        i2c_OLED_send_string(lineBuffer);
    }

#ifdef MAG
    if (sensors(SENSOR_MAG)) {
        tfp_sprintf(lineBuffer, format, "MAG", magADC[X], magADC[Y], magADC[Z]);
        padLineBuffer();
        i2c_OLED_set_line(rowIndex++);
        i2c_OLED_send_string(lineBuffer);
    }
#endif

    tfp_sprintf(lineBuffer, format, "I&H", inclination.values.rollDeciDegrees, inclination.values.pitchDeciDegrees, heading);
    padLineBuffer();
    i2c_OLED_set_line(rowIndex++);
    i2c_OLED_send_string(lineBuffer);

    uint8_t length;

    ftoa(EstG.A[X], lineBuffer);
    length = strlen(lineBuffer);
    while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) {
        lineBuffer[length++] = ' ';
        lineBuffer[length+1] = 0;
    }
    ftoa(EstG.A[Y], lineBuffer + length);
    padLineBuffer();
    i2c_OLED_set_line(rowIndex++);
    i2c_OLED_send_string(lineBuffer);

    ftoa(EstG.A[Z], lineBuffer);
    length = strlen(lineBuffer);
    while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) {
        lineBuffer[length++] = ' ';
        lineBuffer[length+1] = 0;
    }
    ftoa(smallAngle, lineBuffer + length);
    padLineBuffer();
    i2c_OLED_set_line(rowIndex++);
    i2c_OLED_send_string(lineBuffer);

}
Пример #30
0
void gHistogram::draw(float value) {
	uint32_t l = millis()-_chrono;
	if ( (_lapse>0) && (l<_lapse) ) return;
	_chrono = millis();
	
	uint16_t y = _y0+1+_dy-2 - (uint16_t)( (value-_valueMin)*(_dy-2)/(_valueMax-_valueMin) );
	if (y<_y0+2)     y = _y0+2;
	if (y>_y0+_dy-4) y = _y0+_dy-4;
	
	// continuity management
	if (_continous) {
		// right-most side
		if (_n>_dx-4) {
			// move
			_pscreen->copyPaste(_x0+2, _y0, _x0+1, _y0, _dx-3, _dy);
			// new 
			_pscreen->line(_x0+_n+1, _y0+1, _x0+_n+1, _y0+_dy-2, _backColour);
			
			// gridY
			if (_gridY>0) { 
				if (_gridC==0) _pscreen->line(_x0+_n+1, _y0+1, _x0+_n+1, _y0+_dy-2, _pscreen->halfColour(_gridColour));
				_gridC++;
				_gridC %= _gridY;
			} // gridY
			
			// gridX
			if (_gridX>0) for (uint16_t i=1; i<_gridX; i++) 
				_pscreen->point(_x0+_n+1, _y0+map(i, 0, _gridX, 0, _dy), _pscreen->halfColour(_gridColour));
		} 
		else {
			_n++;
		} // right-most side
	} 
	else {
		// right 
		if (_n>_dx-4) {
			_n=1;
		} 
		else {
			_n++;
		} // right 
		
		_pscreen->line(_x0+_n+1, _y0+1, _x0+_n+1, _y0+_dy-2, _backColour);
		_pscreen->line(_x0+_n+2, _y0+1, _x0+_n+2, _y0+_dy-2, _gridColour);
		
		// gridY
		if (_gridY>0) { 
			if (_gridC==0) _pscreen->line(_x0+_n+1, _y0+1, _x0+_n+1, _y0+_dy-2, _pscreen->halfColour(_gridColour));
			_gridC++;
			_gridC %= _gridY;
		} // gridY
		
		// gridX
		if (_gridX>0) for (uint16_t i=1; i<_gridX; i++) 
			_pscreen->point(_x0+_n+1, _y0+map(i, 0, _gridX, 0, _dy), _pscreen->halfColour(_gridColour));
		
	} // continuity management
	
	// value
    _pscreen->setPenSolid(true);    
	_pscreen->dRectangle(_x0+_n, y, 2, 2, _valueColour);
	
	// min and max memory
	if (_memory>0) {
		
		// first time
		if (_n==0) {
			_max = y;
			_min = y;
		}
		
		// max - coordinates in reverse scale
		if (y<=_max) {
			_max = y;
			_amnesiaMax = _memory;
		} 
		else if (_amnesiaMax>0) {
			_pscreen->dRectangle(_x0+_n, _max, 2, 2, _maxColour);
			_amnesiaMax--;
		} 
		else {
			_max = y;
		} // max
		
		// min - coordinates in reverse scale
		if (y>=_min) {
			_min = y;
			_amnesiaMin = _memory;
		} 
		else if (_amnesiaMin>0) {  
			_pscreen->dRectangle(_x0+_n, _min, 2, 2, _minColour);
			_amnesiaMin--;
		} 
		else {
			_min = y;
		} // min
	} // min and max memory
	
	_pscreen->setFont(0);
	_pscreen->setFontSolid(true);
	_pscreen->gText(_x0+3, _y0+2, ftoa(_valueMax, 1, 0), _frontColour);
	_pscreen->gText(_x0+3, _y0+_dy-2-_pscreen->fontY(), ftoa(_valueMin, 1, 0), _frontColour);
	
	// lapse error
	if (_lapse>0) {
		if (l>_lapse) {
			if (_gridY>0) {
				_pscreen->setBackGroundColour(_frontColour);
				_pscreen->gText(_x0+_dx-2-7*_pscreen->fontX(), _y0+_dy-2-_pscreen->fontY(), ttoa(l*_gridY, 1, 7), _backColour);
				_pscreen->setBackGroundColour(_backColour);
			} 
			else {
				_pscreen->setBackGroundColour(_frontColour);
				_pscreen->gText(_x0+_dx-2-7*_pscreen->fontX(), _y0+_dy-2-_pscreen->fontY(), ttoa(l, 0, 7), _backColour);
				_pscreen->setBackGroundColour(_backColour);
			} // _gridY
			
		} 
		else {
			if (_gridY>0) {
				_pscreen->gText(_x0+_dx-2-7*_pscreen->fontX(), _y0+_dy-2-_pscreen->fontY(), ttoa(l*_gridY, 1, 7), _frontColour);
			} 
			else {
				_pscreen->gText(_x0+_dx-2-7*_pscreen->fontX(), _y0+_dy-2-_pscreen->fontY(), ttoa(l, 0, 7), _frontColour);
			} // _gridY
		} // lapse error
	}
	else {
		_pscreen->setBackGroundColour(_frontColour);
		_pscreen->gText(_x0+_dx-2-7*_pscreen->fontX(), _y0+_dy-2-_pscreen->fontY(), ttoa(l, 0, 7), _backColour);
		_pscreen->setBackGroundColour(_backColour);
	} // end _lapse>0
	
}