void InitGyro(void)
{
// initialise A2D, needs a dry run to enable
A2D(1);
A2D(2);
InitFilter(A2D(A2D_CHANNEL_FRONT),A2D(A2D_CHANNEL_SIDE)); // front and left
AngleGyro = 0;
}
FlowScalar * D3dFlowContextGawsSide::Get_Adress(
bool l2r, // L2R or B2T?
int m, // M index
int n, // N index
GawsTerm term // get address of which term
)
{
FlowScalar * retVal = NULL ; // pointer to FlowScalarVar
FlowScalar * var = NULL ; // pointer to FlowScalarVar
MAPDBG_FUN3("D3dFlowContextGawsSide::Get_Adress");
switch ( term )
{
case GawsTerm_A: var = wrka1;
break;
case GawsTerm_B: var = wrka2;
break;
case GawsTerm_C: var = wrka3;
break;
case GawsTerm_D: var = wrka4;
break;
}
if (var != NULL)
{
switch( this->memType )
{
case Mem_Shared:
retVal = A2D(var,m,n);
break;
case Mem_Distributed:
int i;
for ( i = 0 ; ( i < this->numCommPoints ) && ( retVal == NULL ) ; i++ )
{
if ( ( this->commPointAdmins[i].m == m ) &&
( this->commPointAdmins[i].n == n ) &&
( this->commPointAdmins[i].l2r == l2r ) )
{
retVal = var + i;
}
}
break;
}
}
FLOW2D3D->dd->log->Write (Log::DDMAPPER_MINOR, "D3dFlowContextGawsSide::Get_Adress for %d in \"%s\"(%s %2d,%2d): %x, value %12.5f",
term, this->flowIterator->name, (l2r ? "L2R" : "T2B"),
m, n, retVal, ( retVal == NULL ) ? 0.0 : *retVal );
return retVal;
}
void MeasureGyro(void)
{
static int pt=0;
// is there a channel 0 ? this could be pin 62
AddFilterData(A2D(A2D_CHANNEL_FRONT) ,A2D(A2D_CHANNEL_SIDE));
// Acc should now contain acceleratio between -512 to +512 for 0 to 5V where 2.55 V is 0
// instead of taking A2DData directly, a lowpass filter should be applied to A2DData
// A2DAccFiltered = Filter(); Acc = (A2DAccFiltered - A2D_Neutral...
int OmegaGyro = (A2DData[NOW][A2D_FRONT] - A2D_Neutral[A2D_FRONT]);
// amplify noise , oh sorry i meant integrate 2 times to get position
AngleGyro = AngleGyro + OmegaGyro;
// Maybe there should be a moving average on the Angle
pt++;
if(pt==10){
pt=0;
printf("gr=%i\n",AngleGyro);
}
}
int charger_init(CHARGER& charger){
digitalWrite(P_SW, LOW);
analogWrite(P_PWM, MIN_PWM);
charger.fb=charger.vfb=charger.avg_vfb=charger.dfb_v=charger.ifb=charger.dfb_i=0.0;
//
// setpoint value
charger.sp=A2D(V_BATT);
//
// input value (charger.dfb_v is the output)
charger.pwm=charger.open_pwm=MIN_PWM;
pid_init(pid, MIN_PWM, MAX_PWM, aKp, aKi, aKd);
}
//
// main loop, charging
// ===================
// During all loop (fast & constant charge) the charger check
// the temperature and if battery is hot unplugged
//
// 2)If OK control proceeds to the Constant Current loop where the PWM signal
// on-time is adjusted up or down until the measured current matches the
// specified fast charge current.
// -> The charge LED flashes quickly during this mode.
//
// 3)When the Maximum Charge Voltage setting is reached then the
// mode switches to Constant Voltage and a new loop is entered that uses
// the same method as above to control voltage at the Maximum Charge
// Voltage point.
// -> The LED now flashes more slowly.
//
// This continues until 30 minutes after the charge current, which is
// decreasing with time in constant voltage mode, reaches the minimum
// (set at 50 mA per 1600 mAH of capacity).
// ->The charger then shuts off and the LED goes on steady.
//
//
int charger_mainloop(CHARGER& charger){
long charging=1, count=0, constant_voltage=false;
double p_norm=P_NORM, i_norm=I_NORM;
//
// one loop is about (on arduino 16Mhz)
// - 920us, 1087Hz (1x avg_read(3 iter) without Serial.print)
// - 1.8ms, 555Hz (2x avg_read without Serial.print=
// - 3.3ms, 306Hz (only with Serial.print)
while(charging){
digitalWrite(P_SW,HIGH);
// only peak temp every (1.8 - 5.0 ms *1000 => ~2-5 seconds )
if((charging++)%400==0){
//charger.temp=avr_internal_temp();
charging=1;
if (constant_voltage)Serial.print("CST ");
Serial.print("INFO V: ");Serial.print(charger.vfb*V_FACTOR,2);
Serial.print(" PWM: ");Serial.print((int)(charger.pwm));
//Serial.print(" SP: ");Serial.print(A2D(V_BATT));
Serial.print(" TEMP: ");Serial.print(charger.temp);
Serial.print(" FB: ");Serial.print((int)(charger.fb));
Serial.print(" ERROR: ");Serial.print((int)(pid.e));
Serial.print(" P: ");Serial.println(charger.ifb*I_FACTOR*charger.vfb*V_FACTOR,2);
}
//
// read voltage feedback
charger.vfb=avg_read(A_VFB,charger.dfb_v);
charger.avg_vfb=charger.avg_vfb*.9+charger.vfb*.1;
charger.ifb=avg_read(A_IFB,charger.dfb_i);
#if 1
//
// detect over temperature
if(charger.temp>TEMP_MAX){
#ifdef CONFIG_WITH_PRINT
Serial.print("OVER TEMP: ");Serial.println(charger.temp,2);
#endif
charger_reset(charger,MIN_PWM);
while(true);
continue;
}
//
// check mosfet
charger_check_mosfet(charger);
#ifdef CONFIG_WITH_PRINT
//
// detect offline (FOR DEBUG ONLY)
if ((charger.vfb)<=V_IN && (charger.ifb<I_BATT_CHARGED) && count++>300){
Serial.print("OFF : ");
Serial.print(charger.vfb*V_FACTOR,2);
Serial.print(" PWM: ");
Serial.println((int)charger.pwm);
// re init pwm
charger_reset(charger,MIN_PWM);
delay(TIMER_WAIT);
return false;
}
#endif
//
// security trigger on over voltage
if (charger.vfb>O_V){
#ifdef CONFIG_WITH_PRINT
Serial.print("OVER V: ");Serial.print(charger.vfb*V_FACTOR,2);
Serial.print(" PWM: ");Serial.print((int)(charger.pwm));
Serial.print(" I: ");Serial.println(charger.ifb*I_FACTOR,2);
#endif
charger_reset(charger,MIN_PWM);
delay(TIMER_WAIT*3);
continue;
}
//
// security trigger on over current
if ((charger.ifb*charger.vfb)>(P_MAX*1.1)){
#ifdef CONFIG_WITH_PRINT
Serial.print("OVER P: ");Serial.print(charger.ifb*I_FACTOR*charger.vfb*V_FACTOR,2);
Serial.print(" PWM: ");Serial.print((int)(charger.pwm));
Serial.print(" V: ");Serial.println(charger.vfb*V_FACTOR,2);
#endif
charger_reset(charger,MIN_PWM);
delay(TIMER_WAIT*3);
continue;
}
//
// check if cherging is done
if (abs(charger.vfb-V_BATT)<=0.1 && charger.ifb<=I_BATT_CHARGED && charger.ifb>0.01){
#ifdef CONFIG_WITH_PRINT
Serial.print("CHARGED: V=");
Serial.print(charger.vfb*V_FACTOR,2);
Serial.print(" I=");Serial.println(charger.ifb*I_FACTOR,2);
#endif
charger_reset(charger,MIN_PWM);
while(true);
}
#endif
//
// limit on voltage for constant voltage
if ((V_BATT-charger.avg_vfb)<=0.01 || constant_voltage){
constant_voltage=true;
charger.pwm=pid_update(pid,charger.sp,charger.dfb_v);
}else{
//
// limit on power for fast charge
charger.fb=A2D(charger.ifb*charger.vfb);//charger.dfb_v;
charger.pwm=pid_update(pid,A2D(P_MAX),charger.fb);
}
analogWrite(P_PWM, (int)(charger.pwm) );
}
charger_reset(charger,MIN_PWM);
return true;
}
/*==========================================================================*/
BInt4 Get_element ( BInt4 set ,
BText grp_name ,
BText elm_name ,
BInt4 * usr_index ,
BInt4 * usr_order ,
BUInt4 buffer_length,
BData buffer )
{
BUInt4 array_offset[MAX_DIM];
BUInt8 cel_num_bytes ;
BUInt8 cel_pointer;
BText ch_buffer ;
BUInt8 conv_bytes ;
BText cp ;
BInt4 dat_fds ;
BUInt4 element_offset = 0;
BInt4 elm_dimens [ MAX_DIM ];
BUInt4 elm_num_dim ;
BUInt4 elm_single_bytes;
BChar elm_type [ MAX_TYPE+1 ];
BUInt4 file_offset[MAX_DIM];
BInt4 from_xdr ;
BInt4 grp_dimens [MAX_DIM];
BUInt4 grp_num_dim ;
BInt4 grp_order [MAX_DIM];
BUInt8 grp_pointer;
BUInt4 i, j, k, l, m, n, v;
BUInt4 j1=1, j2=1, j3=1;
BUInt8 j_array_offset;
BUInt8 j_file_offset;
BUInt4 k1=1, k2=1, k3=1;
BUInt8 k_array_offset;
BUInt8 k_file_offset;
BUInt4 l1=1, l2=1, l3=1;
BUInt8 l_array_offset;
BUInt8 l_file_offset;
BUInt8 l_pointer;
BUInt4 m1=1, m2=1, m3=1;
BUInt8 m_array_offset;
BUInt8 m_file_offset;
BUInt4 n1=1, n2=1, n3=1;
BUInt8 n_array_offset = ULONG_MAX;
BUInt8 n_file_offset;
BUInt8 read_bytes;
BInt4 size_dat_buf_on_file = SIZE_DAT_BUF;
BInt4 usr_array [MAX_DIM];
BUInt4 usr_offset [MAX_DIM];
BUInt4 v1=1, v2=1, v3=1;
BInt4 var_dim = FALSE ;
BUInt8 var_file_offset = 0;
BInt4 var_num_dim = -1;
BData vp ;
elm_type[MAX_TYPE]='\0';
if ( nefis[set].one_file == TRUE )
{
dat_fds = nefis[set].daf_fds;
}
else
{
dat_fds = nefis[set].dat_fds;
}
if ( nefis[set].file_version == Version_1 )
{
size_dat_buf_on_file -= 3 * SIZE_BINT4;
}
/*
* Retrieve information about group and element
*/
nefis_errno = RT_retrieve ( set , grp_name , elm_name ,
&cel_num_bytes , (BUInt4 *) elm_dimens ,&elm_num_dim ,
&element_offset ,&elm_single_bytes, elm_type ,
(BUInt4 *) grp_dimens ,&grp_num_dim , (BUInt4 *) grp_order ,
&grp_pointer ,&read_bytes );
if ( nefis_errno != 0 )
{
return nefis_errno;
}
/*
* Check if user supplied indexes are within the range (group)
*/
for ( i=0; i<grp_num_dim; i++ )
{
if ( usr_index[A2D(i,0)] > usr_index[A2D(i,1)] )
{
nefis_errcnt += 1 ;
nefis_errno = 3001;
sprintf(error_text,
"Start value user index [%ld,2]=%ld should be smaller than [%ld,2]=%ld\n",
i,usr_index[A2D(i,0)], i,usr_index[A2D(i,1)] );
}
if ( usr_index[A2D(i,2)] < 1 )
{
nefis_errcnt += 1 ;
nefis_errno = 3002;
sprintf(error_text,
"Increment value user index [%ld,2]=%ld should be greater than 0\n",
i,usr_index[A2D(i,2)] );
}
if ( usr_index[A2D(i,0)] < 1 )
{
nefis_errcnt += 1 ;
nefis_errno = 3003;
sprintf(error_text,
"Start value user index [%ld,0]=%ld should be greater than zero\n",
i,usr_index[A2D(i,0)] );
}
if ( grp_dimens[ usr_order[i]-1 ] > 0 )
{
if ( usr_index[A2D(i,1)] > grp_dimens[ usr_order[i]-1 ] )
{
nefis_errcnt += 1 ;
nefis_errno = 3004;
sprintf(error_text,
"Stop value %ld should be smaller than %ld\n",
usr_index[A2D(i,1)], grp_dimens[ usr_order[i]-1 ] );
}
}
}
for ( i=grp_num_dim; i<MAX_DIM; i++ )
{
usr_index[A2D(i,0)] = 1;
usr_index[A2D(i,1)] = 1;
usr_index[A2D(i,2)] = 1;
grp_dimens[i] = 1;
usr_order [i] = i+1;
grp_order [i] = i+1;
}
for ( i=0; i<grp_num_dim; i++ )
{
if ( grp_dimens[ usr_order[i]-1 ] == 0 )
{
var_dim = TRUE;
var_num_dim = i ;
grp_dimens[ usr_order[i]-1 ] = 1;
}
}
if ( nefis_errno != 0 )
{
return nefis_errno;
}
for ( i=0; i<MAX_DIM; i++ )
{
usr_array[ usr_order[i]-1 ] = i+1;
}
usr_offset[0] = 1;
for ( i=1; i<MAX_DIM; i++ )
{
usr_offset[i] = (BUInt4)
((usr_index[A2D(i-1,1)] - usr_index[A2D(i-1,0)] +
usr_index[A2D(i-1,2)]) / usr_index[A2D(i-1,2)]) *
usr_offset[i-1];
}
for ( i=0; i<MAX_DIM; i++ )
{
array_offset[i] = usr_offset[ usr_array[ grp_order[i]-1 ]-1 ];
}
file_offset[0] = 1;
for ( i=1; i<MAX_DIM; i++ )
{
file_offset[i] = (BUInt4)grp_dimens[ grp_order[i-1]-1 ] * file_offset[i-1];
}
/*
* Get elements from data file
*
* Usr_order contains which dimension runs fastest in view of data
* Grp_order contains which dimension runs fastest on data file
*/
if ( var_dim == TRUE )
{
v1 = usr_index[A2D(var_num_dim,0)];
v2 = usr_index[A2D(var_num_dim,1)];
v3 = usr_index[A2D(var_num_dim,2)];
usr_index [A2D(var_num_dim,0)] = 1;
usr_index [A2D(var_num_dim,1)] = 1;
usr_index [A2D(var_num_dim,2)] = 1;
}
if ( grp_num_dim >= 5 )
{
n1 = usr_index[ A2D(usr_array[ grp_order[4]-1 ]-1,0)];
n2 = usr_index[ A2D(usr_array[ grp_order[4]-1 ]-1,1)];
n3 = usr_index[ A2D(usr_array[ grp_order[4]-1 ]-1,2)];
}
if ( grp_num_dim >= 4 )
{
m1 = usr_index[ A2D(usr_array[ grp_order[3]-1 ]-1,0)];
m2 = usr_index[ A2D(usr_array[ grp_order[3]-1 ]-1,1)];
m3 = usr_index[ A2D(usr_array[ grp_order[3]-1 ]-1,2)];
}
if ( grp_num_dim >= 3 )
{
l1 = usr_index[ A2D(usr_array[ grp_order[2]-1 ]-1,0)];
l2 = usr_index[ A2D(usr_array[ grp_order[2]-1 ]-1,1)];
l3 = usr_index[ A2D(usr_array[ grp_order[2]-1 ]-1,2)];
}
if ( grp_num_dim >= 2 )
{
k1 = usr_index[ A2D(usr_array[ grp_order[1]-1 ]-1,0)];
k2 = usr_index[ A2D(usr_array[ grp_order[1]-1 ]-1,1)];
k3 = usr_index[ A2D(usr_array[ grp_order[1]-1 ]-1,2)];
}
if ( grp_num_dim >= 1 )
{
j1 = usr_index[ A2D(usr_array[ grp_order[0]-1 ]-1,0)];
j2 = usr_index[ A2D(usr_array[ grp_order[0]-1 ]-1,1)];
j3 = usr_index[ A2D(usr_array[ grp_order[0]-1 ]-1,2)];
}
conv_bytes = read_bytes *
(j2-j1+j3)/j3 *
(k2-k1+k3)/k3 *
(l2-l1+l3)/l3 *
(m2-m1+m3)/m3 *
(n2-n1+n3)/n3 *
(v2-v1+v3)/v3 ;
/*
* Create character array to read data from file
*/
if ( buffer_length < conv_bytes )
{
nefis_errcnt += 1 ;
nefis_errno = 3005;
sprintf(error_text,
"Buffer length too small, should be %I64u instead of %ld\nGroup \"%s\", element \"%s\"\n",
conv_bytes, buffer_length, grp_name, elm_name);
return nefis_errno;
}
ch_buffer = (BText) malloc( (size_t) conv_bytes * sizeof(BChar) );
for ( v=v1-1; v<v2; v=v+v3 )
{
if ( var_dim == TRUE )
{
nefis_errno = RT_retrieve_var( set , &grp_pointer ,
v , &var_file_offset);
if ( nefis_errno != 0 )
{
nefis_errcnt += 1 ;
nefis_errno = 3006;
sprintf(error_text,
"Variable dimension %ld not found for:\n group \"%s\", element \"%s\"\n",
v+1, grp_name, elm_name);
return nefis_errno;
}
}
n_array_offset = (BUInt8)((v-v1+v3)/v3)*usr_offset[var_num_dim];
for ( n=n1-1; n<n2; n=n+n3 )
{
n_file_offset = (BUInt8)n*file_offset[4];
m_array_offset = n_array_offset;
for ( m=m1-1; m<m2; m=m+m3 )
{
m_file_offset = n_file_offset + (BUInt8)m * file_offset[3];
l_array_offset = m_array_offset;
for ( l=l1-1; l<l2; l=l+l3 )
{
l_file_offset = m_file_offset + (BUInt8)l * file_offset[2];
k_array_offset = l_array_offset;
for ( k=k1-1; k<k2; k=k+k3 )
{
k_file_offset = l_file_offset + (BUInt8)k * file_offset[1];
j_array_offset = k_array_offset;
for ( j=j1-1; j<j2; j=j+j3 )
{
j_file_offset = k_file_offset + (BUInt8)j; /* file_offset[0] = 1 */
if ( var_dim == TRUE )
{
cel_pointer = var_file_offset +
cel_num_bytes * j_file_offset;
}
else
{
cel_pointer = grp_pointer + (BUInt8)size_dat_buf_on_file +
cel_num_bytes * j_file_offset;
}
/*
* j_file_offset = j +
* k * jmax +
* l * kmax * jmax +
* m * lmax * kmax * jmax +
* n * mmax * lmax * kmax * jmax
*/
l_pointer = cel_pointer + (BUInt8)element_offset;
(BVoid) GP_read_file( dat_fds,
ch_buffer + (BUInt4) (j_array_offset*read_bytes),
l_pointer, read_bytes);
j_array_offset += array_offset[0];
}
k_array_offset += array_offset[1];
}
l_array_offset += array_offset[2];
}
m_array_offset += array_offset[3];
}
n_array_offset += array_offset[4];
}
} /* end variable dimension loop */
if ( nefis[set].daf_neutral == TRUE ||
nefis[set].dat_neutral == TRUE )
{
from_xdr = 1;
vp = (BData) malloc ( sizeof(BChar) * (BUInt4) conv_bytes );
cp = (BText) ch_buffer;
nefis_errno =
convert_ieee( &vp, &cp, conv_bytes, elm_single_bytes, elm_type, from_xdr);
memcpy( buffer, vp, (BUInt4) conv_bytes );
free ( (BData) vp );
}
else
{
memcpy( buffer, ch_buffer, (BUInt4) conv_bytes );
}
free( (BData) ch_buffer );
return nefis_errno;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray*prhs[])
{
double angs[3];
const double *p_angs=mxGetPr(prhs[1]);
angs[0]=(double)p_angs[0];
angs[1]=(double)p_angs[1];
angs[2]=(double)p_angs[2];
Matrix1D<double> axis;
getMatrix1D(prhs[2],axis);
Matrix2D<double> A3D, A2D;
bool wrap = (bool)mxGetScalar(prhs[5]);
mwSize ndims = mxGetNumberOfDimensions(prhs[0]);
/*mode: 1 euler, 2 align_with_Z, 3 axis, 4 tform*/
switch ((int)mxGetScalar(prhs[3])) {
case 1:
Euler_angles2matrix(angs[0], angs[1], angs[2], A3D, true);
break;
case 2:
alignWithZ(axis,A3D);
break;
case 3:
rotation3DMatrix(angs[0], axis, A3D);
A2D.resize(3,3);
A2D(0,0)=A3D(0,0); A2D(0,1)=A3D(0,1); A2D(0,2)=A3D(0,2);
A2D(1,0)=A3D(1,0); A2D(1,1)=A3D(1,1); A2D(1,2)=A3D(1,2);
A2D(2,0)=A3D(2,0); A2D(2,1)=A3D(2,1); A2D(2,2)=A3D(2,2);
break;
case 4:
getMatrix2D(prhs[6],A2D);
A3D = A2D;
break;
}
if (ndims == 2)
{
Image<double> img, img_out;
getMatrix2D(prhs[0],img());
if (MAT_XSIZE(A2D) != 0)
{
applyGeometry(BSPLINE3,img_out(), img(), A2D, IS_NOT_INV, wrap);
setMatrix2D(img_out(),plhs[0]);
}
else
{
setMatrix2D(img(),plhs[0]);
}
}
else
{
Image<double> vol, vol_out;
getMatrix3D(prhs[0],vol());
applyGeometry(BSPLINE3, vol_out(), vol(), A3D, IS_NOT_INV, wrap);
setMatrix3D(vol_out(),plhs[0]);
}
}
