time optimal spacecraft attitude control with

TIME OPTIMAL SPACECRAFT ATTITUDE CONTROL WITH CONSIDERATIONS OF FAILURES IN AOCS AND PAYLOAD COMPONENTS

By M. Ibbtisam Asim Hassaan Bin Jalil

Supervisors Dr. Umer Iqbal Bhatti Dr. Ali Nasir Ms. Aisha Rabbia Ahsan

Department of Aeronautics and Astronautics Institute of Space Technology, Islamabad 2015


A thesis submitted to the Institute of Space Technology in partial fulfillment of the requirements for the degree of Bachelor of Science in Aerospace Engineering


Supervisors Dr. Umer Iqbal Bhatti Dr. Ali Nasir Ms. Aisha Rabbia Ahsan

Department of Aeronautics and Astronautics Institute of Space Technology, Islamabad 2015 1

2

Institute of Space Technology Department of Aeronautics & Astronautics



APPROVAL BY BOARD OF EXAMINERS

------------Dr. Umer Iqbal Bhatti

------------Ms. Aisha Rabbia Ahsan 3

1. 2. CERTIFICATE

This is to certify that the research work described in this thesis is the original work of the authors and has been carried out under my direct supervision. I have personally gone through all the data/results/materials reported in the manuscript and certify their correctness/authenticity. I further certify that the material included in this thesis is not plagiarized and has not been used in part or full in a manuscript already submitted or in the process of submission in partial/complete fulfillment of the award of any other degree from any institution. I also certify that the thesis has been prepared under my supervision according to the prescribed format and I endorse its evaluation for the award of Bachelor of Science in Aerospace Engineering degree through the official procedures of the Institute.

__________________ Asst.Prof. Dr. Umer Iqbal Bhatti

4

3. COPYRIGHT © 2015 4. THIS DOCUMENT IS JOINTLY COPYRIGHTED BY THE AUTHORS AND THE INSTITUTE OF SPACE TECHNOLOGY (IST). BOTH AUTHORS AND IST CAN USE, PUBLISH OR REPRODUCE THIS DOCUMENT IN ANY FORM. UNDER THE COPYRIGHT LAW NO PART OF THIS DOCUMENT CAN BE REPRODUCED BY ANYONE, EXCEPT COPYRIGHT HOLDERS, WITHOUT THE PERMISSION OF AUTHORS.

5

5. 6. ACKNOWLEDGEMENTS

First of all we would like to thanks to Allah Almighty, without Whom we are nothing. Secondly we would like to thanks to our supervisors for their valuable guidance. And we would also like to thanks to other faculty members especially to our project coordinator. Finally special thanks to our parents, without their support we would not have achieved anything.

7.

8. ABSTRACT

A simulation based time optimal and fault tolerant attitude control system of a satellite was developed with the aid of one degree of freedom spacecraft simulator called TableSat. TableSat emulates the dynamics, sensing and actuation capabilities required for spacecraft attitude control. Sensors include two gyros; one for attitude control and second to aid the first in fault tolerant scheme. Two computer fans provide actuation capabilities to TableSat in clockwise and anticlockwise direction. A step by step approach is adopted in control scheme by starting from basic PD-controller to regulators and estimators leading to advance optimal control i.e. LQR. Different cases are included in fault tolerant scheme, to decrease the vulnerability of system to faults. Interactive software is developed for reference tracking control of TableSat by gyros and Bluetooth.

9. Table of Contents

1.

INTRODUCTION AND BACKGROUND ............................................................................ 1 1.1

2.

TableSat Background ......................................................Error! Bookmark not defined.

DESCRIPTION OF TABLESAT HARDWARE.................................................................. 11

2.1

Circuit Designing ............................................................................................................... 15

3.

TABLESAT ONBOARD SOFTWARE ................................Error! Bookmark not defined.

4.

SYSTEM MODELING ......................................................................................................... 16 4.1

TableSat Equations of Motion ....................................................................................... 16

4.2

TableSat Moment of Inertia ........................................................................................... 17

4.3

Friction Characterization ................................................................................................ 17

4.3.1

TableSat Friction ........................................................................................................ 18

4.3.1.1 Static Friction .......................................................................................................... 18 4.3.1.2 Dynamic Friction .................................................................................................... 18 4.3.2 4.4

5.

Fan Friction................................................................................................................. 19

TableSat Parameters........................................................................................................... 19

4.4.1

Fan Time Constant...................................................................................................... 19

4.4.2

Voltage to Change in Fan Speed Constant ................................................................. 20

4.4.3

Fan Speed to Fan Force Constant ............................................................................... 21

LINEAR MODEL DEVELOPMENT ................................................................................... 23

5.1

TableSat Friction Compensation........................................................................................ 23

5.2

Fan Friction Compensation ................................................................................................ 23

5.3

Linear Model ...................................................................................................................... 23

6.

MATLAB SIMULATION and CONTROLLER DESIGNING ........................................... 25 6.1

7. 7.1

Controller Designing ...................................................................................................... 27

DELIVERABLES AND TIMELINE .................................................................................... 41 Future Course of Action......................................................Error! Bookmark not defined.

8.

FINANCIAL REPORT ......................................................................................................... 43

9.

CONCLUSION ..................................................................................................................... 44

Appendix A ................................................................................................................................... 46 TableSat Source Code ................................................................................................................... 46 Appendix B ................................................................................................................................... 60 TableSat Source Code- PD Controller .......................................................................................... 60 References ................................................................................................................................... 129

List of Figures

Figure 2-1: Arduino Uno ................................................................................................................ 3 Figure 2-2: MPU 6050 .................................................................................................................... 3 Figure 2-3: TableSat ....................................................................................................................... 3 Figure 2-4: TableSat Mounting Assembly (Downside View) ........................................................ 4 Figure 2-5: TableSat Mounting Assembly...................................................................................... 4 Figure 2-6: TableSat Mounting Assembly MAYA Model ............................................................. 4 Figure 2-7: TableSat Mounting Assembly MAYA Model ............................................................. 4 Figure 2-7: TableSat MAYA Model ............................................................................................... 5 Figure 2-7: TableSat Circuit ........................................................................................................... 5 Figure 3-1: Arduino IDE ................................................................................................................. 6 Figure 3-1: Tablesat Creo Model (Upside View) ........................................................................... 8 Figure 3-1: TableSat Creo Model (Downside View) ...................................................................... 8 Figure 6-1: Root Locus of TableSat Model ...................................Error! Bookmark not defined. Figure 6-2: Step Response of Open Loop TableSat Model ...........Error! Bookmark not defined. Figure 6-3: PD-Controlled Simulink Model ..................................Error! Bookmark not defined. Figure 6-4: Response of TableSat Model to Initial Conditions ...Error! Bookmark not defined.8 Figure 7-1: 7th Semester Timeline ................................................Error! Bookmark not defined. Figure 7-2: 8th Semester Timeline ................................................Error! Bookmark not defined.

1. INTRODUCTION 1.1

Motivation Spacecraft attitude is the positioning of a spacecraft with respect to an external

frame of reference. Almost every spacecraft is required to point to a particular direction. Some satellites are pointed towards Earth and other are facing other planetary or space objects for different research purposes and these purposes even change with the passage of time, so it is mandatory to control the attitude of a spacecraft. In some cases, a part of a satellite faces towards the Earth while its solar panels are required to remain in sunlight, for this purpose attitude control of a spacecraft is also required. Similarly, orbital maneuvers also require attitude control system for a satellite. Space environment is a harsh one, a spacecraft can feel the presence of many forces like solar radiation pressure, galactic radiation, impact by space debris or meteoroids, gravitational field not only by Earth but also by other celestial bodies. Even satellites can also use special control surfaces to counter the effect of impinging radiations, indeed solar radiation pressure is also used as a viable mean of amending spacecraft dynamics, especially for long life missions. So in short, such forces or disturbances can not only portentously alter the attitude and orbital path of a spacecraft but can also damage the components of a spacecraft leading to mission failures. A satellite can also alter its attitude due to some faults in the sensors, or due to some excessive maneuvering forces by a faulty thruster. So for all these reasons, attitude control system becomes an integral part of a spacecraft. History states the failure of many space missions due to occurrence of faults in the components of a spacecraft caused by harsh space environment [9]. In 1994, Telsat, 1

Canada’s Anik E-1 communications satellite, suddenly began to spin out of control. Two hours later its sister satellite, Anik E-2, also without warning, began to spin out of control [54][9]. Telsat engineers quickly determined that the gyroscopic guidance system on both satellites had failed causing an interruption of cable TV, telephone, newswire, and data transfer services throughout Canada. Many similar examples can be quoted, all demanding the need of such a spacecraft which possess a fault tolerant Attitude and Orbit Control System (AOCS), so that it can perform its mission even if it faces some faults in some of its sensors or thrusters in its life cycle. Precision, accuracy and agility is emerging day by day in modern satellites. Rapid maneuvering capability of satellites is a demanding need of both military and commercial users to increase the efficiency and productivity of missions because prompt retargeting maneuvering capabilities means more time on the envisioned object/target and supplementary observations per orbit [pap]. With the advanced commercial Earthimaging satellites like Ikonos and agile microsatellites [1], the need for time optimal attitude control of satellites is rising than ever. Planned impending satellite missions in provision of missile-defense and Earth observation will count on agility of satellites and time optimal maneuvers for mission accomplishment. 1.2

Goals and Objective In the light of the above mentioned facts, the main goal of this research project is

to design an attitude control system (ACS) of a spacecraft which should have the following capabilities;

