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");
}
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;
}
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);
}
string FloatConstNode::stringValue(){
return ftoa( value );
}
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);
}
}
}
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;
}
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");
}
}
/* 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;
}
}
}
void lcd_putint(float x){
char res[64];
ftoa(x,res,4);
lcd_printf(res);
}
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");
}
}
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);
}
string CWFixedPhase::description()
//=======================================================================================
{
unsigned pad = max(0, 23 - (int)name().size());
return " tt_"+name()+string(pad, ' ') + " = "+ftoa(target, "7.0")+ " (dd)\n";
}
/******************************************************
函数：计算强度值
参数：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;
}
void vga_terminal_putfloat(double f) {
char* s = ftoa(f, 2);
printf("%s\n", s);
free(s);
}
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;
}
////////////////////////////////
}
}
/*
** 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);
}
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;
}
String String::ValueOf(float f, int digits)
{
return ftoa(f, digits);
}
void OLED::print(float f, int precision) {
char ret[10];
ftoa(f, ret, precision);
print(ret);
}
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);
}
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);
}
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;
}
////////////////////////////////
}
}
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();
}
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;
}
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");
}
}
//--- DEBUG
void BaseRadio::CheckStatus ( PlayerBase player )
{
string message = "Status: Is broadcasting? [" + ftoa ( this.IsBroadcasting() ) + "] --- Is receiving? [" + ftoa ( this.IsReceiving() ) + "]";
player.MessageAction( message );
}
void printflt(float s){ //print a float
char buffer[20]; //need room for two longs and the .
printstr(ftoa(s,buffer,3));
}
WString ftow(double number)
{
return atow(ftoa(number));
}
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);
}
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
}