void WavePixels::run(Sign &sign, EffectData &data, uint8_t ci){
uint16_t angle = (millis() % cycleTime[ci])*UINT16_MAX/cycleTime[ci];
int16_t delta_hue = hueA[ci] * sin16(angle)/UINT16_MAX;
uint16_t seg_count = sign.segmentCount();
bool on = (ci == 0);
for(uint8_t i=0; i < seg_count; i++){
Segment *curr_seg = sign.segments[i];
if(curr_seg -> isOn == on){
curr_seg -> setColor(color[ci]);
delta_hue = -delta_hue;
int16_t hue_add = delta_hue;
uint8_t pixel_count = curr_seg -> pixelCount();
for(uint8_t j = 0; j < pixel_count; j++ ){
Pixel *currPixel = curr_seg -> pixels[j];
currPixel -> addHue16(hue_add);
hue_add += delta_hue;
}
}
}
}
void Clock::run(Sign &sign, uint8_t layer){
unsigned long time = millis();
if(time - lastUpdated < stepTime){ return; }
this -> off(sign);
lastUpdated = time;
sign.textChanged = true;
//Clock Math
uint8_t hour_per_day = (is24Hour ? 24 : 12);
uint8_t minutes_per = (isClock ? 60 : 100);
unsigned long dilation_period = dilationPeriodMinutes * 60 * 1000;
int32_t time_amp_sec = timeAmpMinutes * 60;
uint16_t angle = (millis() % dilation_period)*UINT16_MAX/dilation_period;
int16_t delta_time = time_amp_sec * sin16(angle)/UINT16_MAX;
unsigned long seconds = time/stepTime + minutesOffset*60 + hoursOffset*3600 + delta_time;
uint8_t minutes = (seconds/60) % minutes_per;
uint8_t hours = (seconds/3600) % hour_per_day;
uint8_t display[2];
if(isClock){
display[0] = minutes;
display[1] = hours + 1;
}else{
display[0] = seconds % 60;
display[1] = minutes;
}
uint8_t letterIdx = LETTERS_COUNT - 1;
char to_letter;
uint8_t dspl;
for(uint8_t set=0; set < 2; set++){
uint8_t devisor = 100;
uint8_t remainder = display[set] % devisor;
for(uint8_t ltr=2; ltr>0; ltr--){
dspl = remainder % 10;
if(dspl == 0 && letterIdx == 0){
to_letter = ' ';
}else{
to_letter = char(dspl + '0');
}
sign.letters[letterIdx] -> setChar(to_letter);
letterIdx--;
remainder /= 10;
}
}
}
/*------------------------------------------------------------------------
*
* PROTOTYPE : void V3XMatrix_Rotate_X_Local(int32_t Theta, V3XSCALAR *Matrice)
*
* DESCRIPTION :
*
*/
void V3XMatrix_Rotate_X_Local(int32_t Theta, V3XSCALAR *Matrice)
{
V3XSCALAR sin, cos;
V3XSCALAR M[9];
sin=(V3XSCALAR)sin16(Theta);
cos=(V3XSCALAR)cos16(Theta);
M[1]=MULF32(cos, Matrice[1])-MULF32(sin, Matrice[2]);
M[4]=MULF32(cos, Matrice[4])-MULF32(sin, Matrice[5]);
M[7]=MULF32(cos, Matrice[7])-MULF32(sin, Matrice[8]);
M[2]=MULF32(sin, Matrice[1])+MULF32(cos, Matrice[2]);
M[5]=MULF32(sin, Matrice[4])+MULF32(cos, Matrice[5]);
M[8]=MULF32(sin, Matrice[7])+MULF32(cos, Matrice[8]);
Matrice[1]=M[1]; Matrice[4]=M[4]; Matrice[7]=M[7];
Matrice[2]=M[2]; Matrice[5]=M[5]; Matrice[8]=M[8];
}
/*------------------------------------------------------------------------
*
* PROTOTYPE : void V3XMatrix_Rotate_Z(int32_t Theta, V3XSCALAR *Matrice)
*
* DESCRIPTION :
*
*/
void V3XMatrix_Rotate_Z(int32_t Theta, V3XSCALAR *Matrice)
{
V3XSCALAR sin, cos;
V3XSCALAR M[9];
sin=(V3XSCALAR)sin16(Theta);
cos=(V3XSCALAR)cos16(Theta);
M[0]=MULF32(cos, Matrice[0])-MULF32(sin, Matrice[3]);
M[1]=MULF32(cos, Matrice[1])-MULF32(sin, Matrice[4]);
M[2]=MULF32(cos, Matrice[2])-MULF32(sin, Matrice[5]);
M[3]=MULF32(sin, Matrice[0])+MULF32(cos, Matrice[3]);
M[4]=MULF32(sin, Matrice[1])+MULF32(cos, Matrice[4]);
M[5]=MULF32(sin, Matrice[2])+MULF32(cos, Matrice[5]);
Matrice[0]=M[0]; Matrice[1]=M[1]; Matrice[2]=M[2];
Matrice[3]=M[3]; Matrice[4]=M[4]; Matrice[5]=M[5];
}
/*------------------------------------------------------------------------
*
* PROTOTYPE : void V3XMatrix_Rotate_Y(int32_t Theta, V3XSCALAR *Matrice)
*
* DESCRIPTION :
*
*/
void V3XMatrix_Rotate_Y(int32_t Theta, V3XSCALAR *Matrice)
{
V3XSCALAR sin, cos;
V3XSCALAR M[9];
sin = (V3XSCALAR)sin16(Theta);
cos = (V3XSCALAR)cos16(Theta);
M[0]=MULF32(cos, Matrice[0])-MULF32(sin, Matrice[6]);
M[1]=MULF32(cos, Matrice[1])-MULF32(sin, Matrice[7]);
M[2]=MULF32(cos, Matrice[2])-MULF32(sin, Matrice[8]);
M[6]=MULF32(sin, Matrice[0])+MULF32(cos, Matrice[6]);
M[7]=MULF32(sin, Matrice[1])+MULF32(cos, Matrice[7]);
M[8]=MULF32(sin, Matrice[2])+MULF32(cos, Matrice[8]);
Matrice[0]=M[0]; Matrice[1]=M[1]; Matrice[2]=M[2];
Matrice[6]=M[6]; Matrice[7]=M[7]; Matrice[8]=M[8];
}
/*------------------------------------------------------------------------
*
* PROTOTYPE :
*
* DESCRIPTION :
*
*/
void V3XMatrix_Rotate_X(int32_t Theta, V3XSCALAR *Matrice)
{
V3XSCALAR sin, cos;
V3XSCALAR M[9];
sin = sin16(Theta);
cos = cos16(Theta);
M[3]=MULF32(cos, Matrice[3])-MULF32(sin, Matrice[6]);
M[4]=MULF32(cos, Matrice[4])-MULF32(sin, Matrice[7]);
M[5]=MULF32(cos, Matrice[5])-MULF32(sin, Matrice[8]);
M[6]=MULF32(sin, Matrice[3])+MULF32(cos, Matrice[6]);
M[7]=MULF32(sin, Matrice[4])+MULF32(cos, Matrice[7]);
M[8]=MULF32(sin, Matrice[5])+MULF32(cos, Matrice[8]);
Matrice[3]=M[3];
Matrice[4]=M[4];
Matrice[5]=M[5];
Matrice[6]=M[6];
Matrice[7]=M[7];
Matrice[8]=M[8];
}
bool ahrsUpdate(int16_t *yaw, int16_t *pitch, int16_t *roll){
int16_t axt, ayt, azt, mxt, myt, mzt;
int32_t ax, ay, az, mx, my, mz;
int32_t rollSin, rollCos, pitchSin, pitchCos;
int32_t tmpA, tmpB, tmpC, tmpD, tmpE;
// Read data from IMU and convert to plane based coordinate system.
// X forward, Y starbord, and Z down, but invert gravity vector as
// well so gravity is positive.
ahrsReadAcc(&axt, &ayt, &azt);
ahrsReadMag(&mxt, &myt, &mzt);
ax = -axt; ay = ayt; az = azt;
mx = mxt; my = -myt; mz = -mzt;
// Calculate the roll angle.
*roll = atan216(ay, az);
// Limit roll angle to -180 to 180.
if (*roll > TRIG16_CYCLE/2){
*roll -= TRIG16_CYCLE;
}
// Compute trig functions of roll.
rollSin = sin16(*roll);
rollCos = cos16(*roll);
// Calculate the pitch angle.
tmpA = (ay*rollSin)/TRIG16_ONE;
tmpB = (az*rollCos)/TRIG16_ONE;
*pitch = atan216(-ax, tmpA + tmpB);
// Limit pitch to -90 to +90
if (*pitch > TRIG16_CYCLE/4){
*pitch = TRIG16_CYCLE/2 - *pitch;
}
if (*pitch < -TRIG16_CYCLE/4){
*pitch = -TRIG16_CYCLE/2 - *pitch;
}
// Compute trig functions of pitch.
pitchSin = sin16(*pitch);
pitchCos = cos16(*pitch);
// Calculate the yaw angle.
tmpA = (mx*pitchCos)/TRIG16_ONE;
tmpB = (mz*pitchSin)/TRIG16_ONE;
tmpC = (((mz*rollSin)/TRIG16_ONE)*pitchCos)/TRIG16_ONE;
tmpD = (((mx*rollSin)/TRIG16_ONE)*pitchSin)/TRIG16_ONE;
tmpE = (my*rollCos)/TRIG16_ONE;
*yaw = atan216(tmpC - tmpD - tmpE, tmpA + tmpB);
// Check for validity of solution.
tmpA = (ax*ax)/IMU_ONE + (ay*ay)/IMU_ONE + (az*az)/IMU_ONE;
if (tmpA < IMU_ACC_MAG_MIN || IMU_ACC_MAG_MAX < tmpA){
return false; // possible errors
}
// Result is valid.
return true;
}