2

 It should be time optimal, in other words the designed attitude control system (ACS) should be capable of maneuvering the spacecraft in minimum possible time.  It should be fault tolerant, means if there is any fault in Attitude and Orbit Control System (AOCS) of the satellite which includes all types of satellite sensors, actuators and thrusters, then the ACS should be capable of working as good as it was before the occurrence of that particular fault. The second part of this project is to implement the designed time optimal and fault tolerant attitude control system on a one degree of freedom table based satellite simulator known as TableSat. 1.3

TableSat’s Introduction and Background Control systems engineering applies control theory to design systems with desired

behaviors. It is rapidly invading in all areas of our lives. We can find its application in small toys for children to complex multi degree of freedom control systems that steers a spacecraft as it flies through solar system. A controller is not a physical object that can be seen, it’s an algorithm implemented in computer that can alter system’s performance and dynamics. It is intangible, it is abstract, it is vague, but it is also very powerful as it can automatically control 3000kg spacecraft without human intervention. The intangibility of the algorithms which enable these feats can make control systems engineering a difficult topic to understand for students. The basic principles of linearity, stability, robustness, and control can be explained in a mathematical sense; Bode, Nyquist, and Root Locus plots can be generated; phase margin, gain margin, and 3

bandwidth can be defined, but the physical meaning of these concepts and tools is often a leap that students cannot make based on theory alone. They cannot see how transforming errors into specific control inputs as specified by a given transfer function will produce the desired results. To help answer these questions, students need something that demonstrates a real control system, something they can see and touch. Something that allows them to see how a given controller will modify a dynamic system, and how modifying the controller parameters corresponds and changes system response. TableSat is a great tool to serve this purpose. TableSat is an interactive, single degree-of-freedom spacecraft simulator designed as a tool for demonstrating and learning control system design. Specifically, it uses hardware and software similar to that found in a current spacecraft attitude control system (ACS), and therefore can illuminate how the ACS design interacts with the physics of a spacecraft. TableSat includes a mechanical structure, an on-board flight processor, attitude and rate sensors, propulsive actuators, a power supply, and a communications system. These features allow the user to simulate an ideal spacecraft with unlimited power, attitude control, communications, etc., as well as a true-to-life spacecraft that includes realistic limitations on these subsystems. In this manner, students can see how neglecting things, such as friction, to allow for a linear analysis, can often have detrimental effects on the controllability and stability of the real system. TableSat was originally designed as a demonstration tool for a class held at NASA’s Goddard Space Flight Center entitled Attitude Control Systems for Non- ACS Engineers. The purpose of this class was to provide a general overview of what attitude control

4

system engineers do. This initial version of TableSat provided a way to demonstrate how the theoretical design of an attitude control system manifests on an actual spacecraft. 1.4

Thesis Outline The thesis is divided into 11 chapters. Basic literature about TableSat is reviewed

in chapter 2. Chapter 3 describes the hardware development and circuit designing of TableSat. TableSat’s basic model is presented in chapter 4. Linear model development of TableSat is discussed in chapter 5. Chapter 6 includes the MATLAB simulation and controller designing part of the TableSat. Chapter 7 briefly explains the onboard programming of TableSat. Chapter 8 presents the real time results of the TableSat. Chapter 9 includes the timeline for 7th and 8th semesters. Chapter 10 present the financial report of the whole project. 11th chapter conclude the whole thesis.

5

2. LITERATURE REVIEW

This chapter includes a brief overview of the major topics involved in the thesis. 2.1

Time Optimal Attitude Control of a Rigid Body Spacecraft The time optimal control of spacecraft has received consistent interest as rapid

attitude maneuvers are critical to various space missions such as military observation and satellite communication. The objective is to reorient the attitude of the spacecraft in a minimal maneuver time with constrained control moments. While the spacecraft has to be considered as a rigid body i.e. an idealization of a solid body in which deformation is neglected. 2.2

Fault Tolerant Control Fault Tolerant Control System (FTCS) is a type of control which is capable of

controlling the system even if there is any fault in the components of a system like sensors and actuators. FTCS can be classified in to two types;  Passive FTCS  Active FTCS In passive FTCS, controller is so much robust that it can accommodate the modelled faults without any alteration in the control system. While in active FTCS, control variables are altered in the case of the modelled faults in the system. So active FTCS has two parts;

6

 Fault detection and diagnosis  Control reconfiguration In the fault detection and diagnosis part, system is capable of detecting the fault and diagnosing it. This diagnosis can include many things according to the situation, some of them can be like these;  Switching off the faulty part/sensor.  Turning on the replacement of the faulty part/sensor.  Applying offset calibration routines to the faulty sensor and so on. The second part in active FTCS is control reconfiguration. In this part FTCS decides how controller has to be reconfigured to cater the current faulty situation. A block diagram representation of active FTCS is given below;

Fig. 2.1. Active FTCS

7

In this block diagram, supervisor is the part which decides which control variable has to be reconfigured and how it has to be reconfigured, all depending on the current faulty situation. 2.3

PD Controller A Proportional-Derivative controller is a control loop feedback controller. It

calculates an error value based on a measured process variable and the set point, and then tries to minimize this error. The term proportional P, depends on the present error and Derivative D, depends on the prediction of future errors based on current rate of change. The weighted sum of these two actions is used to adjust the process via control element such as angular position or angular rate of the TableSat. It can be represented in discrete form as;

𝑢(𝑡) = 𝑘𝑝 𝑒(𝑡) + 𝑘𝑑

𝑑 𝑒(𝑡) 𝑑𝑡

Where kp proportional term and kd is derivative term, e is error as a function of time t, u in control input which also varies with time. 2.4

State observer A state observer estimates the state variables based on the measurement of the

output and control variables. In many practical cases, the observed state vector 𝑥̂ is used in the state feedback to generate the desired control vector. Consider the plant defined by; 𝑥(𝑡 + 1) = 𝐴 𝑥(𝑡) + 𝐵 𝑢(𝑡) 𝑦(𝑡) = 𝐶 𝑥(𝑡) 8

The observer is a subsystem to reconstruct the state vector of the plant. The mathematical model of the state observer is basically the same as that of the plant, except that we include an additional term that includes the estimator error to compensate for the inaccuracies in matrices A and B and the lack of the initial error. The estimation error or the observation error is the difference between the measured output and the estimated output. The initial error is the difference between the initial state and the initial estimated state. Thus we can define the mathematical model of the observer as; 𝑥̂ (𝑡 + 1) = 𝐴𝑥̂(𝑡) + 𝐵 𝑢(𝑡) + 𝐿(𝑦(𝑡) − 𝑦̂(𝑡)) Where 𝑥̂ is the estimated state and 𝑦̂(𝑡) is the estimated output. The inputs to the observer are the outputs 𝑦(𝑡) and the control input 𝑢(𝑡). Matrix L, which is called the observer gain matrix, is a weighing matrix to the correction term involving the difference between the measured output 𝑦(𝑡) and the estimated output 𝑦̂(𝑡). This term continuously correct the model output and improves the performance of the observer. For control purposes the output of the observer system is fed back to the input of both the observer and the plant through the gain matrix K. 𝑢(𝑡) = −𝑘𝑥̂(𝑡) So the observer equation takes the form; 𝑥̂ (𝑡 + 1) = 𝐴𝑥̂(𝑡) − 𝐵 𝑘𝑥̂(𝑡) + 𝐿(𝑦(𝑡) − 𝑦̂(𝑡)) So the overall observer equations become; 𝑥̂ (𝑘 + 1) = (𝐴 − 𝐵𝐾)𝑥̂(𝑘) + 𝐿(𝑦(𝑘) − 𝑦̂(𝑘)) 𝑦̂ (𝑘) = 𝐶𝑥̂(𝑘) 9

Figure below shows the block diagram of the system and the state observer and it is also known as Luenberger observer.

Fig. 2.2. State Observer

10

3. TABLESAT HARDWARE AND CIRCUITRY

The complete TableSat system is pictured in fig. 3.5. The components of TableSat are; 3.1

Flight Computer Arduino UNO is used as a flight processor in TableSat. Arduino Uno (shown in fig.

3.2) is a microcontroller board based on the ATmega328. It has 14 digital input/output pins (of which 6 can be used as PWM outputs), 6 analog inputs, a 16 MHz ceramic resonator, a USB connection, a power jack, an ICSP header, and a reset button. It contains everything needed to support the microcontroller. The reason why Arduino microcontroller is preferred over Digital Signal Processor (DSP) and Programmable Interface Controllers (PIC) is explained by the a comparison graph given below;

Fig. 3.1. Microprocessor Comparison

11

Fig. 3.2. Arduino Uno

3.2

Fig. 3.3. MPU 6050

Fig. 3.4. HC-06

Sensor MPU 6050 is employed as a sensor in TableSat. The MPU-6050 sensor contains a

MEMS accelerometer and a MEMS gyro in a single chip. It is very accurate, as it contains 16-bits analog to digital conversion hardware for each channel. Therefor it captures the x, y, and z channel at the same time. The sensor uses the I2C-bus to interface with the Arduino. Only gyro is used in TableSat. It is figured in fig. 3.3. 3.3

Communication System Bluetooth Module HC-06 is serving the communication needs of TableSat. It is

shown in fig. 3.4. It is capable of sending and receiving data from pc to Arduino and vice versa. 3.4

Propulsion System Two 12 Volts and 0.88 Amperes CPU fans serve as actuators for TableSat for

providing clockwise and anti-clockwise torques. 3.5

Power System Two 1.1 mAh Ni-Cd batteries serve as a power system in TableSat.

12

Other electronic components include LM7812 voltage regulators, LM741 Operational Amplifiers, transistors, diodes, resistors and a LED.

Fig. 3.5. TableSat The hardware is mounted on a 30-cm diameter disc, which is balanced on a rod/pipe with the help of a bearing that acts as TableSat’s rotation point. It is pictured in fig. 3.6 and fig. 3.7.

Fig. 3.6. TableSat Mounting Assembly (Downside View)

