mag n gyro included

Dependencies:   mbed-rtos mbed

Fork of BAE_vr2_gingerbread2 by Seeker of Truth ,

Committer:
sakthipriya
Date:
Wed Dec 17 09:46:36 2014 +0000
Revision:
2:7aede71f4c22
Parent:
1:bd715ccef1bb
Child:
3:3d9e5f48b0c1
watchdog added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakthipriya 0:e9c32e1df869 1 #include "HK.h"
sakthipriya 0:e9c32e1df869 2
sakthipriya 0:e9c32e1df869 3
sakthipriya 0:e9c32e1df869 4 //GPIO pins used=> D2-D12, A0-A1
sakthipriya 0:e9c32e1df869 5
sakthipriya 0:e9c32e1df869 6 DigitalOut SelectLinesA[]={D2,D3,D4,D5};//to mux1=>voltage mux
sakthipriya 0:e9c32e1df869 7 DigitalOut SelectLinesB[]={PTB18,PTB19,PTC10};//to mux2=>current mux(differential mux)
sakthipriya 1:bd715ccef1bb 8 DigitalOut SelectLinesC[]={PTC0,PTC13,PTC6,PTC16};//to mux3=>temp mux
sakthipriya 0:e9c32e1df869 9
sakthipriya 0:e9c32e1df869 10 //--------------------------------------------MSB is SelectLines[0],LSB is SelectLines[3]--------------------------------
sakthipriya 0:e9c32e1df869 11
sakthipriya 0:e9c32e1df869 12 AnalogIn CurrentInput(A0); // Input from Current Mux
sakthipriya 0:e9c32e1df869 13 AnalogIn VoltageInput(A1); // Input from Voltage Multiplexer
sakthipriya 0:e9c32e1df869 14 AnalogIn TemperatureInput(A2); /*Input from Temperature Multiplexer,thermistor Multiplexer- same multiplexer for both(lines 1-4 for thermistor,line 0 for temperature sensor)*/
sakthipriya 0:e9c32e1df869 15
sakthipriya 0:e9c32e1df869 16
sakthipriya 0:e9c32e1df869 17
sakthipriya 0:e9c32e1df869 18
sakthipriya 0:e9c32e1df869 19 int quantiz(float start,float step,float x) // accepts min and measured values and step->quantises on a scale 0-15..(4 bit quantisation)
sakthipriya 0:e9c32e1df869 20 {
sakthipriya 0:e9c32e1df869 21 int y=(x-start)/step;
sakthipriya 0:e9c32e1df869 22 if(y<=0)y=0;
sakthipriya 0:e9c32e1df869 23 if(y>=15)y=15;
sakthipriya 0:e9c32e1df869 24 return y;
sakthipriya 0:e9c32e1df869 25 }
sakthipriya 0:e9c32e1df869 26
sakthipriya 0:e9c32e1df869 27 void init_beacon(ShortBeacy* x,SensorDataQuantised y)
sakthipriya 0:e9c32e1df869 28 {
sakthipriya 0:e9c32e1df869 29 (*x).Voltage[0]=2;//quantised value
sakthipriya 0:e9c32e1df869 30 (*x).Temp[0]=y.PanelTemperature[0];//quantised value
sakthipriya 0:e9c32e1df869 31 (*x).Temp[1]=y.PanelTemperature[1];//quantised value
sakthipriya 0:e9c32e1df869 32 (*x).AngularSpeed[0]=y.AngularSpeed[0];
sakthipriya 0:e9c32e1df869 33 (*x).AngularSpeed[1]=y.AngularSpeed[1];
sakthipriya 0:e9c32e1df869 34
sakthipriya 0:e9c32e1df869 35 (*x).SubsystemStatus[0]=145;//dummy values----------to be changed-------------------
sakthipriya 0:e9c32e1df869 36 (*x).ErrorFlag[0]=3;//dummy values----------to be changed-------------------
sakthipriya 0:e9c32e1df869 37 }
sakthipriya 0:e9c32e1df869 38
sakthipriya 0:e9c32e1df869 39 SensorData Sensor;
sakthipriya 0:e9c32e1df869 40 SensorDataQuantised SensorQuantised;
sakthipriya 0:e9c32e1df869 41 ShortBeacy Shortbeacon;
sakthipriya 0:e9c32e1df869 42 void FUNC_HK_MAIN()
sakthipriya 0:e9c32e1df869 43 {
sakthipriya 0:e9c32e1df869 44 //define structure variables
sakthipriya 0:e9c32e1df869 45
sakthipriya 0:e9c32e1df869 46
sakthipriya 0:e9c32e1df869 47
sakthipriya 0:e9c32e1df869 48
sakthipriya 0:e9c32e1df869 49 //initialise all selectlines to zeroes->1st line of muxes selected
sakthipriya 0:e9c32e1df869 50 SelectLinesA[0]=SelectLinesA[1]=SelectLinesA[2]=SelectLinesA[3]=0;
sakthipriya 0:e9c32e1df869 51 SelectLinesB[0]=SelectLinesB[1]=SelectLinesB[2]=0;
sakthipriya 0:e9c32e1df869 52 SelectLinesC[0]=SelectLinesC[1]=SelectLinesC[2]=SelectLinesC[3]=0;
sakthipriya 0:e9c32e1df869 53
sakthipriya 0:e9c32e1df869 54 int LoopIterator;
sakthipriya 0:e9c32e1df869 55 int SelectLineIterator;
sakthipriya 0:e9c32e1df869 56
sakthipriya 0:e9c32e1df869 57 float resistance_thermistor,voltage_thermistor;//for thermistor
sakthipriya 0:e9c32e1df869 58
sakthipriya 0:e9c32e1df869 59 //measurement from voltage sensor=> 16 sensors in place
sakthipriya 0:e9c32e1df869 60 for(LoopIterator=0; LoopIterator<16; LoopIterator++)
sakthipriya 0:e9c32e1df869 61 {
sakthipriya 0:e9c32e1df869 62 //following lines read the sensor values and stores them in 'SensorData' structure's variable 'Sensor'
sakthipriya 0:e9c32e1df869 63 Sensor.Voltage[LoopIterator]=(VoltageInput.read()*3.3*5.545454);//resistors in voltage divider=>15Mohm,3.3Mohm
sakthipriya 0:e9c32e1df869 64
sakthipriya 0:e9c32e1df869 65 if(LoopIterator%2==0)
sakthipriya 0:e9c32e1df869 66 SensorQuantised.Voltage[LoopIterator/2]=quantiz(vstart,vstep,Sensor.Voltage[LoopIterator]);
sakthipriya 0:e9c32e1df869 67
sakthipriya 0:e9c32e1df869 68 else
sakthipriya 0:e9c32e1df869 69 SensorQuantised.Voltage[(LoopIterator)/2]=SensorQuantised.Voltage[(LoopIterator)/2]<<4+quantiz(vstart,vstep,Sensor.Voltage[LoopIterator]);
sakthipriya 0:e9c32e1df869 70
sakthipriya 0:e9c32e1df869 71
sakthipriya 0:e9c32e1df869 72
sakthipriya 0:e9c32e1df869 73 // The following lines are used to iterate the select lines from 0 to 15
sakthipriya 0:e9c32e1df869 74 //following is an algorithm similar to counting binary numbers of 4 bit
sakthipriya 0:e9c32e1df869 75 for(SelectLineIterator=3;SelectLineIterator>=0;SelectLineIterator--)
sakthipriya 0:e9c32e1df869 76 {
sakthipriya 0:e9c32e1df869 77 if(SelectLinesA[SelectLineIterator]==0)
sakthipriya 0:e9c32e1df869 78 {
sakthipriya 0:e9c32e1df869 79 SelectLinesA[SelectLineIterator]=1;
sakthipriya 0:e9c32e1df869 80 break;
sakthipriya 0:e9c32e1df869 81 }
sakthipriya 0:e9c32e1df869 82 else SelectLinesA[SelectLineIterator]=0;
sakthipriya 0:e9c32e1df869 83
sakthipriya 0:e9c32e1df869 84 }
sakthipriya 0:e9c32e1df869 85
sakthipriya 0:e9c32e1df869 86
sakthipriya 0:e9c32e1df869 87 wait_us(10.0); // A delay of 10 microseconds between each sensor output. Can be changed.
sakthipriya 0:e9c32e1df869 88
sakthipriya 0:e9c32e1df869 89 }
sakthipriya 0:e9c32e1df869 90
sakthipriya 0:e9c32e1df869 91
sakthipriya 0:e9c32e1df869 92
sakthipriya 0:e9c32e1df869 93
sakthipriya 0:e9c32e1df869 94
sakthipriya 0:e9c32e1df869 95 //measurement from current sensor=> 8 sensors in place
sakthipriya 0:e9c32e1df869 96
sakthipriya 0:e9c32e1df869 97 for(LoopIterator=0; LoopIterator<8; LoopIterator++)
sakthipriya 0:e9c32e1df869 98 {
sakthipriya 0:e9c32e1df869 99 //following lines read the sensor values and stores them in 'SensorData' structure variable 'Sensor'
sakthipriya 0:e9c32e1df869 100 Sensor.Current[LoopIterator]=(CurrentInput.read()*3.3/(50*rsens));
sakthipriya 0:e9c32e1df869 101 if(LoopIterator%2==0)
sakthipriya 0:e9c32e1df869 102 SensorQuantised.Current[LoopIterator/2]=quantiz(cstart,cstep,Sensor.Current[LoopIterator]);
sakthipriya 0:e9c32e1df869 103 else
sakthipriya 0:e9c32e1df869 104 SensorQuantised.Current[(LoopIterator)/2]=SensorQuantised.Current[(LoopIterator)/2]<<4+quantiz(cstart,cstep,Sensor.Current[LoopIterator]);
sakthipriya 0:e9c32e1df869 105
sakthipriya 0:e9c32e1df869 106
sakthipriya 0:e9c32e1df869 107 // The following lines are used to iterate the select lines from 0 to 7
sakthipriya 0:e9c32e1df869 108 //following is an algorithm similar to counting binary numbers of 3 bits
sakthipriya 0:e9c32e1df869 109 for(SelectLineIterator=2;SelectLineIterator>=0;SelectLineIterator--)
sakthipriya 0:e9c32e1df869 110 {
sakthipriya 0:e9c32e1df869 111 if(SelectLinesB[SelectLineIterator]==0)
sakthipriya 0:e9c32e1df869 112 {
sakthipriya 0:e9c32e1df869 113 SelectLinesB[SelectLineIterator]=1;
sakthipriya 0:e9c32e1df869 114 break;
sakthipriya 0:e9c32e1df869 115 }
sakthipriya 0:e9c32e1df869 116 else SelectLinesB[SelectLineIterator]=0;
sakthipriya 0:e9c32e1df869 117
sakthipriya 0:e9c32e1df869 118 }
sakthipriya 0:e9c32e1df869 119
sakthipriya 0:e9c32e1df869 120
sakthipriya 0:e9c32e1df869 121 wait_us(10.0); // A delay of 10 microseconds between each sensor output. Can be changed.
sakthipriya 0:e9c32e1df869 122
sakthipriya 0:e9c32e1df869 123 }
sakthipriya 0:e9c32e1df869 124
sakthipriya 0:e9c32e1df869 125
sakthipriya 0:e9c32e1df869 126 //measurement of temperature
sakthipriya 0:e9c32e1df869 127 //temperature measurement=> 4 thermistors, 1 temperature sensor
sakthipriya 0:e9c32e1df869 128 //mux line 1=>temp sensor, mux lines 2 to 5 =>thermistors
sakthipriya 0:e9c32e1df869 129
sakthipriya 0:e9c32e1df869 130 for(LoopIterator=0; LoopIterator<5; LoopIterator++)
sakthipriya 0:e9c32e1df869 131 {
sakthipriya 0:e9c32e1df869 132 //following lines read the sensor values and stores them in 'SensorData' structure variable 'Sensor'
sakthipriya 0:e9c32e1df869 133 Sensor.Temperature[LoopIterator]=(-90.7*3.3*TemperatureInput.read()+190.1543);
sakthipriya 0:e9c32e1df869 134 voltage_thermistor=TemperatureInput.read()*3.3;//voltage across thermistor
sakthipriya 0:e9c32e1df869 135 resistance_thermistor=24000*voltage_thermistor/(3.3-voltage_thermistor);//resistance of thermistor
sakthipriya 0:e9c32e1df869 136 //PanelTemperature will be updated depending on voltage_thermistor value later in the lines to follow
sakthipriya 0:e9c32e1df869 137
sakthipriya 0:e9c32e1df869 138 if(LoopIterator%2==0)
sakthipriya 0:e9c32e1df869 139 {
sakthipriya 0:e9c32e1df869 140 if(LoopIterator<1) //->corresponding to temperature sensor
sakthipriya 0:e9c32e1df869 141 SensorQuantised.Temperature[(LoopIterator)/2]=quantiz(tstart,tstep,Sensor.Temperature[LoopIterator]);
sakthipriya 0:e9c32e1df869 142
sakthipriya 0:e9c32e1df869 143 else //->corresponding to thermistor
sakthipriya 0:e9c32e1df869 144 {
sakthipriya 0:e9c32e1df869 145 if(voltage_thermistor<1.378) //Temperature>12 degC
sakthipriya 0:e9c32e1df869 146 Sensor.PanelTemperature[(LoopIterator-1)]=(3694/log(24.032242*resistance_thermistor));
sakthipriya 0:e9c32e1df869 147
sakthipriya 0:e9c32e1df869 148 else
sakthipriya 0:e9c32e1df869 149 Sensor.PanelTemperature[(LoopIterator-1)]=(3365.4792/log(7.60404*resistance_thermistor));
sakthipriya 0:e9c32e1df869 150
sakthipriya 0:e9c32e1df869 151
sakthipriya 0:e9c32e1df869 152 SensorQuantised.PanelTemperature[(LoopIterator-1)/2]=quantiz(tstart_thermistor,tstep_thermistor,Sensor.PanelTemperature[(LoopIterator-1)]);
sakthipriya 0:e9c32e1df869 153
sakthipriya 0:e9c32e1df869 154 }
sakthipriya 0:e9c32e1df869 155
sakthipriya 0:e9c32e1df869 156 }
sakthipriya 0:e9c32e1df869 157
sakthipriya 0:e9c32e1df869 158 else
sakthipriya 0:e9c32e1df869 159 {
sakthipriya 0:e9c32e1df869 160 if(LoopIterator<1)
sakthipriya 0:e9c32e1df869 161 SensorQuantised.Temperature[(LoopIterator)/2]=SensorQuantised.Temperature[(LoopIterator)/2]<<4+quantiz(tstart,tstep,Sensor.Temperature[LoopIterator]);
sakthipriya 0:e9c32e1df869 162
sakthipriya 0:e9c32e1df869 163 else
sakthipriya 0:e9c32e1df869 164 {
sakthipriya 0:e9c32e1df869 165 if(voltage_thermistor<1.378) //Temperature>12 degC
sakthipriya 0:e9c32e1df869 166 Sensor.PanelTemperature[LoopIterator-1]=(3694/log(24.032242*resistance_thermistor));
sakthipriya 0:e9c32e1df869 167
sakthipriya 0:e9c32e1df869 168
sakthipriya 0:e9c32e1df869 169 else
sakthipriya 0:e9c32e1df869 170 Sensor.PanelTemperature[LoopIterator-1]=(3365.4792/log(7.60404*resistance_thermistor));
sakthipriya 0:e9c32e1df869 171
sakthipriya 0:e9c32e1df869 172 SensorQuantised.PanelTemperature[(LoopIterator-1)/2]=SensorQuantised.PanelTemperature[(LoopIterator-1)/2]<<4+quantiz(tstart_thermistor,tstep_thermistor,Sensor.PanelTemperature[LoopIterator-1]);
sakthipriya 0:e9c32e1df869 173
sakthipriya 0:e9c32e1df869 174 }
sakthipriya 0:e9c32e1df869 175
sakthipriya 0:e9c32e1df869 176 }
sakthipriya 0:e9c32e1df869 177
sakthipriya 2:7aede71f4c22 178 strcpy(SensorQuantised.Voltage,"green");
sakthipriya 0:e9c32e1df869 179
sakthipriya 0:e9c32e1df869 180 // The following lines are used to iterate the select lines from 0 to 4
sakthipriya 0:e9c32e1df869 181
sakthipriya 0:e9c32e1df869 182 //following is an algorithm similar to counting binary numbers of 4 bit
sakthipriya 0:e9c32e1df869 183 for(SelectLineIterator=3;SelectLineIterator>=0;SelectLineIterator--)
sakthipriya 0:e9c32e1df869 184 {
sakthipriya 0:e9c32e1df869 185 if(SelectLinesC[SelectLineIterator]==0)
sakthipriya 0:e9c32e1df869 186 {
sakthipriya 0:e9c32e1df869 187 SelectLinesC[SelectLineIterator]=1;
sakthipriya 0:e9c32e1df869 188 break;
sakthipriya 0:e9c32e1df869 189 }
sakthipriya 0:e9c32e1df869 190 else SelectLinesC[SelectLineIterator]=0;
sakthipriya 0:e9c32e1df869 191
sakthipriya 0:e9c32e1df869 192 }
sakthipriya 0:e9c32e1df869 193
sakthipriya 0:e9c32e1df869 194
sakthipriya 0:e9c32e1df869 195 wait_us(10.0); // A delay of 10 microseconds between each sensor output. Can be changed.
sakthipriya 0:e9c32e1df869 196
sakthipriya 0:e9c32e1df869 197 }
sakthipriya 0:e9c32e1df869 198
sakthipriya 0:e9c32e1df869 199
sakthipriya 0:e9c32e1df869 200
sakthipriya 0:e9c32e1df869 201
sakthipriya 0:e9c32e1df869 202
sakthipriya 0:e9c32e1df869 203 //update magnetometer data->
sakthipriya 0:e9c32e1df869 204 //populate values in structure variable 'Sensor' from data to be given by Green
sakthipriya 0:e9c32e1df869 205 SensorQuantised.AngularSpeed[0]=quantiz(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[1]);
sakthipriya 0:e9c32e1df869 206 SensorQuantised.AngularSpeed[0]=SensorQuantised.AngularSpeed[0]<<4+quantiz(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[0]);
sakthipriya 0:e9c32e1df869 207 SensorQuantised.AngularSpeed[1]=quantiz(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[2]);
sakthipriya 0:e9c32e1df869 208
sakthipriya 0:e9c32e1df869 209 //update gyro data->
sakthipriya 0:e9c32e1df869 210 //populate values in structure variable 'Sensor' from data to be given by Green
sakthipriya 0:e9c32e1df869 211 SensorQuantised.Bnewvalue[0]=quantiz(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[1]);
sakthipriya 0:e9c32e1df869 212 SensorQuantised.Bnewvalue[0]=SensorQuantised.Bnewvalue[0]<<4+quantiz(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[0]);
sakthipriya 0:e9c32e1df869 213 SensorQuantised.Bnewvalue[1]=quantiz(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[2]);
sakthipriya 0:e9c32e1df869 214
sakthipriya 0:e9c32e1df869 215
sakthipriya 0:e9c32e1df869 216
sakthipriya 0:e9c32e1df869 217
sakthipriya 0:e9c32e1df869 218 //update beacon structure
sakthipriya 0:e9c32e1df869 219 init_beacon(&Shortbeacon,SensorQuantised);//Shortbeacon is passed
sakthipriya 0:e9c32e1df869 220
sakthipriya 0:e9c32e1df869 221
sakthipriya 0:e9c32e1df869 222 }
sakthipriya 0:e9c32e1df869 223
sakthipriya 0:e9c32e1df869 224
sakthipriya 0:e9c32e1df869 225