I have a barometer (BMP180) and am interfacing it with aSTM32. I have to read the altitude which should have a precision of a foot.
While interfacing the barometer, I get a consistent real value of temperature but the pressure comes in the range of 59400 Pa (Pa - as per Bosch Datasheet). The example given in the datasheet also has pressure of 69964 Pa.
I have taken care of the delays required for the barometer to sample the data by using software based counters. Also, I checked my sensor with available libraries and everything comes consistent. But due to some academic reasons I can't use the libraries and so I need help in measuring the pressure.
Here is my STM32 Code:
//calibration params from the sensor
static int16_t AC1 = 8353;
static int16_t AC2 = -1041;
static int16_t AC3 = -14621;
static uint16_t AC4 = 33880;
static uint16_t AC5 = 25158;
static uint16_t AC6 = 18608;
static int16_t B1 = 6515;
static int16_t B2 = 35;
static int16_t MB = -32768;
static int16_t MC = -11786;
static int16_t MD = 2724;
static int TMPCNT = 0, PRSCNT=0; //loop counters
static double temperature, pressure;
static uint8_t oss = 3; // bmp high resolution
static long UT, X1, X2, B5, UP;
int main() {
while (1) {
if (TMPCNT == 2) { //2.7*2 > 5ms so sample temp every 5 ms
MSR_TEMP(); //measure for previous request
RQST_TEMP(); // request again
TMPCNT = 0;
}
TMPCNT++;
if (PRSCNT == 10) { //sample pressure every 27ms (25ms wait time for UH resolution)
MSR_PRESS();
RQST_PRESS();
PRSCNT = 0;
}
PRSCNT++;
/* some more code which takes 2.7 milliseconds */
}
}
void RQST_TEMP() {
i2c_transmit(BMP, 0xF4, 0x2E); // transmit 0x2E to 0xF4 reg of BMP addr
}
void MSR_TEMP() {
char buff[2];
requestData(BMP, 0xF6, buff, 2); //request 2 bytes from BMP addr, reg 0xF6, and put it in the buff
UT = (buff[0] << 8 | buff[1]);
X1 = (long)((UT - AC6) * AC5 * pow(2, -15));
X2 = (long)(MC * pow(2, 11) / (X1 + MD));
B5 = X1 + X2;
temperature = ((B5 + 8) * pow(2, -4)) / 10.0;
}
void RQST_PRESS(){
i2c_transmit(BMP, 0xF4, (char)(0x34 + (oss << 6)));
}
void MSR_PRESS() {
long B6,x1,x2,x3,B3,p;
unsigned long B4,B7;
char buff[3];
requestData(BMP, 0xF6, buff, 3);
UP = ((buff[0] << 16) + (buff[1] << 8) + buff[2]) >> (8-oss);
B6 = B5 - 4000;
x1 = (long)((B2 * (B6 * B6 * pow(2, -12))) * pow(2, -11));
x2 = (long)(AC2 * B6 * pow(2, -11));
x3 = x1 + x2;
B3 = ((((long)AC1 * 4 + x3) << oss) + 2) / 4;
x1 = AC3 * B6 * pow(2, -13);
x2 = (B1 * (B6 * B6 * pow(2, -12))) * pow(2, -16);
x3 = ((x1 + x2) + 2) * pow(2, -2);
B4 = AC4 * (unsigned long)(x3 + 32768) * pow(2, -15);
B7 = ((unsigned long)UP - B3) * (50000 >> oss);
if (B7 < 0x80000000) {
p = (B7 * 2) / B4;
} else {
p = (B7 / B4) * 2;
}
x1 = (p * pow(2, -8)) * (p * pow(2, -8));
x1 = (x1 * 3038) * pow(2, -16);
x2 = (-7357 * p) * pow(2, -16);
pressure = p + ((x1 + x2 + 3791) * pow(2, -4)); // pressure is in Pa as per data sheet
// SerialPrint("%lf\n",pressure);
}
The code written is based on the algorithm mentioned in the datasheet. Here is the datasheet. I dont know what I am doing wrong. My location should have pressure of 101209Pa which is way far from what I am getting. My I2C driver functions are also working as they should. So I cant figure out where exactly the problem is. I'll surely appreciate some help.
Related
I am trying to run a code that has a GUI built with pyglet.
but it gives this error. I have searched and found that I need to directly set the version of GLSL to be used by the code but I don't know how. would be happy if you helped me out with it.
b"0:20(27): error: cannot initialize uniform weight in GLSL 1.10 (GLSL 1.20 required)\n0:20(27): error: array constructors forbidden in GLSL 1.10 (GLSL 1.20 or GLSL ES 3.00 required)\n0:20(27): error: initializer of uniform variable `weight' must be a constant expression\n0:79(17): error: could not implicitly convert operands to arithmetic operator\n0:79(16): error: operands to arithmetic operators must be numeric\n0:89(7): warning: `coeff' used uninitialized\n"
this is the shader.py file:
Update
added the glsl file with the uniform weight in it
#ifdef GL_ES
precision highp float;
#endif
uniform sampler2D inlet;
uniform sampler2D disp0;
uniform float sigma;
uniform bool xmirror;
uniform vec3 colors[9];
uniform float streams[9];
/*mat4 gaussm = mat4(0.00000067, 0.00002292, 0.00019117, 0.00038771,
0.00002292, 0.00078634, 0.00655965, 0.01330373,
0.00019117, 0.00655965, 0.05472157, 0.11098164,
0.00038771, 0.01330373, 0.11098164, 0.22508352);*/
uniform float weight[5] = float[](0.2270270270, 0.1945946, 0.1216216216, 0.0540540541, 0.0162162162);
vec4 sample(vec2 p)
{
vec4 col;
if(streams[0] >= 0.)
{
int stream = 0;
for(int i=0;streams[stream] < min(p.x, 1.) && stream < 8;stream++) { }
col = vec4(colors[stream], 1.);
}
else {
col = texture2D(inlet, p);
}
return col;
}
float gaussian(float d, float s)
{
float pi = 3.141592653;
//return exp(- d*d / (4.0 * pi * s * s));
//return pow(4*pi*s*s, 0.5)*exp(- d*d / (4.0 * pi * s * s));
return exp(- d*d / (2.*s*s));
}
float gaussian2(float d, float s)
{
float pi = 3.141592653;
float c = pow(1.0 / (4.0 * pi * s), 0.5);
float e = -1.0*(d * d) / (4.0 * s);
return c * exp(e);
}
float gaussf(int i, int j,float nf, float s)
{
//return gaussm[i][j];
float fi = float(i)/nf;
float jf = float(j)/nf;
return gaussian2(sqrt(fi*fi+jf*jf), s);
}
float cosh(float x)
{
return (exp(x)+exp(-x))/2.;
}
float rect_calc(vec2 d)
{
float pi = 3.141592653;
float AR = 0.25;
float offset = 0.125;
float m = 155.;
float n = 155.;
vec3 xyz = vec3(0., (d.x/1.), (d.y/8. + offset));
float u = 0.;
float coeff = (16 * pow(AR, 2.)) / pow(pi, 4.);
float num;
float den;
for(float i = 1.; i <= n; i += 2.)
{
for(float j = 1.; j <= m; j += 2.)
{
num = sin(i * pi * xyz.y) * sin(j * pi * ((xyz.z)/AR));
den = i * j * (pow(AR, 2.) * pow(AR, 2.) + pow(j, 2.));
u += coeff * (num / den);
}
}
// Convert velocity to time-of-flight
float L = 2.0;
float u_mean = 0.0043;
float u_norm = u/u_mean;
return L / u_norm;
}
void main()
{
vec2 uv = gl_TexCoord[0].st;
if(xmirror)
{
uv.x = 1.-uv.x;
}
vec2 d = texture2D(disp0, uv).yz * vec2(1.,8.);
if(xmirror)
{
d.x = -d.x;
uv.x = 1.-uv.x;
}
vec2 p = uv + d;
if(sigma <= 0.)
{
gl_FragColor = sample(p);
} else {
//Sample
vec4 c = vec4(0.);
float Dt = sigma*rect_calc(uv.xy);
float s = pow(Dt, 0.5);
float s2 = 1.0;
float t = 0.;
int ni = 8;
float n = 8.;
for(int ii = 0; ii < ni-1; ii++)
{
float i = float(ii);
for(int jj = 0; jj < ni-1; jj++)
{
float j = float(jj);
t += gaussf(ii,jj,n-1.,s2)*4.;
c += gaussf(ii,jj,n-1.,s2) * (sample(p + vec2((n-1.-i)*s, (n-1.-j)*s)) + sample(p + vec2(-(n-1.-i)*s, (n-1.-j)*s)) + sample(p + vec2(-(n-1.-i)*s, -(n-1.-j)*s)) + sample(p + vec2((n-1.-i)*s, -(n-1.-j)*s)));
}
t += gaussf(ii,ni-1,n-1.,s2)*4.;
c += gaussf(ii,ni-1,n-1.,s2) * (sample(p + vec2((n-1.-i)*s, 0.)) + sample(p + vec2(-(n-1.-i)*s, 0.))+ sample(p + vec2(0., (n-1.-i)*s))+ sample(p + vec2(0., -(n-1.-i)*s)));
}
t += gaussf(ni-1,ni-1,n-1.,s2);
c += gaussf(ni-1,ni-1,n-1.,s2) * sample(p);
//gl_FragColor = c;
gl_FragColor = c/t;
//gl_FragColor = (sigma*rect_calcu(uv.xy))*c/t;
}
}
well it got solved!
just needed to add the directive #version 120 at the beginning of the shader like this:
#version 120
#ifdef GL_ES
precision highp float;
#endif
uniform sampler2D inlet;
uniform sampler2D disp0;
uniform float sigma;
uniform bool xmirror;
uniform vec3 colors[9];
uniform float streams[9];
/*mat4 gaussm = mat4(0.00000067, 0.00002292, 0.00019117, 0.00038771,
0.00002292, 0.00078634, 0.00655965, 0.01330373,
0.00019117, 0.00655965, 0.05472157, 0.11098164,
0.00038771, 0.01330373, 0.11098164, 0.22508352);*/
uniform float weight[5] = float[](0.2270270270, 0.1945946, 0.1216216216, 0.0540540541, 0.0162162162);
vec4 sample(vec2 p)
{
vec4 col;
if(streams[0] >= 0.)
{
int stream = 0;
for(int i=0;streams[stream] < min(p.x, 1.) && stream < 8;stream++) { }
col = vec4(colors[stream], 1.);
}
else {
col = texture2D(inlet, p);
}
return col;
}
float gaussian(float d, float s)
{
float pi = 3.141592653;
//return exp(- d*d / (4.0 * pi * s * s));
//return pow(4*pi*s*s, 0.5)*exp(- d*d / (4.0 * pi * s * s));
return exp(- d*d / (2.*s*s));
}
float gaussian2(float d, float s)
{
float pi = 3.141592653;
float c = pow(1.0 / (4.0 * pi * s), 0.5);
float e = -1.0*(d * d) / (4.0 * s);
return c * exp(e);
}
float gaussf(int i, int j,float nf, float s)
{
//return gaussm[i][j];
float fi = float(i)/nf;
float jf = float(j)/nf;
return gaussian2(sqrt(fi*fi+jf*jf), s);
}
float cosh(float x)
{
return (exp(x)+exp(-x))/2.;
}
float rect_calc(vec2 d)
{
float pi = 3.141592653;
float AR = 0.25;
float offset = 0.125;
float m = 155.;
float n = 155.;
vec3 xyz = vec3(0., (d.x/1.), (d.y/8. + offset));
float u = 0.;
float coeff = (16 * pow(AR, 2.)) / pow(pi, 4.);
float num;
float den;
for(float i = 1.; i <= n; i += 2.)
{
for(float j = 1.; j <= m; j += 2.)
{
num = sin(i * pi * xyz.y) * sin(j * pi * ((xyz.z)/AR));
den = i * j * (pow(AR, 2.) * pow(AR, 2.) + pow(j, 2.));
u += coeff * (num / den);
}
}
// Convert velocity to time-of-flight
float L = 2.0;
float u_mean = 0.0043;
float u_norm = u/u_mean;
return L / u_norm;
}
void main()
{
vec2 uv = gl_TexCoord[0].st;
if(xmirror)
{
uv.x = 1.-uv.x;
}
vec2 d = texture2D(disp0, uv).yz * vec2(1.,8.);
if(xmirror)
{
d.x = -d.x;
uv.x = 1.-uv.x;
}
vec2 p = uv + d;
if(sigma <= 0.)
{
gl_FragColor = sample(p);
} else {
//Sample
vec4 c = vec4(0.);
float Dt = sigma*rect_calc(uv.xy);
float s = pow(Dt, 0.5);
float s2 = 1.0;
float t = 0.;
int ni = 8;
float n = 8.;
for(int ii = 0; ii < ni-1; ii++)
{
float i = float(ii);
for(int jj = 0; jj < ni-1; jj++)
{
float j = float(jj);
t += gaussf(ii,jj,n-1.,s2)*4.;
c += gaussf(ii,jj,n-1.,s2) * (sample(p + vec2((n-1.-i)*s, (n-1.-j)*s)) + sample(p + vec2(-(n-1.-i)*s, (n-1.-j)*s)) + sample(p + vec2(-(n-1.-i)*s, -(n-1.-j)*s)) + sample(p + vec2((n-1.-i)*s, -(n-1.-j)*s)));
}
t += gaussf(ii,ni-1,n-1.,s2)*4.;
c += gaussf(ii,ni-1,n-1.,s2) * (sample(p + vec2((n-1.-i)*s, 0.)) + sample(p + vec2(-(n-1.-i)*s, 0.))+ sample(p + vec2(0., (n-1.-i)*s))+ sample(p + vec2(0., -(n-1.-i)*s)));
}
t += gaussf(ni-1,ni-1,n-1.,s2);
c += gaussf(ni-1,ni-1,n-1.,s2) * sample(p);
//gl_FragColor = c;
gl_FragColor = c/t;
//gl_FragColor = (sigma*rect_calcu(uv.xy))*c/t;
}
}
Closed. This question needs debugging details. It is not currently accepting answers.
Edit the question to include desired behavior, a specific problem or error, and the shortest code necessary to reproduce the problem. This will help others answer the question.
Closed 1 year ago.
Improve this question
I am working to implement CUDA for the following code. The first version has been written serially and the second version is written with CUDA. I am sure about its results in serial version. I expect that the second version that I have added CUDA functionality also give me the same result, but it seems that kernel function does not do any thing and it gives me the initial value of u and v. I know due to lack of my experience, the bug may be obvious, but I cannot figure it out. Also, please do not recommend using flatten array, because it is harder for me to understand the indexing in code.
First version:
#include <fstream>
#include <iostream>
#include <math.h>
#include <vector>
#include <chrono>
#include <omp.h>
using namespace std;
const int M = 1024;
const int N = 1024;
const double A = 1;
const double B = 3;
const double Du = 5 * pow(10, -5);
const double Dv = 5 * pow(10, -6);
const int Max_Itr = 1000;
const double h = 1.0 / static_cast<double>(M - 1);
const double delta_t = 0.0025;
const double s1 = (Du * delta_t) / pow(h, 2);
const double s2 = (Dv * delta_t) / pow(h, 2);
int main() {
double** u=new double* [M];
double** v=new double* [M];
for (int i=0; i<M; i++){
u[i]=new double [N];
v[i]=new double [N];
}
for (int j = 0; j < M; j++) {
for (int i = 0; i < N;i++) {
u[i][j]=0.02;
v[i][j]=0.02;
}
}
for (int k = 1; k < Max_Itr; k++) {
for (int i = 1; i < N - 1; i++) {
for (int j = 1; j < M - 1; j++) {
u[i][j] = ((1 - (4 * s1)) * u[i][j]) + (s1 * (u[i + 1][j] + u[i - 1][j] + u[i][j + 1] + u[i][j - 1])) +
(A * delta_t) + (delta_t * pow(u[i][j], 2) * v[i][j]) - (delta_t * (B + 1) * u[i][j]);
v[i][j] = ((1 - (4 * s2)) * v[i][j]) + (s2 * (v[i + 1][j] + v[i - 1][j] + v[i][j + 1] + v[i][j - 1])) + (B * delta_t * u[i][j])
- (delta_t * pow(u[i][j], 2) * v[i][j]);
}
}
}
cout<<"u: "<<u[512][512]<<" v: "<<v[512][512]<<endl;
return 0;
}
Second version:
#include <fstream>
#include <iostream>
#include <math.h>
#include <vector>
using namespace std;
#define M 1024
#define N 1024
__global__ void my_kernel(double** v, double** u){
int i= blockIdx.y * blockDim.y + threadIdx.y;
int j= blockIdx.x * blockDim.x + threadIdx.x;
double A = 1;
double B = 3;
int Max_Itr = 1000;
double delta_t = 0.0025;
double Du = 5 * powf(10, -5);
double Dv = 5 * powf(10, -6);
double h = 1.0 / (M - 1);
double s1 = (Du * delta_t) / powf(h, 2);
double s2 = (Dv * delta_t) / powf(h, 2);
for (int k = 1; k < Max_Itr; k++) {
u[i][j] = ((1 - (4 * s1))
* u[i][j]) + (s1 * (u[i + 1][j] + u[i - 1][j] + u[i][j + 1] + u[i][j - 1])) +
(A * delta_t) + (delta_t * pow(u[i][j], 2) * v[i][j]) - (delta_t * (B + 1) * u[i][j]);
v[i][j] = ((1 - (4 * s2))
* v[i][j]) + (s2 * (v[i + 1][j] + v[i - 1][j] + v[i][j + 1] + v[i][j - 1])) + (B * delta_t * u[i][j])
- (delta_t * pow(u[i][j], 2) * v[i][j]);
__syncthreads();
}
}
int main() {
double** u=new double* [M];
double** v=new double* [M];
for (int i=0; i<M; i++){
u[i]=new double [N];
v[i]=new double [N];
}
dim3 blocks(32,32);
dim3 grids(M/32 +1, N/32 + 1);
for (int j = 0; j < M; j++) {
for (int i = 0; i < N;i++) {
u[i][j]=0.02;
v[i][j]=0.02;
}
}
double **u_d, **v_d;
int d_size = N * M * sizeof(double);
cudaMalloc(&u_d, d_size);
cudaMalloc(&v_d, d_size);
cudaMemcpy(u_d, u, d_size, cudaMemcpyHostToDevice);
cudaMemcpy(v_d, v, d_size, cudaMemcpyHostToDevice);
my_kernel<<<grids, blocks>>> (v_d,u_d);
cudaDeviceSynchronize();
cudaMemcpy(v, v_d, d_size, cudaMemcpyDeviceToHost);
cudaMemcpy(u, u_d, d_size, cudaMemcpyDeviceToHost);
cout<<"u: "<<u[512][512]<<" v: "<<v[512][512]<<endl;
return 0;
}
What I expect from the second version is :
u: 0.2815 v: 1.7581
Your two-dimensional array - in the first version of the program - is implemented using an array of pointers, each of which to a separately-allocated array of double values.
In your second version, you are using the same pointer-to-pointer-to-double type, but - you're not allocating any space for the actual data, just for the array of pointers (and not copying any of the data to the GPU - just the pointers; which are useless to copy anyway, since they're pointers to host-side memory.)
What is most likely happening is that your kernel attempts to access memory at an invalid address, and its execution is aborted.
If you were to properly check for errors, as #njuffa noted, you would know that is what happened.
Now, you could avoid having to make multiple memory allocations if you were to use a single data area instead of separate allocations for each second-dimension 1D array; and that is true both for the first and the second version of your program. That would not quite be array flattening. See an explanation of how to do this (C-language-style) on this page.
Note, however, that double-dereferencing, which you insist on performing in your kernel, is likely slowing it down significantly.
I am trying to tune quad copter (single axis) via string setup.
For pitch axis it is some what ok (I think), quad stabilized with P alone (is it weird ?).
But for Roll - i'm unable to tune it. it just rolls as soon small throttle applied. (I dont even see oscillations)
I changed X config to + Config but same reaction.
I am thinking there may be some bug in my code for roll (or pitch ) PID calculation (I'm not sure) which is not visible to me.
So If you guys found anything weird in attached code let me know.
/*
ref - http://ardupilot.org/dev/docs/apmcopter-programming-attitude-control-2.html and ymfc-al code
angle error (difference between target error and actual angle) is converted in to desired rotation rate followed by
previous rate pid from ksrmQuadRateMode
The input pulses from RC receiver are converted to the +/- rate of rotation.
The pilot input acts as the SetPoint to roll,pitch and yaw rate PID's. Measured input to PID comes from
gyroscope
Motor directions for Quad + configuration
5 (cw)
|
|
(ccw) 6 ------|------ (ccw)4
|
|
7 (cw)
Arduino Pin
m0 right - ccw 4
m1 top - cw 5
m2 left - ccw 6
m3 bottom - cw 7
MPU6050 Connections
SCL - A5
SDA - A4
Rx connections
Enable PCMSK0 Register - PCMSK0 - portB (D8-D13) (PCINT0-PCINT6)
ArduinoPin PCINTx
ch1 - 8 PCINT0 - Roll
ch2 - 9 PCINT1 - Pitch
ch3 - 10 PCINT2 - Throttle
ch4 - 11 PCINT3 - YAW
Arm and Disarm mechanism
To Arm - Throttle Min && Yaw Max
To DisArm - Throttle Min && Roll Max
*/
// Dont use servo - it is limited to 50Hz and conflict with Software serial
// If you are planning for more serial devices go for arduino Mega
// I use blutooth to get flight parameters - it will be used with hardware serial
#include<PID_v1.h>
#include<Wire.h>
#include<math.h>
//used to send debug info via bluetooth
//#define DEBUG
#define DATA_INTERVAL 200 //send debug info every 200 milli seconds
unsigned long btDataStartMillis;
//motor connections
#define m0 4
#define m1 5
#define m2 6
#define m3 7
//receiver connections
#define ch0 8
#define ch1 9
#define ch2 10
#define ch3 11
#define motorMinValue 1150
#define motorMaxValue 1800
//variables used for MPU6050 Angle calculation
const float GYRO_SENSITIVITY_SCALE_FACTOR = 65.5; //for 500 degrees/sec full scale
//MPU6050 I2C address
const int MPU6050_ADDR = 0b1101000;
//MPU6050 Register Addresses
const int PWR_REG_ADDR = 0x6B;
const int GYRO_CONFIG_REG_ADDR = 0x1B;
const int GYRO_CONFIG_REG_VALUE = 0x08;//for 500 degrees / sec
const int ACCR_CONFIG_REG_ADDR = 0x1C;
const int ACCR_CONFIG_REG_VALUE = 0x10; //for +/- 8g
const int ACCR_READ_START_ADDR = 0x3B;
//variables used for angular rate and angle calculations
const float radToDegreeConvert = 180.0 / PI;
int16_t accX, accY, accZ, gyroX, gyroY, gyroZ, tmp; //used to store the data from MPU6050 registers
float rotX, rotY, rotZ; //used to store the angular rotation from gyro
float accAngleX = 0.0, accAngleY = 0.0, accrAngleZ = 0.0;
float compAngleX = 0.0, compAngleY = 0.0;
float pitchAngle = 0.0, rollAngle = 0.0;
float pitchAngleAdjust = 0.0, rollAngleAdjust = 0.0;
unsigned long prevTime = 0, currentTime = 0; //used to calculate the dt for gyro integration
unsigned long loopTimer = 0, now = 0, difference = 0; //used to calculate the loop execution time
//gyro offset variables
float gyroOffsetValX = 0, gyroOffsetValY = 0, gyroOffsetValZ = 0;
float accrOffsetValX = 0, accrOffsetValY = 0;
const int noOfSamplesForOffset = 400;
//variables used in pinChange ISR
volatile boolean armMotors = false; //used to arm and disarm quad
volatile int pwmDuration[4]; //used to store pwm duration
volatile unsigned long pwmStart[4];
//port status
volatile byte prevPortState[] = {0, 0, 0, 0};
volatile byte presentPortState[4];
//pinDeclarations for Rx
const byte rxCh[] = {ch0, ch1, ch2, ch3};
const byte noOfChannels = sizeof(rxCh);
//PID Constants //0.5
const float pitchRatePGain = 1.0;
const float pitchRateIGain = 0.0;
const float pitchRateDGain = 0.0;
const float rollRatePGain = 1.0;
const float rollRateIGain = 0.0;
const float rollRateDGain = 0.0;
const float yawRatePGain = 0.0;
const float yawRateIGain = 0.0;
const float yawRateDGain = 0.0;
const int pidOutMax = 400;
// PID variables
float pidPitchRateIn = 0.0, pidPitchRateOut = 0.0, pidPitchRateSetPoint = 0.0;
float pidRollRateIn = 0.0, pidRollRateOut = 0.0, pidRollRateSetPoint = 0.0;
float pidYawRateIn = 0.0, pidYawRateOut = 0.0, pidYawRateSetPoint = 0.0;
float pitchError = 0.0, rollError = 0.0, yawError = 0.0;
float pitchPrevError = 0.0, rollPrevError = 0.0, yawPrevError = 0.0;
float pitchErrorSum = 0.0, rollErrorSum = 0.0, yawErrorSum = 0.0; //for integral coefficient
float pitchErrorDelta = 0.0 , rollErrorDelta = 0.0 , yawErrorDelta = 0.0; //for differntial coeffcient
const int pidMax = 400;
//variables to store the motor values calculated using pid and throttle
int m0Value, m1Value, m2Value, m3Value;
//Interrupt Service Routine will fire when for PinChange in PortB
ISR(PCINT0_vect) {
for (int ch = 0; ch < noOfChannels ; ch++) {
presentPortState[ch] = digitalRead(rxCh[ch]);
}//end of for looptr
for (int c = 0; c < noOfChannels ; c++) {
if (prevPortState[c] == 0 & presentPortState[c] == 1) {
//if previous state is 0 and present state is 1 (Raising Edge) then take the time stamp
pwmStart[c] = micros();
prevPortState[c] = 1; //update the prevPort State
} else if (prevPortState[c] == 1 & presentPortState[c] == 0) {
//if previous state is 1 and present state is 0 (Falling Edge) then calculate the width based on the change
pwmDuration[c] = micros() - pwmStart[c];
prevPortState[c] = 0; //update Present PortState
}
}//end of for loop
//ARM motors
if (pwmDuration[2] < 1250 && pwmDuration[3] > 1700) {
//Throttle Min And Yaw Max
armMotors = true;
}
//DISARM Motors
if (pwmDuration[2] < 1250 && pwmDuration[0] > 1600) {
//Throttle Min and Roll Max
armMotors = false;
}
}//end of ISR Fcn
void initReceiver() {
cli(); //Clear all interrupts
PCICR |= 1 << PCIE0; //Enable port B Registers i.e D8-D13
PCMSK0 |= 1 << PCINT3 | 1 << PCINT2 | 1 << PCINT1 | 1 << PCINT0 ; // Pin11,10,9,8
sei(); //enable all interrupts
for (int i = 0 ; i < noOfChannels ; i++) {
pinMode(rxCh[i], INPUT);
digitalWrite(rxCh[i], HIGH); //enable pullup
}//end of for loop
}//end of initReceiver Fcn
void updateMotors() {
int throttle = pwmDuration[2];
m0Value = throttle - pidRollRateOut + pidYawRateOut; //right
m1Value = throttle - pidPitchRateOut - pidYawRateOut; //top
m2Value = throttle + pidRollRateOut + pidYawRateOut; //left
m3Value = throttle + pidPitchRateOut - pidYawRateOut; //bottom
//dont send values more than motorMaxValue - ESC may get into trouble
if (m0Value > motorMaxValue) m0Value = motorMaxValue;
if (m1Value > motorMaxValue) m1Value = motorMaxValue;
if (m2Value > motorMaxValue) m2Value = motorMaxValue;
if (m3Value > motorMaxValue) m3Value = motorMaxValue;
//dont send values less than motorMinValue - Keep motors running
if (m0Value < motorMinValue) m0Value = motorMinValue + 10;
if (m1Value < motorMinValue) m1Value = motorMinValue + 10;
if (m2Value < motorMinValue) m2Value = motorMinValue + 10;
if (m3Value < motorMinValue) m3Value = motorMinValue + 10;
// Refresh rate is 250Hz: send ESC pulses every 4000µs
while ((now = micros()) - loopTimer < 4000);
// Update loop timer
loopTimer = now;
// Set pins #4 #5 #6 #7 HIGH
PORTD |= B11110000;
// Wait until all pins #4 #5 #6 #7 are LOW
while (PORTD >= 16) {
now = micros();
difference = now - loopTimer;
if (difference >= m0Value) PORTD &= B11101111; // Set pin #4 LOW
if (difference >= m1Value) PORTD &= B11011111; // Set pin #5 LOW
if (difference >= m2Value) PORTD &= B10111111; // Set pin #6 LOW
if (difference >= m3Value) PORTD &= B01111111; // Set pin #7 LOW
}//end of while loop
}//end of updateMotors Fcn
void initializeMotors() {
pinMode(m0, OUTPUT);
pinMode(m1, OUTPUT);
pinMode(m2, OUTPUT);
pinMode(m3, OUTPUT);
}//end of initializeMotors Fcn
/*
This function will convert Rx inputs into angular rate
which are used as SetPoints for PID
*/
void setPointUpdate() {
pidRollRateSetPoint = 0.0;
pidPitchRateSetPoint = 0.0;
pidYawRateSetPoint = 0.0;
if (pwmDuration[0] > 1508) {
pidRollRateSetPoint = pwmDuration[0] - 1508;
} else if (pwmDuration[0] < 1492) {
pidRollRateSetPoint = pwmDuration[0] - 1492;
}
rollAngleAdjust = rollAngle * 15; // convert 0 - 30 to 0 -450
pidRollRateSetPoint = pidRollRateSetPoint - rollAngleAdjust; //calculate angleError
pidRollRateSetPoint = pidRollRateSetPoint / 3.0 ;// the max roll rate is aprox 164 degrees per second (500/3 = 165 d/s )
if (pwmDuration[1] > 1508) {
pidPitchRateSetPoint = pwmDuration[1] - 1508;
} else if (pwmDuration[1] < 1492) {
pidPitchRateSetPoint = pwmDuration[1] - 1492;
}
pitchAngleAdjust = pitchAngle * 15;
pidPitchRateSetPoint = pidPitchRateSetPoint - pitchAngleAdjust;
pidPitchRateSetPoint = pidPitchRateSetPoint / 3.0;
if (pwmDuration[3] > 1508) {
pidYawRateSetPoint = pwmDuration[3] - 1508;
} else if (pwmDuration[3] < 1492) {
pidYawRateSetPoint = pwmDuration[3] - 1492;
}
pidYawRateSetPoint = pidYawRateSetPoint / 3.0;
}//end of SetPointUpdate Fcn
void initPID() {
//nothing to initialize here
}//end of initPID Fcn
/*
In this fuction we configure mpu6050 and calculate offsets i.e calibration
*/
void initMPU6050() {
configureMPU();
calculateOffsets();
}//end of initMPU6050 Fcn
void calculateOffsets() {
float gyroTotalValX = 0, gyroTotalValY = 0, gyroTotalValZ = 0;
float accrTotalValX = 0, accrTotalValY = 0;
for (int i = 0 ; i < noOfSamplesForOffset ; i++) {
readMPU();
delayMicroseconds(10);
gyroTotalValX = gyroTotalValX + rotX;
gyroTotalValY = gyroTotalValY + rotY;
gyroTotalValZ = gyroTotalValZ + rotZ;
accAngleY = atan(accX / sqrt(pow(accY, 2) + pow(accZ, 2))) * radToDegreeConvert;
accAngleX = atan(accY / sqrt(pow(accX, 2) + pow(accZ, 2))) * radToDegreeConvert;
accrTotalValX = accrTotalValX + accAngleX;
accrTotalValY = accrTotalValY + accAngleY;
// Serial.println("..........");
}//end of for loop
gyroOffsetValX = gyroTotalValX / noOfSamplesForOffset;
gyroOffsetValY = gyroTotalValY / noOfSamplesForOffset;
gyroOffsetValZ = gyroTotalValZ / noOfSamplesForOffset;
accrOffsetValX = accrTotalValX / noOfSamplesForOffset;
accrOffsetValY = accrTotalValY / noOfSamplesForOffset;
//read again to get current values
readMPU();
accAngleY = atan(accX / sqrt(pow(accY, 2) + pow(accZ, 2))) * radToDegreeConvert;
accAngleX = atan(accY / sqrt(pow(accX, 2) + pow(accZ, 2))) * radToDegreeConvert;
//apply offsets to accr values
accAngleX = accAngleX - accrOffsetValX;
accAngleY = accAngleY - accrOffsetValY;
//set initial values for gyro and complementary filter
rotX = accAngleX;
rotY = accAngleY;
compAngleX = accAngleX;
compAngleY = accAngleY;
}//end of calculateGyroOffsets Fcn
void readMPU() {
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(ACCR_READ_START_ADDR); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_ADDR, 14, true);
while (Wire.available() < 14) {
//wait until data is available
#ifdef DEBUG
Serial.println(F("MPU6050 waiting"));
#endif
}
accX = Wire.read() << 8 | Wire.read();
accY = Wire.read() << 8 | Wire.read();
accZ = Wire.read() << 8 | Wire.read();
tmp = Wire.read() << 8 | Wire.read();
gyroX = Wire.read() << 8 | Wire.read();
gyroY = Wire.read() << 8 | Wire.read();
gyroZ = Wire.read() << 8 | Wire.read();
//apply scale factor for gyro reading
rotX = gyroX / GYRO_SENSITIVITY_SCALE_FACTOR;
rotY = gyroY / GYRO_SENSITIVITY_SCALE_FACTOR;
rotZ = gyroZ / GYRO_SENSITIVITY_SCALE_FACTOR;
}//end of readGyroX Fcn
/*
This function will apply complementary filer to gyro angular rates and
assign them as PID inputs
*/
void calculateRotationRates() {
//apply the offset
rotX = rotX - gyroOffsetValX;
rotY = rotY - gyroOffsetValY;
rotZ = rotZ - gyroOffsetValZ;
//complemenrary filter for gyro readings
pidPitchRateIn = (pidPitchRateIn * 0.8) + (rotX * 0.2);
pidRollRateIn = (pidRollRateIn * 0.8) + (rotY * 0.2);
pidYawRateIn = (pidYawRateIn * 0.8) + (rotZ * 0.2);
}//end of calculateRotationRates Fcn
/*
This Fcn will calculate the angles using complementary filter
*/
void calculateAngles() {
//angles from accr
accAngleY = atan(accX / sqrt(pow(accY, 2) + pow(accZ, 2))) * radToDegreeConvert;
accAngleX = atan(accY / sqrt(pow(accX, 2) + pow(accZ, 2))) * radToDegreeConvert;
//apply offsets to accr values
accAngleX = accAngleX - accrOffsetValX;
accAngleY = accAngleY - accrOffsetValY;
//double dt = 0.004 = 1/250
//The Mighty Complementary filter
compAngleX = 0.98 * (compAngleX + rotX * 0.004) + 0.02 * accAngleX;
compAngleY = 0.98 * (compAngleY + rotY * 0.004) + 0.02 * accAngleY;
// assigning pitch and roll
pitchAngle = compAngleX;
rollAngle = compAngleY;
// //if imu yawed convert roll to pitch //0.004 = 1/250
//copied from ymfc-al code
pitchAngle = pitchAngle - rollAngle * sin(rotZ * 0.004 * ((3.142 * PI) / 180));
rollAngle = rollAngle + pitchAngle * sin(rotZ * 0.004 * ((3.142 * PI) / 180));
}//end of calculateAngles Fcn
//get register values from mpu6050 and calculate angles based on complementary filter
void updateAnglesFromMPU() {
readMPU();
calculateRotationRates();
calculateAngles();
}//end of updateAnglesFromMPU Fcn
void configureMPU() {
//Power Register
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(PWR_REG_ADDR);//Access the power register
Wire.write(0b00000000);//check datasheet
Wire.endTransmission();
//Gyro Config
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(ACCR_CONFIG_REG_ADDR);
Wire.write(ACCR_CONFIG_REG_VALUE);//check data sheet for more info
Wire.endTransmission();
//Accr Config
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(GYRO_CONFIG_REG_ADDR);
Wire.write(GYRO_CONFIG_REG_VALUE);//check datasheet for more info
Wire.endTransmission();
}//end of setUpMPU Fcn
void computePID() {
//input updated in updateAngles Fcn
//update SetPoint according to throttle
setPointUpdate();
//calculate errors
pitchError = pidPitchRateIn - pidPitchRateSetPoint;
rollError = pidRollRateIn - pidRollRateSetPoint;
yawError = pidYawRateIn - pidYawRateSetPoint;
//calculate sum of errors
pitchErrorSum = pitchError + pitchPrevError;
rollErrorSum = rollError + rollPrevError;
yawErrorSum = yawError + yawPrevError;
//calculate delta of errors
pitchErrorDelta = pitchError - pitchPrevError;
rollErrorDelta = rollError - rollPrevError;
yawErrorDelta = yawError - yawPrevError;
//save current error as prev error for next iteration
pitchPrevError = pitchError;
rollPrevError = rollError;
yawPrevError = yawError;
//PID Calculation
pidPitchRateOut = (pitchError * pitchRatePGain) + (pitchErrorSum * pitchRateIGain) + (pitchErrorDelta * pitchRateDGain);
pidRollRateOut = (rollError * rollRatePGain) + (rollErrorSum * rollRateIGain) + (rollErrorDelta * rollRateDGain);
pidYawRateOut = (yawError * yawRatePGain) + (yawErrorSum * yawRateIGain) + (yawErrorDelta * yawRateDGain);
if (pidPitchRateOut > pidMax) pidPitchRateOut = pidMax;
else if (pidPitchRateOut < -pidMax) pidPitchRateOut = pidMax * -1;
if (pidRollRateOut > pidMax) pidRollRateOut = pidMax;
else if (pidRollRateOut < -pidMax) pidRollRateOut = pidMax * -1;
if (pidYawRateOut > pidMax) pidYawRateOut = pidMax;
else if (pidYawRateOut < -pidMax ) pidYawRateOut = pidMax * -1;
}//end of computePID Fcn
void resetPID() {
}//end of resetPID Fcn
void sendBTOutput() {
if (millis() - btDataStartMillis > DATA_INTERVAL) {
btDataStartMillis = millis();
Serial.print(F("y")); Serial.print(pidYawRateIn); Serial.print(F(">"));
Serial.print(F("p")); Serial.print(pitchAngle); Serial.print(F(">"));
Serial.print(F("r")); Serial.print(rollAngle); Serial.print(F(">"));
Serial.print(F("ps")); Serial.print(pidPitchRateSetPoint); Serial.print(F(">"));
Serial.print(F("rs")); Serial.print(pidRollRateSetPoint); Serial.print(F(">"));
Serial.print(F("ys")); Serial.print(pidYawRateSetPoint); Serial.print(F(">"));
Serial.print(F("po")); Serial.print(pidPitchRateOut); Serial.print(F(">"));
Serial.print(F("ro")); Serial.print(pidRollRateOut); Serial.print(F(">"));
Serial.print(F("yo")); Serial.print(pidYawRateOut); Serial.print(F(">"));
Serial.print(F("m0-")); Serial.print(m0Value); Serial.print(F(">"));
Serial.print(F("m1-")); Serial.print(m1Value); Serial.print(F(">"));
Serial.print(F("m2-")); Serial.print(m2Value); Serial.print(F(">"));
Serial.print(F("m3-")); Serial.print(m3Value); Serial.print(F(">"));
Serial.print(F("c0")); Serial.print(pwmDuration[0]); Serial.print(F(">"));
Serial.print(F("c1")); Serial.print(pwmDuration[1]); Serial.print(F(">"));
Serial.print(F("c2")); Serial.print(pwmDuration[2]); Serial.print(F(">"));
Serial.print(F("c3")); Serial.print(pwmDuration[3]); Serial.print(F(">"));
Serial.println("");
}//end of IF
}//end of sendBTOutput Fcn
void setup() {
pinMode(13, OUTPUT);
digitalWrite(13, LOW); //used to indicate FC is ready
initReceiver();
#ifdef DEBUG
Serial.begin(115200); //HC-05 is using hardware serial, 38400 is HC-05 default baud rate
btDataStartMillis = millis();
Serial.println("DEBUG mode ON");
#endif
initMPU6050();
initPID();
initializeMotors();
//prevTime = millis(); //used to calculate dt for gyro integration
loopTimer = micros();//used to keep track of loop time
digitalWrite(13, HIGH); //FC is Ready
}//end of setup Fcn
void loop() {
updateAnglesFromMPU();
if (armMotors == true) {
computePID();
updateMotors();
}//end of armMotors
#ifdef DEBUG
sendBTOutput();
#endif
}//end of loop Fcn
Edit :
These are the directions for angle and angular rate
Roll and Roll Rate directions are inverted.
Angle
Roll
Right Down --> -ve
Left Down --> +ve
Pitch
Nose Up --> +ve
Node Down --> -ve
Angular Rate
Roll
Right Down --> + ve
Left Down --> - ve
Pitch
Nose Up --> + ve
Nose Down --> -ve
are these directions are ok ? Or DId I mess up something in the code ?
Changing
rotY = rotY - gyroOffsetValY;
to
rotY = rotY - gyroOffsetValY;
rotY = -1 * rotY
seems to solve this problem for me
I am trying to find the tilt angle of an object using LSM6DS3 in arduino mega board, using Madgwick filter algorithm. But the accelero meter values drops suddenly to zero readings, so that the filter gives error output. Attaching my codes with this post.
/* Sensor SDI/SDA - Arduino Digital Pin 51 or MOSI (Master Out Slave In)
* Sensor SCK/SCL - Arduino Digital Pin 52 or SCK (Serial Clock)
* Sensor SDO/ADR - Arduino Digital Pin 50 or MISO (Master In Slave Out)
* Sensor CS - Arduino Digital Pin 10 or 53 (Slave Select or Chip Select)
*/
#include <SPI.h> // Include Arduino SPI library
#include <math.h>
#define OUT_X_L_G 0x22 // Register addresses from sensor datasheet.
#define OUT_X_H_G 0x23 // Only the registers that are used
#define OUT_Y_L_G 0x24 // in this program are defined here.
#define OUT_Y_H_G 0x25
#define OUT_Z_L_G 0x26
#define OUT_Z_H_G 0x27
#define CTRL1_A 0x10
#define CTRL2_G 0x11
#define CTRL3_C 0x12
#define CTRL4_C 0x13
#define CTRL9_A 0x18
#define CTRL10_C 0x19
#define OUT_X_L_A 0x28
#define OUT_X_H_A 0x29
#define OUT_Y_L_A 0x2A
#define OUT_Y_H_A 0x2B
#define OUT_Z_L_A 0x2C
#define OUT_Z_H_A 0x2D
#define WHO_AM_I 0x0F
#define CTRL7_G 0x16 // setting hpf for gyro
#define STATUS_REG 0x1E
#define sampleFreq 512.0f // sample frequency in Hz 512
#define betaDef 0.1f // 2 * proportional gain 0.1
#define M_PI 3.14
int8_t readData = 0x80;
int8_t writeData = 0x00;
int8_t k ;
int16_t Ax, Ay, Az; // 16-bit variables to hold raw data from sensor
int16_t Gx, Gy, Gz;
float Aax, Aay, Aaz;
float Ggx, Ggy, Ggz;
const int CS = 53; // Chip Select pin for SPI
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
volatile float beta = betaDef;
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
float invSqrt(float x);
float roll,pitch;
void setup()
{
Serial.begin(57600);
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV4);
SPI.setBitOrder(MSBFIRST);
SPI.setDataMode(SPI_MODE3);
pinMode(CS, OUTPUT);
writeReg(CTRL3_C, 0x40); // BDU UPDATE
delay(100);
writeReg(CTRL9_A, 0x38); // Enable x,y,z axes of ACCEL
writeReg(CTRL10_C, 0x38); //Enable x,y,z of GYRO
writeReg(CTRL1_A, 0x7C); // ODR=833, scale = 8g
writeReg(CTRL2_G, 0x7C); // scale= =2000dps, ODR = 833
writeReg(CTRL4_C, 0x04); // SPI only, I2C disabled
writeReg(CTRL7_G, 0x70); // setting high pass filter for gyro
delay(100);
}
void loop()
{
Gx = (int16_t) readReg(OUT_X_H_G) <<8 | readReg(OUT_X_L_G); // typecast as
Gy = (int16_t) readReg(OUT_Y_H_G) <<8 | readReg(OUT_Y_L_G); // 16-bit
Gz = (int16_t) readReg(OUT_Z_H_G) <<8 | readReg(OUT_Z_L_G);
Ax = (int16_t) readReg(OUT_X_H_A) <<8 | readReg(OUT_X_L_A);
Ay = (int16_t) readReg(OUT_Y_H_A) <<8 | readReg(OUT_Y_L_A);
Az = (int16_t) readReg(OUT_Z_H_A) <<8 | readReg(OUT_Z_L_A);
Aax = (Ax*0.244)/1000;
Aay = (Ay*0.244)/1000;
Aaz = (Az*0.244)/1000;
Ggx = (Gx*0.07)*(M_PI/180);
Ggy = (Gy*0.07)*(M_PI/180);
Ggz = (Gz*0.07)*(M_PI/180);
MadgwickAHRSupdateIMU( Ggx, Ggy, Ggz, Aax,Aay,Aaz);
roll=atan2(2*q2*q3-2*q0*q1,(2*q0*q0)+(2*q3*q3)-1)*180/M_PI;
pitch=-1*asin(2*q1*q3+2*q0*q2)*180/M_PI;
Serial.println(roll);
Serial.println(pitch);
}
int8_t readReg(int8_t address) {
int8_t buffer = 0;
digitalWrite(CS, LOW);
SPI.transfer(readData | address);
buffer = SPI.transfer(writeData);
digitalWrite(CS, HIGH);
return(buffer);
}
void writeReg(int8_t address, int8_t val)
{
digitalWrite(CS, LOW);
SPI.transfer(writeData | address);
SPI.transfer(val);
digitalWrite(CS, HIGH);
}
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az)
{
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
float invSqrt(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
I am trying to get my arduino code for gemma, with neopixels, which has 5310 bytes of memory smaller so I can get more things into the program.
Currently I am trying to remove floats / reduce the size of the code snippet below:
void gradient(Color c1, Color c2, float time) {
for (float i = 0; i < time; i += 0.001) {
Color result(0, 0, 0);
result.Red = c1.Red * (1 - (i / time)) + c2.Red * (i / time);
result.Green = c1.Green * (1 - (i / time)) + c2.Green * (i / time);
result.Blue = c1.Blue * (1 - (i / time)) + c2.Blue * (i / time);
for (uint8_t x = 0; x < 20; x++)pixels.setPixelColor(x, result.Red, result.Green, result.Blue);
pixels.show();
delay(1);
}
}
I managed to reduce it by 30 bytes to:
void gradient(Color c1, Color c2, float time) {
float stepsize = 0.01; // Stepsize in seconds
float lambda;
int maxiter = (int) (time/ stepsize);
Color result(0, 0, 0);
for (int i = 0; i <= maxiter; i++) {
lambda = (float) i / maxiter;
result.Red = c1.Red * (1 - lambda) + c2.Red * (lambda);
result.Green = c1.Green * (1 - lambda) + c2.Green * (lambda);
result.Blue = c1.Blue * (1 - lambda) + c2.Blue * (lambda);
for (uint8_t x = 0; x < 20; x++)pixels.setPixelColor(x, result.Red, result.Green, result.Blue);
pixels.show();
delay(stepsize * 1000); // delay in milliseconds
}
}
But am trying still to make it smaller.
For those wondering the Color object is just an object with 3 ints called Red, Green and Blue. An example usage of this code would be:
gradient(Color(255, 0, 0), Color(0, 255, 0), 2);
Which would be a gradient from Red to Green over 2 seconds.
Thanks in advance!
If you can pull "delay()" out of all your code, it seems to avoid including a 100 byte size library? idk tbh, but here is my suggested modification, which in my testing saves 100 bytes of memory:
void gradient(Color c1, Color c2, float time) {
float stepsize = 0.01; // Stepsize in seconds
float lambda;
int maxiter = (int) (time/ stepsize);
Color result(0, 0, 0);
for (int i = 0; i <= maxiter; i++) {
lambda = (float) i / maxiter;
result.Red = c1.Red * (1 - lambda) + c2.Red * (lambda);
result.Green = c1.Green * (1 - lambda) + c2.Green * (lambda);
result.Blue = c1.Blue * (1 - lambda) + c2.Blue * (lambda);
for (uint8_t x = 0; x < 20; x++)pixels.setPixelColor(x, result.Red, result.Green, result.Blue);
pixels.show();
//delay(stepsize * 1000); // delay in milliseconds
long lastTime=millis();
long delayTime = stepsize * 1000;
while(millis()-lastTime<delayTime){}
}
}
-First off, your color object should take 3 unsigned chars (0-255) there is no reason to put ints in there. (byte type in arduino)
-Second, I am not sure how you are implementing time, but generally in arduino you are working in milliseconds. Furthermore, without seeing your other implementation, I am guessing that time is a segment of time and based on your delay, I am going to guess that you could send time as a short (up multiply x1000 if necessary) (This would hold up to 32 seconds, in milliseconds)
void gradient(Color c1, Color c2, short time) {
short maxiter = (short) (time/ 10);
Color result(0, 0, 0);
for (int i = 0; i <= maxiter; i++) {
result.Red = (c1.Red * (maxiter-i) + c2.Red * i)/maxiter;
result.Green = (c1.Green* (maxiter-i) + c2.Green* i)/maxiter;
result.Blue = (c1.Blue* (maxiter-i) + c2.Blue* i)/maxiter;
for (uint8_t x = 0; x < 20; x++)pixels.setPixelColor(x, result.Red, result.Green, result.Blue);
pixels.show();
delay(10); // delay in milliseconds
}
}