Fig. 3.7. TableSat Mounting Assembly 13

The assembly shown in above figure is splitted in parts in the figure (modelled in MAYA), to explain the mounting platform more explicitly;

Fig. 3.8. TableSat Mounting Assembly

Fig. 3.9. TableSat Mounting Assembly

MAYA Model

MAYA Model

The complete physical platform of TableSat hardware is shown below with every part being separated from each other to give a better understanding to the reader (tool used is MAYA).

Fig. 3.10. TableSat MAYA Model 14

3.6

Circuit Designing

Designed circuit is shown in figure below, tool used is Fritzing;

Fig. 3.11. TableSat Circuit

15

4. SYSTEM MODELING One of the main goals of this project is to develop a model of the TableSat system which can be used to design and test controllers and estimators. Ideally, the model would be linear so that linear controllers can be designed and will behave as predicted by theory. However, TableSat, like all real systems, is subject to nonlinearities such as friction and actuator saturation. Therefore, part of the model development includes finding ways to linearize the system by compensating for and reducing the effects of those nonlinearities in the system response. This chapter discusses the development and linearization of the TableSat model.

4.1

TableSat Equations of Motion To a first approximation, TableSat is assumed to have the following equations of

motion [1]: 𝐼𝜔̇ = 𝑙𝐾𝜔𝑓 (𝑣1 − 𝑣2 ) − 𝑓𝑇𝑆 (𝜔) 𝑣̇ 1 = −∝ 𝑣1 + 𝐾𝑉𝜔̇ (𝑣𝑝 − 𝑓𝑓𝑎𝑛1 (𝑣1 )) 𝑣̇ 2 = −∝ 𝑣2 + 𝐾𝑉𝜔̇ (𝑣𝑛 − 𝑓𝑓𝑎𝑛2 (𝑣2 )) Where 𝐼 is the TableSat moment of inertia, 𝜔 is the TableSat angular velocity, 𝑣1 is the speed of the positive fan, 𝑣2 is the speed of the negative fan, 𝑙 is moment arm, 𝑓𝑇𝑆 is the TableSat friction and could be a function of 𝜔, 𝐾𝜔𝑓 is the fan speed to force constant, 𝑣𝑝 is the voltage applied to the positive fan, 𝑣𝑛 is the voltage applied to the negative fan, α is

16

the fan time constant, 𝐾𝑉𝜔̇ fan voltage to change in speed constant, and 𝑓𝑓𝑎𝑛1 , 𝑓𝑓𝑎𝑛2 are the frictions of fans, could be functions of 𝑣1 , 𝑣2 respectively.

4.2

TableSat Moment of Inertia TableSat Moment of Inertia is calculated using Creo 2.0. All the parts of the

TableSat are separately modeled in Creo Parametric with their respective dimensions and densities, then these parts are assembled in Creo Assembly to calculate the total moment of inertia of the TableSat. Creo model is shown in figures below; Inertia was found out to be 𝐼 = 0.013 𝐾𝑔. 𝑚2

Fig. 4.2. TableSat Creo Model (Downside View)

Fig. 4.1. TableSat Creo Model (Upside View) 4.3

Friction Characterization The different friction functions supposed in TableSat modeling are calculated as

follows;

17

4.3.1 TableSat Friction The TableSat Friction is composed of two parts: the static friction and the dynamic friction. The spin up-spin down tests can be used to characterize both types of TableSat friction. 4.3.1.1 Static Friction The static friction in TableSat can be represented by the voltage magnitude, applied to either the positive or the negative fan, below which TableSat will not move. The static friction, therefore, is found out to be 6 Volts. 4.3.1.2 Dynamic Friction TableSat platform disc is rotating on a deep grove ball bearing-6205. So we can calculate the dynamic friction of the TableSat by bearing characteristics. The friction moment of rolling bearings can be obtained from their bearing bore diameter, using the following equation [2]:

𝑀=

𝜇𝑃𝑑 2

Where:  M:

friction moment (N · m)

 𝜇:

friction coefficient (𝜇 = 0.0015 for deep grove ball bearings [2])

 P:

load on the bearing (P=13.4 N, total load of assembly on bearing)

 d:

bore diameter (d=0.025 m, from bearing characteristics)

18

So we get; 𝑀 = 0.01009 𝑁. 𝑚 4.3.2 Fan Friction There exists a voltage below which the fan motor cannot turn the fan blades. This friction voltage can be determined by commanding the fans with smaller and smaller voltages until they do not have enough power to overcome their friction and thus do not spin. Through testing, the friction in the fans was found to be approximately 5 Volts, resulting in an actuator dead-band ranging from 0 Volts to 5 Volts. 𝑓𝑓𝑎𝑛1 (𝑣1 ) = 𝑓𝑓𝑎𝑛2 (𝑣2 ) = 5 𝑉𝑜𝑙𝑡𝑠 4.4

TableSat Parameters Now that the TableSat inertia, TableSat friction, and fan friction have been

characterized, we wish to find nominal values for the other parameters in the equations of motion. Nominal values for remaining parameters can be determined using the hardware specifications from the fan and the TableSat equations of motion. 4.4.1 Fan Time Constant The fan time constant, α is the time it takes for the fans to reach 63% of their steady state output force for a given commanded voltage. In other words, it is a measure of how quickly the fans can respond to a change in input. Without a tachometer to measure the fan’s rotational speed, it is difficult to determine the fan time constant directly. So fan time constant was determined experimentally with the help of stop watch.

19

Both the team members did this experiment at the same time to remove human errors. A total of 30 readings were taken, and average was calculated, so it comes out to be;

𝛼 = 0.34 /𝑠 4.4.2 Voltage to Change in Fan Speed Constant To determine a nominal estimate of 𝐾𝑣𝜔̇ , some data from fan’s datasheet is taken. For a constant applied voltage, the change in fan speed will be equal to zero when the fans reach a steady state speed. Consider the fan equation from chapter 4; 𝑣̇ 1 = −∝ 𝑣1 + 𝐾𝑉𝜔̇ (𝑣𝑝 − 𝑓𝑓𝑎𝑛1 (𝑣1 )) For constant voltage it will become; 0 = −∝ 𝑣1 + 𝐾𝑉𝜔̇ (𝑣𝑝 − 𝑓𝑓𝑎𝑛1 (𝑣1 )) From fan’s datasheet; 𝑣1 = 𝑣 = 5000 𝑅𝑃𝑀 𝑣 = 30000 𝑑𝑒𝑔/𝑠 𝑣𝑝 = 𝑣𝑛 = 12 𝑉 So after calculations we get;

𝐾𝑉𝜔̇ = 1457.14

20

𝐷𝑒𝑔𝑟𝑒𝑒𝑠 (𝑠𝑒𝑐 2 )(𝑉𝑜𝑙𝑡𝑠)

4.4.3 Fan Speed to Fan Force Constant From fluid dynamics, we know that the thrust force of a fluid exiting a propulsion unit can be calculated as follows: 𝐹𝑡ℎ𝑟𝑢𝑠𝑡 = 𝑚̇𝑒 𝑉𝑒 − 𝑚̇𝑖 𝑉𝑖 Where; 𝑚̇𝑒 𝑎𝑛𝑑 𝑚̇𝑖 are output and input mass flow rates of the fluid respectively. 𝑉𝑒 𝑎𝑛𝑑 𝑉𝑖 are output and input velocities of the fluid, respectively. For TableSat, the fluid being moved is air, and we assume that the input air velocity is zero, which implies that the air coming into the fan is stationary. Strictly speaking, if TableSat has a non-zero angular velocity, this assumption would be false. However, since the input air velocity would be much less than the output air velocity, the assumption is still reasonable. Based on the above assumption, we can neglect the second term in the above equation. The output mass flow rate and fluid velocities can be calculated using the following equation: 𝑚̇ = 𝜌𝑉𝑟 = 𝜌𝐴𝑉 Where 𝑉 is defined above, ρ is the density of the fluid being moved by the propulsive unit, 𝑉𝑟 is the volumetric flow rate of the propulsion unit, and A is the area through which the fluid is being moved. So we get;

𝐹𝑡ℎ𝑟𝑢𝑠𝑡

𝜌𝑒 𝑉𝑟2 = 𝐴𝑒

𝜌𝑒 = 1.29 𝑘𝑔/𝑚3 𝐴𝑒 = 0.00581 𝑚2

21

(From fan datasheet)

𝑉𝑟 = 0.047 𝑚3 /𝑠𝑒𝑐

(From fan datasheet)

So we get; 𝐹𝑡ℎ𝑟𝑢𝑠𝑡 = 0.4909 𝑁 𝐾𝜔𝑓 =

𝐹𝑡ℎ𝑟𝑢𝑠𝑡 𝑁 𝜔 𝑑𝑒𝑔/𝑠𝑒𝑐

𝜔 is calculated experimentally when the TableSat fans are spinning at their maximum speed, it comes out to be; 𝜔 = 1080 𝑑𝑒𝑔/𝑠𝑒𝑐 So we get; 𝐾𝜔𝑓 = 0.000454

22

𝑁 𝑑𝑒𝑔/𝑠𝑒𝑐

5. LINEAR MODEL DEVELOPMENT

The equations of motion of TableSat include friction terms for the fans and for TableSat itself, making the model nonlinear in nature. The nature of these friction components were identified in Chapter 4. The goal in this chapter is to determine ways to compensate for these friction terms, effectively eliminating their contributions to the equations of motion. Without friction in the system, the equations of motion can be reduced to a linear model of TableSat. 5.1

TableSat Friction Compensation In chapter 4, static and dynamic friction of TableSat is calculated, based on these

frictions a dead-band of 0 to 6 Volts is introduced in the system. 5.2

Fan Friction Compensation Similarly based on fan friction calculated in chapter 4, a dead-band of 0 to 5 Volts

