-
Notifications
You must be signed in to change notification settings - Fork 0
/
sf_MPU6050_2xDriver_GxAyz_wrapper.cpp
117 lines (98 loc) · 2.63 KB
/
sf_MPU6050_2xDriver_GxAyz_wrapper.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
/*
* Include Files
*
*/
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
# ifndef MATLAB_MEX_FILE
// Required for I2Cdev library
#define ARDUINO 103 // using wire, and twi from arduino 1.0.3
#include <Arduino.h>
#include "Wire.h"
//#include "gyro.h"
# include "twi.h"
# include "twi.c"
# include "Wire.cpp"
# include "I2Cdev.cpp"
# include "MPU6050.cpp"
# include "I2Cdev.h"
# include "MPU6050.h"
//L3G gyro;
MPU6050 accelgyro(0x68);
MPU6050 accelgyro2(0x69);
#endif
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
#define u_width
#define y_width 1
/*
* Create external references here.
*
*/
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */
/*
* Output functions
*
*/
extern "C" void sf_MPU6050_2xDriver_GxAyz_Outputs_wrapper(int16_T *x_vel,
int16_T *y_acc,
int16_T *z_acc,
int16_T *x_vel_2,
int16_T *y_acc_2,
int16_T *z_acc_2,
const real_T *xD)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
/* This sample sets the output equal to the input
y0[0] = u0[0];
For complex signals use: y0[0].re = u0[0].re;
y0[0].im = u0[0].im;
y1[0].re = u1[0].re;
y1[0].im = u1[0].im;
*/
if(xD[0] == 1)
{
#ifndef MATLAB_MEX_FILE
x_vel[0]=accelgyro.getRotationX();
y_acc[0]=accelgyro.getAccelerationY();
z_acc[0]=accelgyro.getAccelerationZ();
x_vel_2[0]=accelgyro2.getRotationX();
y_acc_2[0]=accelgyro2.getAccelerationY();
z_acc_2[0]=accelgyro2.getAccelerationZ();
#endif
}
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
* Updates function
*
*/
extern "C" void sf_MPU6050_2xDriver_GxAyz_Update_wrapper(const int16_T *x_vel,
const int16_T *y_acc,
const int16_T *z_acc,
const int16_T *x_vel_2,
const int16_T *y_acc_2,
const int16_T *z_acc_2,
real_T *xD)
{
/* %%%-SFUNWIZ_wrapper_Update_Changes_BEGIN --- EDIT HERE TO _END */
if(xD[0] != 1){
# ifndef MATLAB_MEX_FILE
Wire.begin();
accelgyro.initialize();
accelgyro.setDLPFMode(MPU6050_DLPF_BW_42);
accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
accelgyro2.initialize();
accelgyro2.setDLPFMode(MPU6050_DLPF_BW_42);
accelgyro2.setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
#endif
//done with initialization
xD[0] = 1;
}
/* %%%-SFUNWIZ_wrapper_Update_Changes_END --- EDIT HERE TO _BEGIN */
}