| 9,35 → 9,35 |
|---|
| |
| int16_t c_sin_8192(int16_t angle) |
| { |
| int8_t m,n; |
| int16_t sinus; |
| |
| // avoid negative angles |
| if (angle < 0) |
| { |
| m = -1; |
| angle = abs(angle); |
| } |
| else m = +1; |
| |
| // fold angle to intervall 0 to 359 |
| angle %= 360; |
| |
| // check quadrant |
| if (angle <= 90) n=1; // first quadrant |
| else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
| else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
| else {angle = 360 - angle; n = -1;} //fourth quadrant |
| // get lookup value |
| sinus = pgm_read_word(&pgm_sinlookup[angle]); |
| // calculate sinus value |
| return (sinus * m * n); |
| int8_t m,n; |
| int16_t sinus; |
| |
| // avoid negative angles |
| if (angle < 0) |
| { |
| m = -1; |
| angle = abs(angle); |
| } |
| else m = +1; |
| |
| // fold angle to intervall 0 to 359 |
| angle %= 360; |
| |
| // check quadrant |
| if (angle <= 90) n=1; // first quadrant |
| else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
| else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
| else {angle = 360 - angle; n = -1;} //fourth quadrant |
| // get lookup value |
| sinus = pgm_read_word(&pgm_sinlookup[angle]); |
| // calculate sinus value |
| return (sinus * m * n); |
| } |
| |
| // Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
| int16_t c_cos_8192(int16_t angle) |
| { |
| return (c_sin_8192(90 - angle)); |
| return (c_sin_8192(90 - angle)); |
| } |
| |
| // Arcustangens returns degree in a range of +/. 180 deg |
| 45,35 → 45,35 |
|---|
| |
| int16_t c_atan2(int16_t y, int16_t x) |
| { |
| int16_t index, angle; |
| int8_t m; |
| |
| if (!x && !y) return 0; //atan2(0, 0) is undefined |
| |
| if (y < 0) m = -1; |
| else m = 1; |
| |
| if (!x) return (90 * m); // atan2(y,0) = +/- 90 deg |
| |
| index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table |
| if (index < 0) index = -index; |
| |
| if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]); // lookup for 0 deg to 79 deg |
| else if (index > 7334) angle = 90; // limit is 90 deg |
| else if (index > 2444) angle = 89; // 89 deg to 80 deg is mapped via intervalls |
| else if (index > 1465) angle = 88; |
| else if (index > 1046) angle = 87; |
| else if (index > 813) angle = 86; |
| else if (index > 664) angle = 85; |
| else if (index > 561) angle = 84; |
| else if (index > 486) angle = 83; |
| else if (index > 428) angle = 82; |
| else if (index > 382) angle = 81; |
| else angle = 80; // (index>345) |
| |
| if (x > 0) return (angle * m); // 1st and 4th quadrant |
| else if ((x < 0) && (m > 0)) return (180 - angle); // 2nd quadrant |
| else return (angle - 180); // ( (x < 0) && (y < 0)) 3rd quadrant |
| int16_t index, angle; |
| int8_t m; |
| |
| if (!x && !y) return 0; //atan2(0, 0) is undefined |
| |
| if (y < 0) m = -1; |
| else m = 1; |
| |
| if (!x) return (90 * m); // atan2(y,0) = +/- 90 deg |
| |
| index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table |
| if (index < 0) index = -index; |
| |
| if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]); // lookup for 0 deg to 79 deg |
| else if (index > 7334) angle = 90; // limit is 90 deg |
| else if (index > 2444) angle = 89; // 89 deg to 80 deg is mapped via intervalls |
| else if (index > 1465) angle = 88; |
| else if (index > 1046) angle = 87; |
| else if (index > 813) angle = 86; |
| else if (index > 664) angle = 85; |
| else if (index > 561) angle = 84; |
| else if (index > 486) angle = 83; |
| else if (index > 428) angle = 82; |
| else if (index > 382) angle = 81; |
| else angle = 80; // (index>345) |
| |
| if (x > 0) return (angle * m); // 1st and 4th quadrant |
| else if ((x < 0) && (m > 0)) return (180 - angle); // 2nd quadrant |
| else return (angle - 180); // ( (x < 0) && (y < 0)) 3rd quadrant |
| } |
| |
| // Arcustangens returns degree in a range of +/. 180 deg |
| 81,39 → 81,39 |
|---|
| |
| int c_asin_8192(int y) |
| { |
| int index, m; |
| if (y < 0) |
| { |
| m = -1; |
| index = -y / 64; |
| } |
| else |
| { |
| m = 1; |
| index = y / 64; |
| } |
| |
| if (index > 127) |
| { |
| index = 127; |
| } |
| return(m * pgm_read_byte(&pgm_asinlookup[index])); |
| int index, m; |
| if (y < 0) |
| { |
| m = -1; |
| index = -y / 64; |
| } |
| else |
| { |
| m = 1; |
| index = y / 64; |
| } |
| |
| if (index > 127) |
| { |
| index = 127; |
| } |
| return(m * pgm_read_byte(&pgm_asinlookup[index])); |
| } |
| // integer square root |
| uint32_t c_sqrt(uint32_t number) |
| { |
| uint32_t s1, s2; |
| uint8_t iter = 0; |
| // initialization of iteration |
| s2 = number; |
| do // iterative formula to solve x^2 - n = 0 |
| { |
| s1 = s2; |
| s2 = number / s1; |
| s2 += s1; |
| s2 /= 2; |
| iter++; |
| //if(iter > 40) break; |
| }while( ( (s1-s2) > 1) && (iter < 40)); |
| return s2; |
| uint32_t s1, s2; |
| uint8_t iter = 0; |
| // initialization of iteration |
| s2 = number; |
| do // iterative formula to solve x^2 - n = 0 |
| { |
| s1 = s2; |
| s2 = number / s1; |
| s2 += s1; |
| s2 /= 2; |
| iter++; |
| //if(iter > 40) break; |
| }while( ( (s1-s2) > 1) && (iter < 40)); |
| return s2; |
| } |