is introduced in the fan power system. 5.3

Linear Model The linear model of TableSat is created so that it can be used to design controllers

and state estimators for the system. Of course, a linear model is only valid if the system itself can be considered linear. When the friction compensation methods described above are implemented, the friction nonlinearities in the system can be virtually eliminated, which implies that 𝑓𝑇𝑆 (𝜔), 𝑓𝑓𝑎𝑛1 (𝑣1 ) and 𝑓𝑓𝑎𝑛2 (𝑣2 ) can be neglected in the TableSat equations of motions. In addition, because the static fan friction has been virtually 23

eliminated, the two separate fan equations can be reduced to one equation, representing a single bi-directional fan. The simplified equations of motion are then: 𝜃̇ = 𝜔 𝜔̇ = 𝑙

𝐾𝜔𝑓 𝐼

𝑣

𝑣̇ = −𝛼𝑣 + 𝐾𝑣𝜔̇ 𝑉 Which can be written in state space form as: 0 1 0 0 𝜃̇ 𝐾𝜔𝑓 𝜃 [𝜔̇ ] = [0 0 𝑙 ] [𝜔] + [ 0 ] 𝑉 𝐼 𝐾𝑣𝜔̇ 𝑣 𝑣̇ 0 0 −𝛼 1 𝜃 [ ]=[ 0 𝜔

𝜃 0 0 ] [𝜔 ] 1 0 𝑣

𝑥̇ = 𝐴𝑥 + 𝐵𝑉 𝑦 = 𝐶𝑥 The transfer function for angular position control is calculated as;

𝐺(𝑠) =

𝑙𝐾𝜔𝑓 𝐾𝑣𝜔̇ 𝜃(𝑠) 5.247 = 2 = 2 𝑉(𝑠) 𝐼𝑠 (𝑠 + 𝛼) 𝑠 (𝑠 + 0.34)

The transfer function for angular rate control is calculated as;

𝐻(𝑠) =

𝑙𝐾𝜔𝑓 𝐾𝑉𝜔̇ 𝜔 (𝑠) 5.247 = = 𝑉(𝑠) 𝐼𝑠 (𝑠 + 𝛼) 𝑠(𝑠 + 0.34)

24

6. MATLAB SIMULATION AND CONTROLLER DESIGNING Using transfer function from chapter 5, TableSat is simulated in MATLAB. The figure below shows the Root Locus of the open loop system G(s);

Fig. 6.1. Root Locus of TableSat Model G(s)

From the root locus it is clear that system is unstable (due to two poles in right half plane), systems response due to step command in figure below, also shows the instability of the system.

25

Fig. 6.2. Step Response of Open Loop TableSat Model G(s)

Similarly the figure below shows the Root Locus of the open loop system H(s);

Fig. 6.3. Root Locus of TableSat Model H(s) 26

From the above root locus it is clear that the system is on the verge of instability, systems response due to step command is shown in figure below. Its settling time needs to be decreased to make it time optimal.

Fig. 6.4. Step Response of Open Loop TableSat Model H(s)

6.1 Controller Designing Controllers for angular position control and angular rate control are designed for the TableSat. 6.1.1 TableSat’s Angular Position Control For TableSat angular position control, using the transfer function G(s), first PController was designed in Simulink, but it was not giving a stable system, then a PDController was designed and tuned resulting in satisfactory response of the system. PID27

controller and PI-Controller was also designed but PD-controller results were more satisfactory. So PD-Controller was selected and implemented in the real system. On real system it is applied by writing the code in Arduino IDE and burning it on Arduino UNO (flight processor). Real system implemented PD-Controller is tuned again to get the best possible settling time. PD-controller coding is shown in Appendix-B. PD-Controller designed in Simulink is shown below;

Fig. 6.5. PD-Controlled Simulink Model

And its response due to some initial conditions is shown next;

28

Theta (degrees)

Time (s) Fig. 6.6. Response of TableSat Model to Initial Conditions

So from above figure it is clear that after PD-Controller implementation, system becomes stable, but its settling time is large (approx. 1 minute), so next goal is to make it time optimal. 6.1.2 TableSat’s Angular Rate Control Luenberger observer is designed in Matlab and Simulink for TableSat as in Figure C-6 given next.

29

Fig. 6.7. Luenberger Observer for TableSat

One of the main goals of the researchers is to make the TableSat a time optimal system, to accomplish this, Linear Quadratic Regulator (LQR) controller is used to find optimal gains in Matlab by deploying the H(s) dynamics model of TableSat. The resultant time optimal response of TableSat to step command is;

Fig. 6.8. LQR Controlled TableSat's Response to Step Command

30

Luenberger observer equations are implemented in the TableSat’s microcontroller in angular rate control setup. 𝑥̂ (𝑘 + 1) = (𝐴 − 𝐵𝐾)𝑥̂(𝑘) + 𝐿(𝑦(𝑘) − 𝑦̂(𝑘)) 𝑦̂ (𝑘) = (𝐶 − 𝐷𝐾)𝑥̂(𝑘) To make the system time optimal, gain matrix K in observer equations is replaced by the optimal gains calculated from LQR in Matlab.

31

7. TABLESAT ONBOARD PROGRAMMING The controllers designed in previous chapter are coded in C++ and then implemented on TableSat by uploading them on flight processor (Arduino Uno) using Arduino IDE (Integrated Development Environment), an open-source Arduino Software (shown below in fig. 7.1) used to write code and upload it to the board. The code is shown in appendix A, B, C and D.

Fig. 7.1. Arduino IDE

7.1

Gyro Calibration A simple code is used for calibration of gyros which provides the offsets of gyros.

These offsets are employed in control law development to achieve steady rotation rates and yaw angles. This control routine initializes the hardware, and executes a loop, nominally at 10 Hz. Every execution cycle reads both gyros data sequentially, applies

32

calibration offsets to convert this data into meaningful results in radian/seconds, then calculates inputs for actuators based on a reference position fixed by the operator. The overall TableSat software is summarized in the below flowchart.

Start (Variable initialization)

Set Gyro Offsets For Gyro 1 and Gyro 2 Get Gyro Readings

Fault Detection & Control Reconfiguration

State Observer & LQR Or PD Controller

Controller Implementation

The fault tolerance part consists of 2 sub-parts i.e. fault detection and control reconfiguration. In fault detection part, TableSat’s flight processor keeps on checking whether there is a fault in a sensor. If it finds any fault, the control reconfiguration mode takes over and shift the system to second gyro, while it keeps on checking if the faulty gyro is working correctly or not, as soon as it works correctly, the system goes back to

33

fault detection mode where it takes the readings from both gyros and uses the average value. Three types of gyro errors are considered in fault detection routine; a. If a gyro shows no rate even in the presence of a disturbance b. If a gyro shows random meaning less values c. If a gyro shows rates exactly opposite to the disturbance In all the above 3 cases primary gyros readings are taken as reference and secondary gyro is compared to the primary gyro, to locate faults in it. Other sensors can also be added for better fault detection like magnetometer for north tracking, but in every case some reference has to be fixed and considered as fault free, so in this case, the simplest idea of fault tolerance is adapted. On the next page fault tolerant part of TableSat’s on board programming is summarized.

34

35

Controller implementation part is explained below, where from -0.03 to 0.03 is the allowed dead band [13] of TableSat both in both heading control (in radians) and angular rate control (in radians/sec). This dead band is due to the limitation of sensors i.e. the employed gyros in the TableSat, keep on showing readings in the range of -0.03 to 0.03 even at rest position.

Controller Implementation

If

If

If

Control Gyro = 0.03

Control Gyro 0.1 ) { cyaw[0]=yaw1[0]; } else

55

{ cyaw[0]=avgyaw; } // ///////////////////////////////// case 2 no changes if (yaw2[0]==0) { cyaw[0]=yaw1[0]; } else { cyaw[0]=avgyaw; } ///////////////////////////////// case 3 yaw opposite to torque applied if(yaw1[0]>=0 && yaw2[0]0) { Input = cyaw[0]; Input=Input* (-1);}

57

