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