else {Input = cyaw[0];} if(cyaw[0]>=0.09) { myPID.SetTunings(aggKp, aggKi, aggKd); myPID.Compute(); analogWrite(pin2,Output); analogWrite(pin1,20); Serial.print("1:\t"); Serial.print(cyaw[0]); Serial.print("\t"); Serial.print(Output); Serial.print("\t"); } else {if(cyaw[0]>=-0.09) analogWrite(pin2,0); analogWrite(pin1,0); } if(cyaw[0]= 100 #include "Arduino.h" #else #include "WProgram.h" #endif #include PID::PID(double* Input, double* Output, double* Setpoint, double Kp, double Ki, double Kd, int ControllerDirection) { myOutput = Output; myInput = Input; mySetpoint = Setpoint; inAuto = false; PID::SetOutputLimits(20, 130);

//default output limit corresponds to

//the arduino pwm limits 60

SampleTime = 10;

//default Controller Sample Time is 0.1 seconds

PID::SetControllerDirection(ControllerDirection); PID::SetTunings(Kp, Ki, Kd); lastTime = millis()-SampleTime; } bool PID::Compute() { if(!inAuto) return false; unsigned long now = millis(); unsigned long timeChange = (now - lastTime); if(timeChange>=SampleTime) { /*Compute all the working error variables*/ double input = *myInput; double error = *mySetpoint - input; ITerm+= (ki * error); if(ITerm > outMax) ITerm= outMax; else if(ITerm < outMin) ITerm= outMin;

61

double dInput = (input - lastInput); /*Compute PID Output*/ double output = kp * error + ITerm- kd * dInput; if(output > outMax) output = outMax; else if(output < outMin) output = outMin; *myOutput = output; /*Remember some variables for next time*/ lastInput = input; lastTime = now; return true; } else return false; } void PID::SetTunings(double Kp, double Ki, double Kd) { if (Kp outMax) *myOutput = outMax; else if(*myOutput < outMin) *myOutput = outMin; if(ITerm > outMax) ITerm= outMax; else if(ITerm < outMin) ITerm= outMin; } } void PID::SetMode(int Mode) {

64

bool newAuto = (Mode == AUTOMATIC); if(newAuto == !inAuto) { /*we just went from manual to auto*/ PID::Initialize(); } inAuto = newAuto; } void PID::Initialize() { ITerm = *myOutput; lastInput = *myInput; if(ITerm > outMax) ITerm = outMax; else if(ITerm < outMin) ITerm = outMin; } void PID::SetControllerDirection(int Direction) { if(inAuto && Direction !=controllerDirection) {

65

kp = (0 - kp); ki = (0 - ki); kd = (0 - kd); } controllerDirection = Direction; } double PID::GetKp(){ return dispKp; } double PID::GetKi(){ return dispKi;} double PID::GetKd(){ return dispKd;} int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} int PID::GetDirection(){ return controllerDirection; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////end///////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////

66

14.APPENDIX C 14.1 TableSat Source Code 14.1.1 Time Optimal Angular Rate Control, Luenberger Observer and LQR #include #include "I2Cdev.h" #include #include #define pin1 10 #define pin2 11 float omega; float omegarad; float omegaradfix; float u22; #include "MPU6050_6Axis_MotionApps20.h" #define MPU6050_AUX_VDDIO

0x01 // R/W

#define MPU6050_SMPLRT_DIV

0x19 // R/W

#define MPU6050_CONFIG

0x1A // R/W

#define MPU6050_GYRO_CONFIG

0x1B // R/W 67

#define MPU6050_ACCEL_CONFIG

0x1C // R/W

#define MPU6050_FF_THR

0x1D // R/W

#define MPU6050_FF_DUR

0x1E // R/W

#define MPU6050_MOT_THR

0x1F // R/W

#define MPU6050_MOT_DUR

0x20 // R/W

#define MPU6050_ZRMOT_THR

0x21 // R/W

#define MPU6050_ZRMOT_DUR

0x22 // R/W

#define MPU6050_FIFO_EN

0x23 // R/W

#define MPU6050_I2C_MST_CTRL #define MPU6050_I2C_SLV0_ADDR #define MPU6050_I2C_SLV0_REG

0x24 // R/W 0x25 // R/W 0x26 // R/W

#define MPU6050_I2C_SLV0_CTRL

0x27 // R/W

#define MPU6050_I2C_SLV1_ADDR

0x28 // R/W

#define MPU6050_I2C_SLV1_REG

0x29 // R/W


0x2A // R/W


0x2B // R/W

#define MPU6050_I2C_SLV2_REG #define MPU6050_I2C_SLV2_CTRL

0x2C // R/W 0x2D // R/W

68


0x2E // R/W

#define MPU6050_I2C_SLV3_REG

0x2F // R/W


0x30 // R/W


0x31 // R/W

#define MPU6050_I2C_SLV4_REG #define MPU6050_I2C_SLV4_DO

0x32 // R/W 0x33 // R/W

#define MPU6050_I2C_SLV4_CTRL #define MPU6050_I2C_SLV4_DI

0x34 // R/W 0x35 // R

#define MPU6050_I2C_MST_STATUS

0x36 // R

#define MPU6050_INT_PIN_CFG

0x37 // R/W

#define MPU6050_INT_ENABLE

0x38 // R/W

#define MPU6050_INT_STATUS

0x3A // R

#define MPU6050_ACCEL_XOUT_H

0x3B // R

#define MPU6050_ACCEL_XOUT_L

0x3C // R

#define MPU6050_ACCEL_YOUT_H

0x3D // R

#define MPU6050_ACCEL_YOUT_L

0x3E // R

#define MPU6050_ACCEL_ZOUT_H

0x3F // R

#define MPU6050_ACCEL_ZOUT_L

0x40 // R

69

#define MPU6050_TEMP_OUT_H

0x41 // R

#define MPU6050_TEMP_OUT_L

0x42 // R

#define MPU6050_GYRO_XOUT_H

0x43 // R

#define MPU6050_GYRO_XOUT_L

0x44 // R

#define MPU6050_GYRO_YOUT_H

0x45 // R

#define MPU6050_GYRO_YOUT_L

0x46 // R

#define MPU6050_GYRO_ZOUT_H

0x47 // R

#define MPU6050_GYRO_ZOUT_L

0x48 // R

#define MPU6050_EXT_SENS_DATA_00 0x49 // R #define MPU6050_EXT_SENS_DATA_01 0x4A // R #define MPU6050_EXT_SENS_DATA_02 0x4B // R #define MPU6050_EXT_SENS_DATA_03 0x4C // R #define MPU6050_EXT_SENS_DATA_04 0x4D // R #define MPU6050_EXT_SENS_DATA_05 0x4E // R #define MPU6050_EXT_SENS_DATA_06 0x4F // R #define MPU6050_EXT_SENS_DATA_07 0x50 // R #define MPU6050_EXT_SENS_DATA_08 0x51 // R #define MPU6050_EXT_SENS_DATA_09 0x52 // R

70

#define MPU6050_EXT_SENS_DATA_10 0x53 // R #define MPU6050_EXT_SENS_DATA_11 0x54 // R #define MPU6050_EXT_SENS_DATA_12 0x55 // R #define MPU6050_EXT_SENS_DATA_13 0x56 // R #define MPU6050_EXT_SENS_DATA_14 0x57 // R #define MPU6050_EXT_SENS_DATA_15 0x58 // R #define MPU6050_EXT_SENS_DATA_16 0x59 // R #define MPU6050_EXT_SENS_DATA_17 0x5A // R #define MPU6050_EXT_SENS_DATA_18 0x5B // R #define MPU6050_EXT_SENS_DATA_19 0x5C // R #define MPU6050_EXT_SENS_DATA_20 0x5D // R #define MPU6050_EXT_SENS_DATA_21 0x5E // R #define MPU6050_EXT_SENS_DATA_22 0x5F // R #define MPU6050_EXT_SENS_DATA_23 0x60 // R #define MPU6050_MOT_DETECT_STATUS 0x61 // R #define MPU6050_I2C_SLV0_DO

0x63 // R/W

#define MPU6050_I2C_SLV1_DO

0x64 // R/W


0x65 // R/W

71


0x66 // R/W

#define MPU6050_I2C_MST_DELAY_CTRL 0x67 // R/W #define MPU6050_SIGNAL_PATH_RESET 0x68 // R/W #define MPU6050_MOT_DETECT_CTRL 0x69 // R/W #define MPU6050_USER_CTRL

0x6A // R/W

#define MPU6050_PWR_MGMT_1

0x6B // R/W

#define MPU6050_PWR_MGMT_2

0x6C // R/W

#define MPU6050_FIFO_COUNTH

0x72 // R/W

#define MPU6050_FIFO_COUNTL

0x73 // R/W

#define MPU6050_FIFO_R_W

0x74 // R/W

#define MPU6050_WHO_AM_I

0x75 // R

#define MPU6050_D0 0 #define MPU6050_D1 1 #define MPU6050_D2 2 #define MPU6050_D3 3 #define MPU6050_D4 4 #define MPU6050_D5 5 #define MPU6050_D6 6

72

#define MPU6050_D7 7 // AUX_VDDIO Register #define MPU6050_AUX_VDDIO MPU6050_D7 // I2C high: 1=VDD, 0=VLOGIC #define MPU6050_DLPF_CFG0

MPU6050_D0

#define MPU6050_DLPF_CFG1

MPU6050_D1

#define MPU6050_DLPF_CFG2

MPU6050_D2

#define MPU6050_EXT_SYNC_SET0 MPU6050_D3 #define MPU6050_EXT_SYNC_SET1 MPU6050_D4 #define MPU6050_EXT_SYNC_SET2 MPU6050_D5 // Combined definitions for the EXT_SYNC_SET values #define MPU6050_EXT_SYNC_SET_0 (0) #define MPU6050_EXT_SYNC_SET_1 (bit(MPU6050_EXT_SYNC_SET0)) #define MPU6050_EXT_SYNC_SET_2 (bit(MPU6050_EXT_SYNC_SET1)) #defineMPU6050_EXT_SYNC_SET_3 (bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_EXT_SYNC_SET0)) #define MPU6050_EXT_SYNC_SET_4 (bit(MPU6050_EXT_SYNC_SET2)) #defineMPU6050_EXT_SYNC_SET_5 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET0))

73

#defineMPU6050_EXT_SYNC_SET_6 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1)) #defineMPU6050_EXT_SYNC_SET_7 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_E XT_SYNC_SET0))

// Alternative names for the combined definitions. #define MPU6050_EXT_SYNC_DISABLED

MPU6050_EXT_SYNC_SET_0

#define MPU6050_EXT_SYNC_TEMP_OUT_L MPU6050_EXT_SYNC_SET_1 #define MPU6050_EXT_SYNC_GYRO_XOUT_L MPU6050_EXT_SYNC_SET_2 #define MPU6050_EXT_SYNC_GYRO_YOUT_L MPU6050_EXT_SYNC_SET_3 #define MPU6050_EXT_SYNC_GYRO_ZOUT_L MPU6050_EXT_SYNC_SET_4 #define MPU6050_EXT_SYNC_ACCEL_XOUT_L MPU6050_EXT_SYNC_SET_5 #define MPU6050_EXT_SYNC_ACCEL_YOUT_L MPU6050_EXT_SYNC_SET_6 #define MPU6050_EXT_SYNC_ACCEL_ZOUT_L MPU6050_EXT_SYNC_SET_7

// Combined definitions for the DLPF_CFG values #define MPU6050_DLPF_CFG_0 (0) #define MPU6050_DLPF_CFG_1 (bit(MPU6050_DLPF_CFG0)) 74

#define MPU6050_DLPF_CFG_2 (bit(MPU6050_DLPF_CFG1)) #defineMPU6050_DLPF_CFG_3 (bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0)) #define MPU6050_DLPF_CFG_4 (bit(MPU6050_DLPF_CFG2)) #defineMPU6050_DLPF_CFG_5 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG0)) #defineMPU6050_DLPF_CFG_6 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1)) #defineMPU6050_DLPF_CFG_7 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0 )) #define MPU6050_DLPF_260HZ MPU6050_DLPF_CFG_0 #define MPU6050_DLPF_184HZ MPU6050_DLPF_CFG_1 #define MPU6050_DLPF_94HZ

MPU6050_DLPF_CFG_2

#define MPU6050_DLPF_44HZ

MPU6050_DLPF_CFG_3


MPU6050_DLPF_CFG_4


MPU6050_DLPF_CFG_5


MPU6050_DLPF_CFG_6

#define MPU6050_DLPF_RESERVED MPU6050_DLPF_CFG_7

75

#define MPU6050_FS_SEL0 MPU6050_D3 #define MPU6050_FS_SEL1 MPU6050_D4 #define MPU6050_ZG_ST MPU6050_D5 #define MPU6050_YG_ST MPU6050_D6 #define MPU6050_XG_ST MPU6050_D7

// Combined definitions for the FS_SEL values #define MPU6050_FS_SEL_0 (0) #define MPU6050_FS_SEL_1 (bit(MPU6050_FS_SEL0)) #define MPU6050_FS_SEL_2 (bit(MPU6050_FS_SEL1)) #define MPU6050_FS_SEL_3 (bit(MPU6050_FS_SEL1)|bit(MPU6050_FS_SEL0))

// Alternative names for the combined definitions // The name uses the range in degrees per second. #define MPU6050_FS_SEL_250 MPU6050_FS_SEL_0 #define MPU6050_FS_SEL_500 MPU6050_FS_SEL_1 #define MPU6050_FS_SEL_1000 MPU6050_FS_SEL_2 #define MPU6050_FS_SEL_2000 MPU6050_FS_SEL_3

76

#define MPU6050_ACCEL_HPF0 MPU6050_D0 #define MPU6050_ACCEL_HPF1 MPU6050_D1 #define MPU6050_ACCEL_HPF2 MPU6050_D2 #define MPU6050_AFS_SEL0 MPU6050_D3 #define MPU6050_AFS_SEL1 MPU6050_D4 #define MPU6050_ZA_ST

MPU6050_D5

#define MPU6050_YA_ST

MPU6050_D6

#define MPU6050_XA_ST

MPU6050_D7

// Combined definitions for the ACCEL_HPF values #define MPU6050_ACCEL_HPF_0 (0) #define MPU6050_ACCEL_HPF_1 (bit(MPU6050_ACCEL_HPF0)) #define MPU6050_ACCEL_HPF_2 (bit(MPU6050_ACCEL_HPF1)) #defineMPU6050_ACCEL_HPF_3 (bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_HPF0)) #define MPU6050_ACCEL_HPF_4 (bit(MPU6050_ACCEL_HPF2)) #defineMPU6050_ACCEL_HPF_7 (bit(MPU6050_ACCEL_HPF2)|bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_ HPF0)) 77

// Alternative names for the combined definitions // The name uses the Cut-off frequency. #define MPU6050_ACCEL_HPF_RESET MPU6050_ACCEL_HPF_0 #define MPU6050_ACCEL_HPF_5HZ MPU6050_ACCEL_HPF_1 #define MPU6050_ACCEL_HPF_2_5HZ MPU6050_ACCEL_HPF_2 #define MPU6050_ACCEL_HPF_1_25HZ MPU6050_ACCEL_HPF_3 #define MPU6050_ACCEL_HPF_0_63HZ MPU6050_ACCEL_HPF_4 #define MPU6050_ACCEL_HPF_HOLD MPU6050_ACCEL_HPF_7

// Combined definitions for the AFS_SEL values #define MPU6050_AFS_SEL_0 (0) #define MPU6050_AFS_SEL_1 (bit(MPU6050_AFS_SEL0)) #define MPU6050_AFS_SEL_2 (bit(MPU6050_AFS_SEL1)) #defineMPU6050_AFS_SEL_3 (bit(MPU6050_AFS_SEL1)|bit(MPU6050_AFS_SEL0))

// Alternative names for the combined definitions // The name uses the full scale range for the accelerometer.

78

#define MPU6050_AFS_SEL_2G MPU6050_AFS_SEL_0 #define MPU6050_AFS_SEL_4G MPU6050_AFS_SEL_1 #define MPU6050_AFS_SEL_8G MPU6050_AFS_SEL_2 #define MPU6050_AFS_SEL_16G MPU6050_AFS_SEL_3

#define MPU6050_SLV0_FIFO_EN MPU6050_D0 #define MPU6050_SLV1_FIFO_EN MPU6050_D1 #define MPU6050_SLV2_FIFO_EN MPU6050_D2 #define MPU6050_ACCEL_FIFO_EN MPU6050_D3 #define MPU6050_ZG_FIFO_EN MPU6050_D4 #define MPU6050_YG_FIFO_EN MPU6050_D5 #define MPU6050_XG_FIFO_EN

MPU6050_D6

#define MPU6050_TEMP_FIFO_EN MPU6050_D7 #define MPU6050_I2C_MST_CLK0 MPU6050_D0 #define MPU6050_I2C_MST_CLK1 MPU6050_D1 #define MPU6050_I2C_MST_CLK2 MPU6050_D2 #define MPU6050_I2C_MST_CLK3 MPU6050_D3

79

#define MPU6050_I2C_MST_P_NSR MPU6050_D4 #define MPU6050_SLV_3_FIFO_EN MPU6050_D5 #define MPU6050_WAIT_FOR_ES MPU6050_D6 #define MPU6050_MULT_MST_EN MPU6050_D7

// Combined definitions for the I2C_MST_CLK #define MPU6050_I2C_MST_CLK_0 (0) #define MPU6050_I2C_MST_CLK_1 (bit(MPU6050_I2C_MST_CLK0)) #define MPU6050_I2C_MST_CLK_2 (bit(MPU6050_I2C_MST_CLK1)) #defineMPU6050_I2C_MST_CLK_3 (bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) #define MPU6050_I2C_MST_CLK_4 (bit(MPU6050_I2C_MST_CLK2)) #defineMPU6050_I2C_MST_CLK_5 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK0)) #defineMPU6050_I2C_MST_CLK_6 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)) #defineMPU6050_I2C_MST_CLK_7 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_ MST_CLK0))

80

#define MPU6050_I2C_MST_CLK_8 (bit(MPU6050_I2C_MST_CLK3)) #defineMPU6050_I2C_MST_CLK_9 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK0)) #defineMPU6050_I2C_MST_CLK_10 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK1)) #defineMPU6050_I2C_MST_CLK_11 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_ MST_CLK0)) #defineMPU6050_I2C_MST_CLK_12 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)) #defineMPU6050_I2C_MST_CLK_13 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_ MST_CLK0)) #defineMPU6050_I2C_MST_CLK_14 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_ MST_CLK1)) #defineMPU6050_I2C_MST_CLK_15 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_ MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) // Alternative names for the combined definitions // The names uses I2C Master Clock Speed in kHz. 81

#define MPU6050_I2C_MST_CLK_348KHZ MPU6050_I2C_MST_CLK_0 #define MPU6050_I2C_MST_CLK_333KHZ MPU6050_I2C_MST_CLK_1 #define MPU6050_I2C_MST_CLK_320KHZ MPU6050_I2C_MST_CLK_2 #define MPU6050_I2C_MST_CLK_308KHZ MPU6050_I2C_MST_CLK_3 #define MPU6050_I2C_MST_CLK_296KHZ MPU6050_I2C_MST_CLK_4 #define MPU6050_I2C_MST_CLK_286KHZ MPU6050_I2C_MST_CLK_5 #define MPU6050_I2C_MST_CLK_276KHZ MPU6050_I2C_MST_CLK_6 #define MPU6050_I2C_MST_CLK_267KHZ MPU6050_I2C_MST_CLK_7 #define MPU6050_I2C_MST_CLK_258KHZ MPU6050_I2C_MST_CLK_8 #define MPU6050_I2C_MST_CLK_500KHZ MPU6050_I2C_MST_CLK_9 #define MPU6050_I2C_MST_CLK_471KHZ MPU6050_I2C_MST_CLK_10 #define MPU6050_I2C_MST_CLK_444KHZ MPU6050_I2C_MST_CLK_11 #define MPU6050_I2C_MST_CLK_421KHZ MPU6050_I2C_MST_CLK_12 #define MPU6050_I2C_MST_CLK_400KHZ MPU6050_I2C_MST_CLK_13 #define MPU6050_I2C_MST_CLK_381KHZ MPU6050_I2C_MST_CLK_14 #define MPU6050_I2C_MST_CLK_364KHZ MPU6050_I2C_MST_CLK_15 #define MPU6050_I2C_SLV0_RW MPU6050_D7 #define MPU6050_I2C_SLV0_LEN0

MPU6050_D0

82

#define MPU6050_I2C_SLV0_LEN1

MPU6050_D1


MPU6050_D2


MPU6050_D3

#define MPU6050_I2C_SLV0_GRP

MPU6050_D4

#define MPU6050_I2C_SLV0_REG_DIS MPU6050_D5 #define MPU6050_I2C_SLV0_BYTE_SW MPU6050_D6 #define MPU6050_I2C_SLV0_EN

MPU6050_D7

// A mask for the length #define MPU6050_I2C_SLV0_LEN_MASK 0x0F #define MPU6050_I2C_SLV1_RW MPU6050_D7 #define MPU6050_I2C_SLV1_LEN0

MPU6050_D0


MPU6050_D1


MPU6050_D2


MPU6050_D3


MPU6050_D4

#define MPU6050_I2C_SLV1_REG_DIS MPU6050_D5 #define MPU6050_I2C_SLV1_BYTE_SW MPU6050_D6

83

#define MPU6050_I2C_SLV1_EN

MPU6050_D7


MPU6050_D0


MPU6050_D1


MPU6050_D2


MPU6050_D3


MPU6050_D4


MPU6050_D7


MPU6050_D0


MPU6050_D1

84


MPU6050_D2


MPU6050_D3


MPU6050_D4


MPU6050_D7

// A mask for the length #define MPU6050_I2C_SLV3_LEN_MASK 0x0F #define MPU6050_I2C_SLV4_RW MPU6050_D7 #define MPU6050_I2C_MST_DLY0

MPU6050_D0

#define MPU6050_I2C_MST_DLY1

MPU6050_D1


MPU6050_D2


MPU6050_D3


MPU6050_D4

#define MPU6050_I2C_SLV4_REG_DIS MPU6050_D5 #define MPU6050_I2C_SLV4_INT_EN MPU6050_D6 #define MPU6050_I2C_SLV4_EN

MPU6050_D7

85

// A mask for the delay #define MPU6050_I2C_MST_DLY_MASK 0x1F #define MPU6050_I2C_SLV0_NACK MPU6050_D0 #define MPU6050_I2C_SLV1_NACK MPU6050_D1 #define MPU6050_I2C_SLV2_NACK MPU6050_D2 #define MPU6050_I2C_SLV3_NACK MPU6050_D3 #define MPU6050_I2C_SLV4_NACK MPU6050_D4 #define MPU6050_I2C_LOST_ARB MPU6050_D5 #define MPU6050_I2C_SLV4_DONE MPU6050_D6 #define MPU6050_PASS_THROUGH MPU6050_D7

#define MPU6050_CLKOUT_EN

MPU6050_D0

#define MPU6050_I2C_BYPASS_EN MPU6050_D1 #define MPU6050_FSYNC_INT_EN MPU6050_D2 #define MPU6050_FSYNC_INT_LEVEL MPU6050_D3 #define MPU6050_INT_RD_CLEAR

MPU6050_D4

#define MPU6050_LATCH_INT_EN MPU6050_D5

86

#define MPU6050_INT_OPEN

MPU6050_D6

#define MPU6050_INT_LEVEL

MPU6050_D7

#define MPU6050_DATA_RDY_EN MPU6050_D0 #define MPU6050_I2C_MST_INT_EN MPU6050_D3 #define MPU6050_FIFO_OFLOW_EN MPU6050_D4 #define MPU6050_ZMOT_EN #define MPU6050_MOT_EN #define MPU6050_FF_EN

MPU6050_D5 MPU6050_D6 MPU6050_D7

#define MPU6050_DATA_RDY_INT MPU6050_D0 #define MPU6050_I2C_MST_INT MPU6050_D3 #define MPU6050_FIFO_OFLOW_INT MPU6050_D4 #define MPU6050_ZMOT_INT #define MPU6050_MOT_INT #define MPU6050_FF_INT

MPU6050_D5 MPU6050_D6 MPU6050_D7

#define MPU6050_MOT_ZRMOT MPU6050_D0 #define MPU6050_MOT_ZPOS MPU6050_D2 #define MPU6050_MOT_ZNEG MPU6050_D3 #define MPU6050_MOT_YPOS MPU6050_D4

87

#define MPU6050_MOT_YNEG MPU6050_D5 #define MPU6050_MOT_XPOS MPU6050_D6 #define MPU6050_MOT_XNEG MPU6050_D7 #define MPU6050_I2C_SLV0_DLY_EN MPU6050_D0 #define MPU6050_I2C_SLV1_DLY_EN MPU6050_D1 #define MPU6050_I2C_SLV2_DLY_EN MPU6050_D2 #define MPU6050_I2C_SLV3_DLY_EN MPU6050_D3 #define MPU6050_I2C_SLV4_DLY_EN MPU6050_D4 #define MPU6050_DELAY_ES_SHADOW MPU6050_D7 #define MPU6050_TEMP_RESET MPU6050_D0 #define MPU6050_ACCEL_RESET MPU6050_D1 #define MPU6050_GYRO_RESET MPU6050_D2 #define MPU6050_MOT_COUNT0

MPU6050_D0

#define MPU6050_MOT_COUNT1

MPU6050_D1

#define MPU6050_FF_COUNT0

MPU6050_D2

#define MPU6050_FF_COUNT1

MPU6050_D3

#define MPU6050_ACCEL_ON_DELAY0 MPU6050_D4 #define MPU6050_ACCEL_ON_DELAY1 MPU6050_D5

88

// Combined definitions for the MOT_COUNT #define MPU6050_MOT_COUNT_0 (0) #define MPU6050_MOT_COUNT_1 (bit(MPU6050_MOT_COUNT0)) #define MPU6050_MOT_COUNT_2 (bit(MPU6050_MOT_COUNT1)) #defineMPU6050_MOT_COUNT_3 (bit(MPU6050_MOT_COUNT1)|bit(MPU6050_MOT_COUNT0))

// Alternative names for the combined definitions #define MPU6050_MOT_COUNT_RESET MPU6050_MOT_COUNT_0

// Combined definitions for the FF_COUNT #define MPU6050_FF_COUNT_0 (0) #define MPU6050_FF_COUNT_1 (bit(MPU6050_FF_COUNT0)) #define MPU6050_FF_COUNT_2 (bit(MPU6050_FF_COUNT1)) #defineMPU6050_FF_COUNT_3 (bit(MPU6050_FF_COUNT1)|bit(MPU6050_FF_COUNT0)) // Alternative names for the combined definitions #define MPU6050_FF_COUNT_RESET MPU6050_FF_COUNT_0 89

// Combined definitions for the ACCEL_ON_DELAY #define MPU6050_ACCEL_ON_DELAY_0 (0) #define MPU6050_ACCEL_ON_DELAY_1 (bit(MPU6050_ACCEL_ON_DELAY0)) #define MPU6050_ACCEL_ON_DELAY_2 (bit(MPU6050_ACCEL_ON_DELAY1)) #defineMPU6050_ACCEL_ON_DELAY_3 (bit(MPU6050_ACCEL_ON_DELAY1)|bit(MPU6050_ACCEL_ON_DELAY0))

// Alternative names for the ACCEL_ON_DELAY #define MPU6050_ACCEL_ON_DELAY_0MS MPU6050_ACCEL_ON_DELAY_0 #define MPU6050_ACCEL_ON_DELAY_1MS MPU6050_ACCEL_ON_DELAY_1 #define MPU6050_ACCEL_ON_DELAY_2MS MPU6050_ACCEL_ON_DELAY_2 #define MPU6050_ACCEL_ON_DELAY_3MS MPU6050_ACCEL_ON_DELAY_3 #define MPU6050_SIG_COND_RESET MPU6050_D0 #define MPU6050_I2C_MST_RESET MPU6050_D1 #define MPU6050_FIFO_RESET

MPU6050_D2

#define MPU6050_I2C_IF_DIS

MPU6050_D4 // must be 0 for MPU-6050

#define MPU6050_I2C_MST_EN

MPU6050_D5

#define MPU6050_FIFO_EN

MPU6050_D6

#define MPU6050_CLKSEL0

MPU6050_D0 90


MPU6050_D1


MPU6050_D2

#define MPU6050_TEMP_DIS

MPU6050_D3

#define MPU6050_CYCLE #define MPU6050_SLEEP

MPU6050_D5 MPU6050_D6

// 1: disable temperature sensor // 1: sample and sleep

// 1: sleep mode

#define MPU6050_DEVICE_RESET MPU6050_D7

// 1: reset to default values

// Combined definitions for the CLKSEL #define MPU6050_CLKSEL_0 (0) #define MPU6050_CLKSEL_1 (bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_2 (bit(MPU6050_CLKSEL1)) #define MPU6050_CLKSEL_3 (bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_4 (bit(MPU6050_CLKSEL2)) #define MPU6050_CLKSEL_5 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_6 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1)) #defineMPU6050_CLKSEL_7 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0)) // Alternative names for the combined definitions #define MPU6050_CLKSEL_INTERNAL MPU6050_CLKSEL_0 91

#define MPU6050_CLKSEL_X

MPU6050_CLKSEL_1

#define MPU6050_CLKSEL_Y

MPU6050_CLKSEL_2

#define MPU6050_CLKSEL_Z

MPU6050_CLKSEL_3

#define MPU6050_CLKSEL_EXT_32KHZ MPU6050_CLKSEL_4 #define MPU6050_CLKSEL_EXT_19_2MHZ MPU6050_CLKSEL_5 #define MPU6050_CLKSEL_RESERVED MPU6050_CLKSEL_6 #define MPU6050_CLKSEL_STOP

MPU6050_CLKSEL_7

#define MPU6050_STBY_ZG

MPU6050_D0

#define MPU6050_STBY_YG

MPU6050_D1

#define MPU6050_STBY_XG

MPU6050_D2

#define MPU6050_STBY_ZA

MPU6050_D3

#define MPU6050_STBY_YA

MPU6050_D4

#define MPU6050_STBY_XA

MPU6050_D5

#define MPU6050_LP_WAKE_CTRL0 MPU6050_D6 #define MPU6050_LP_WAKE_CTRL1 MPU6050_D7 // Combined definitions for the LP_WAKE_CTRL #define MPU6050_LP_WAKE_CTRL_0 (0)

92

#define MPU6050_LP_WAKE_CTRL_1 (bit(MPU6050_LP_WAKE_CTRL0)) #define MPU6050_LP_WAKE_CTRL_2 (bit(MPU6050_LP_WAKE_CTRL1)) #defineMPU6050_LP_WAKE_CTRL_3 (bit(MPU6050_LP_WAKE_CTRL1)|bit(MPU6050_LP_WAKE_CTRL0)) #define MPU6050_LP_WAKE_1_25HZ MPU6050_LP_WAKE_CTRL_0 #define MPU6050_LP_WAKE_2_5HZ MPU6050_LP_WAKE_CTRL_1 #define MPU6050_LP_WAKE_5HZ

MPU6050_LP_WAKE_CTRL_2

#define MPU6050_LP_WAKE_10HZ MPU6050_LP_WAKE_CTRL_3 #define MPU6050_I2C_ADDRESS 0x68 MPU6050 mpu1(0x69); #define OUTPUT_READABLE_YAWPITCHROLL #define LED_PIN 13 bool blinkState = false; bool dmpReady = false; uint8_t mpuIntStatus; uint8_t devStatus; uint16_t packetSize; uint16_t fifoCount; uint8_t fifoBuffer[64]; 93

Quaternion q;

// [w, x, y, z]

quaternion container

VectorInt16 aa;

// [x, y, z]

accel sensor measurements

VectorInt16 aaReal;

// [x, y, z]

VectorInt16 aaWorld; VectorFloat gravity;

gravity-free accel sensor measurements

// [x, y, z]

world-frame accel sensor measurements

// [x, y, z]

gravity vector

float euler[3];

// [psi, theta, phi]

Euler angle container

float ypr[3];

// [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

float cyaw[3]; double u2; ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////State

Space

Model////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////////////////////////////////// float A[2][2] = { {0.9666,1.3983}, {0,1} }; float B[2][1] ={ {0}, {0.25}

94

}; float C[1][2] = { {1,0} }; float xh[2][1] = { {0}, {0} }; float k2[1][2] = { /////////////////////////////////////////////////////////////////LQR GAIN MATRIX {0.98,3.2} }; float L2[2][1] = { {10}, {10} }; //////////////////////////////////////////////////////////////////////////////////////////////////////// // packet structure for InvenSense teapot demo uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

95

typedef union accel_t_gyro_union { struct { uint8_t x_accel_h; uint8_t x_accel_l; uint8_t y_accel_h; uint8_t y_accel_l; uint8_t z_accel_h; uint8_t z_accel_l; uint8_t t_h; uint8_t t_l; uint8_t x_gyro_h; uint8_t x_gyro_l; uint8_t y_gyro_h; uint8_t y_gyro_l; uint8_t z_gyro_h; uint8_t z_gyro_l;

96

} reg; struct { int16_t x_accel; int16_t y_accel; int16_t z_accel; int16_t temperature; int16_t x_gyro; int16_t y_gyro; int16_t z_gyro; } value; }; volatile bool mpuInterrupt = false;

// indicates whether MPU interrupt pin has gone

high void dmpDataReady() { mpuInterrupt = true; } void setup() { 97

int error; uint8_t c; Serial.begin(9600); // Initialize the 'Wire' class for the I2C-bus. Wire.begin(); error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1); error = MPU6050_read (MPU6050_PWR_MGMT_1, &c, 1); // Clear the 'sleep' bit to start the sensor. MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0); /////////////////////////////////////////////// // join I2C bus (I2Cdev library doesn't do this automatically) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif mpu1.initialize(); devStatus = mpu1.dmpInitialize();

98

////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////Gyro Offsets/////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////

mpu1.setXGyroOffset(83); mpu1.setYGyroOffset(-67); mpu1.setZGyroOffset(-12); mpu1.setZAccelOffset(1860); // 1688 factory default for my test chip // make sure it worked (returns 0 if so) if (devStatus == 0) { mpu1.setDMPEnabled(true); attachInterrupt(0, dmpDataReady, RISING); mpuIntStatus = mpu1.getIntStatus(); dmpReady = true; packetSize = mpu1.dmpGetFIFOPacketSize(); } } void loop()

99

{ int error; double dT; accel_t_gyro_union accel_t_gyro; Serial.println(F("")); error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro)); uint8_t swap; #define SWAP(x,y) swap = x; x = y; y = swap SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l); SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l); SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l); SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l); SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l); SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l); SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l); //Serial.println(accel_t_gyro.value.x_gyro/131, DEC); //omega=(accel_t_gyro.value.x_gyro/131)*0.017453; omega=(accel_t_gyro.value.x_gyro/131); 100

omegarad=omega*0.017453; omegaradfix=omegarad+0.03; //Serial.print("omega rad/s\t"); //Serial.println(omegaradfix); // if programming failed, don't try to do anything if (!dmpReady) return; // reset interrupt flag and get INT_STATUS byte mpuInterrupt = false; mpuIntStatus = mpu1.getIntStatus(); // get current FIFO count fifoCount = mpu1.getFIFOCount(); // check for overflow (this should never happen unless our code is too inefficient) if ((mpuIntStatus & 0x10) || fifoCount == 1024) { // reset so we can continue cleanly mpu1.resetFIFO(); Serial.println(F("FIFO overflow!")); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (mpuIntStatus & 0x02) {

101

// wait for correct available data length, should be a VERY short wait while (fifoCount < packetSize) fifoCount = mpu1.getFIFOCount(); // read a packet from FIFO mpu1.getFIFOBytes(fifoBuffer, packetSize); fifoCount -= packetSize; #ifdef OUTPUT_READABLE_YAWPITCHROLL // display Euler angles in degrees mpu1.dmpGetQuaternion(&q, fifoBuffer); mpu1.dmpGetGravity(&gravity, &q); mpu1.dmpGetYawPitchRoll(ypr, &q, &gravity); #endif } ///////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////Defining Sample Time/////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////

int sampletime = 100; int lasttime = millis()-sampletime; unsigned long Now = millis();

102

unsigned long timechange = (Now - lasttime); if(timechange>=sampletime) { ////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////Luenberger Observer//////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////

float a2bk1=A[0][0]-(B[0][0]*k2[0][0]); float a2bk2=A[0][1]-(B[0][0]*k2[0][1]); float a2bk3=A[1][0]-(B[1][0]*k2[0][0]); float a2bk4=A[1][1]-(B[1][0]*k2[0][1]);

float a2bkLC1=a2bk1-(L2[0][0]*C[0][0]); float a2bkLC2=a2bk2-(L2[0][0]*C[0][1]); float a2bkLC3=a2bk3-(L2[1][0]*C[0][0]); float a2bkLC4=a2bk4-(L2[1][0]*C[0][1]);

float a2bkLCx[2][1]={ {(a2bkLC1*xh[0][0]) + (a2bkLC2*xh[1][0])},

103

{(a2bkLC3*xh[0][0]) + (a2bkLC4*xh[1][0])} };

float ly2[2][1]= { {L2[0][0]*( (omegaradfix))}, {L2[1][0]*( (omegaradfix))} };

float xh2[2][1]= { {a2bkLCx[0][0] + ly2[0][0]}, {a2bkLCx[1][0] + ly2[1][0]} };

double u2 = ( (k2[0][0]*xh2[0][0]) + (k2[0][1]*xh2[1][0]) ); if(u2>0) { u2= constrain(u2, 0, 110); }

104

if(u2=0.01) { analogWrite(pin2,u2); //Serial.print("fan1 input\t"); // Serial.println(u2); analogWrite(pin1,0); lasttime = Now; int t=millis(); t=t/1000; Serial.print(t); Serial.print("\t"); Serial.print(omegaradfix); Serial.print("\t");

105

Serial.print(ypr[0]); Serial.print("\t"); Serial.print(u2); Serial.print("\t"); } else { if(ypr[0]>=-0.01) { // Serial.println(ypr[0]); analogWrite(pin2,0); analogWrite(pin1,0); } } if(ypr[0]

time optimal spacecraft attitude control with

time optimal spacecraft attitude control with

Suggest Documents

Adaptive Spacecraft Attitude Tracking Control with ... - CiteSeerX

Spacecraft Digital Attitude Control with Automatic ...

Adaptive Spacecraft Attitude Tracking Control with ... - CiteSeerX

On Spacecraft Magnetic Attitude Control

Fault-Tolerant Attitude Control System for a Spacecraft with Control ...

Preserving Spacecraft Attitude Control Accuracy ...

Robust Control Design for Spacecraft Attitude ...

Radiometric Actuators for Spacecraft Attitude Control

Attitude Tracking Control for Spacecraft Formation ... - dept.aoe.vt.edu

Radiometric Actuators for Spacecraft Attitude Control - arXiv

Fault-tolerant spacecraft attitude control system

Integrated Spacecraft Power and Attitude Control ... - dept.aoe.vt.edu

ROBUST SPACECRAFT ATTITUDE CONTROL ... - John L. Crassidis

Finite-Time Attitude Tracking of Spacecraft With Fault ... - IEEE Xplore

Attitude Dynamics/Control of Dual-Body Spacecraft with ... - CiteSeerX

Attitude Dynamics of Spacecraft with Control by Relocatable ... - IAENG

Time-Optimal Control of Axi-Symmetric Rigid Spacecraft ... - CiteSeerX

Optimal Control with Noisy Time

Adaptive Spacecraft Attitude Tracking Control with Actuator - CiteSeerX

Robust attitude control of spacecraft with magnetic ...

Attitude Control of Spacecraft by NMPC with ...

Adaptive Spacecraft Attitude Tracking Control with Actuator ... - DCSL

time-optimal control of axi-symmetric rigid spacecraft - CiteSeerX

DES Spacecraft drag-free attitude control system design with ...