[187] | 1 | <?xml version='1.0' encoding='UTF-8' standalone='no'?> |
---|
| 2 | <doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.5.6"> |
---|
| 3 | <compounddef id="ekf_8cpp" kind="file"> |
---|
| 4 | <compoundname>ekf.cpp</compoundname> |
---|
| 5 | <includes local="no">math.h</includes> |
---|
| 6 | <includes refid="fixed_8h" local="yes">fixed.h</includes> |
---|
| 7 | <includes refid="reference_8h" local="yes">reference.h</includes> |
---|
| 8 | <includes refid="matrix_8h" local="yes">matrix.h</includes> |
---|
| 9 | <includes refid="ekf_8h" local="yes">ekf.h</includes> |
---|
| 10 | <incdepgraph> |
---|
| 11 | <node id="717"> |
---|
| 12 | <label>ekf.h</label> |
---|
| 13 | <link refid="ekf_8h-source"/> |
---|
| 14 | </node> |
---|
| 15 | <node id="712"> |
---|
| 16 | <label>work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp</label> |
---|
| 17 | <link refid="ekf.cpp"/> |
---|
| 18 | <childnode refid="713" relation="include"> |
---|
| 19 | </childnode> |
---|
| 20 | <childnode refid="714" relation="include"> |
---|
| 21 | </childnode> |
---|
| 22 | <childnode refid="715" relation="include"> |
---|
| 23 | </childnode> |
---|
| 24 | <childnode refid="716" relation="include"> |
---|
| 25 | </childnode> |
---|
| 26 | <childnode refid="717" relation="include"> |
---|
| 27 | </childnode> |
---|
| 28 | </node> |
---|
| 29 | <node id="713"> |
---|
| 30 | <label>math.h</label> |
---|
| 31 | </node> |
---|
| 32 | <node id="715"> |
---|
| 33 | <label>reference.h</label> |
---|
| 34 | <link refid="reference_8h-source"/> |
---|
| 35 | </node> |
---|
| 36 | <node id="714"> |
---|
| 37 | <label>fixed.h</label> |
---|
| 38 | <link refid="fixed_8h-source"/> |
---|
| 39 | </node> |
---|
| 40 | <node id="716"> |
---|
| 41 | <label>matrix.h</label> |
---|
| 42 | <link refid="matrix_8h-source"/> |
---|
| 43 | </node> |
---|
| 44 | </incdepgraph> |
---|
| 45 | <sectiondef kind="define"> |
---|
| 46 | <memberdef kind="define" id="ekf_8cpp_105679eb18ff6f0bd4ba53411a0fe0afb" prot="public" static="no"> |
---|
| 47 | <name>K_PREVOD_FORM</name> |
---|
| 48 | <initializer>4</initializer> |
---|
| 49 | <briefdescription> |
---|
| 50 | </briefdescription> |
---|
| 51 | <detaileddescription> |
---|
| 52 | </detaileddescription> |
---|
| 53 | <inbodydescription> |
---|
| 54 | </inbodydescription> |
---|
| 55 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="24" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="24" bodyend="-1"/> |
---|
| 56 | </memberdef> |
---|
| 57 | </sectiondef> |
---|
| 58 | <sectiondef kind="var"> |
---|
| 59 | <memberdef kind="variable" id="ekf_8cpp_1415bdef5e664918e4e4b773c36f4e103" prot="public" static="no" mutable="no"> |
---|
| 60 | <type>double</type> |
---|
| 61 | <definition>double ladeni_EKF[10]</definition> |
---|
| 62 | <argsstring>[10]</argsstring> |
---|
| 63 | <name>ladeni_EKF</name> |
---|
| 64 | <briefdescription> |
---|
| 65 | </briefdescription> |
---|
| 66 | <detaileddescription> |
---|
| 67 | </detaileddescription> |
---|
| 68 | <inbodydescription> |
---|
| 69 | </inbodydescription> |
---|
| 70 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="36" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="36" bodyend="-1"/> |
---|
| 71 | </memberdef> |
---|
| 72 | <memberdef kind="variable" id="ekf_8cpp_1f66eddc642fe2a42f062f5cd834102a5" prot="public" static="yes" mutable="no"> |
---|
| 73 | <type>int</type> |
---|
| 74 | <definition>int H[8]</definition> |
---|
| 75 | <argsstring>[8]</argsstring> |
---|
| 76 | <name>H</name> |
---|
| 77 | <initializer>{0x7FFF,0,0,0,0,0x7FFF,0,0}</initializer> |
---|
| 78 | <briefdescription> |
---|
| 79 | </briefdescription> |
---|
| 80 | <detaileddescription> |
---|
| 81 | </detaileddescription> |
---|
| 82 | <inbodydescription> |
---|
| 83 | </inbodydescription> |
---|
| 84 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="39" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="39" bodyend="-1"/> |
---|
| 85 | </memberdef> |
---|
| 86 | <memberdef kind="variable" id="ekf_8cpp_1f5a21f73c9dd58ca98b25b4525fedb11" prot="public" static="yes" mutable="no"> |
---|
| 87 | <type>int</type> |
---|
| 88 | <definition>int Ht[8]</definition> |
---|
| 89 | <argsstring>[8]</argsstring> |
---|
| 90 | <name>Ht</name> |
---|
| 91 | <initializer>{0x7FFF,0,0,0x7FFF,0,0,0,0}</initializer> |
---|
| 92 | <briefdescription> |
---|
| 93 | </briefdescription> |
---|
| 94 | <detaileddescription> |
---|
| 95 | </detaileddescription> |
---|
| 96 | <inbodydescription> |
---|
| 97 | </inbodydescription> |
---|
| 98 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="40" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="40" bodyend="-1"/> |
---|
| 99 | </memberdef> |
---|
| 100 | <memberdef kind="variable" id="ekf_8cpp_1893628019411e1d74b2ca67f76d647d9" prot="public" static="yes" mutable="no"> |
---|
| 101 | <type>int</type> |
---|
| 102 | <definition>int Q[16]</definition> |
---|
| 103 | <argsstring>[16]</argsstring> |
---|
| 104 | <name>Q</name> |
---|
| 105 | <initializer>{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}</initializer> |
---|
| 106 | <briefdescription> |
---|
| 107 | </briefdescription> |
---|
| 108 | <detaileddescription> |
---|
| 109 | </detaileddescription> |
---|
| 110 | <inbodydescription> |
---|
| 111 | </inbodydescription> |
---|
| 112 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="43" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="43" bodyend="-1"/> |
---|
| 113 | </memberdef> |
---|
| 114 | <memberdef kind="variable" id="ekf_8cpp_167b63ae29fb23d1a034f9ba7e0787745" prot="public" static="yes" mutable="no"> |
---|
| 115 | <type>int</type> |
---|
| 116 | <definition>int R[4]</definition> |
---|
| 117 | <argsstring>[4]</argsstring> |
---|
| 118 | <name>R</name> |
---|
| 119 | <initializer>{0,0,0,0}</initializer> |
---|
| 120 | <briefdescription> |
---|
| 121 | </briefdescription> |
---|
| 122 | <detaileddescription> |
---|
| 123 | </detaileddescription> |
---|
| 124 | <inbodydescription> |
---|
| 125 | </inbodydescription> |
---|
| 126 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="44" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="44" bodyend="-1"/> |
---|
| 127 | </memberdef> |
---|
| 128 | <memberdef kind="variable" id="ekf_8cpp_1db2f2b0f082a8ce53b3e61a9ce7e6c94" prot="public" static="yes" mutable="no"> |
---|
| 129 | <type>int</type> |
---|
| 130 | <definition>int x_est[4]</definition> |
---|
| 131 | <argsstring>[4]</argsstring> |
---|
| 132 | <name>x_est</name> |
---|
| 133 | <initializer>{0,0,0,0}</initializer> |
---|
| 134 | <briefdescription> |
---|
| 135 | </briefdescription> |
---|
| 136 | <detaileddescription> |
---|
| 137 | </detaileddescription> |
---|
| 138 | <inbodydescription> |
---|
| 139 | </inbodydescription> |
---|
| 140 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="47" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="47" bodyend="-1"/> |
---|
| 141 | </memberdef> |
---|
| 142 | <memberdef kind="variable" id="ekf_8cpp_1b5b26d093bdcad628aef9156a376174f" prot="public" static="yes" mutable="no"> |
---|
| 143 | <type>int</type> |
---|
| 144 | <definition>int x_pred[4]</definition> |
---|
| 145 | <argsstring>[4]</argsstring> |
---|
| 146 | <name>x_pred</name> |
---|
| 147 | <initializer>{0,0,0,0}</initializer> |
---|
| 148 | <briefdescription> |
---|
| 149 | </briefdescription> |
---|
| 150 | <detaileddescription> |
---|
| 151 | </detaileddescription> |
---|
| 152 | <inbodydescription> |
---|
| 153 | </inbodydescription> |
---|
| 154 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="48" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="48" bodyend="-1"/> |
---|
| 155 | </memberdef> |
---|
| 156 | <memberdef kind="variable" id="ekf_8cpp_1683087dfe47600d1a450302a2ccbb836" prot="public" static="yes" mutable="no"> |
---|
| 157 | <type>int</type> |
---|
| 158 | <definition>int P_pred[16]</definition> |
---|
| 159 | <argsstring>[16]</argsstring> |
---|
| 160 | <name>P_pred</name> |
---|
| 161 | <initializer>{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}</initializer> |
---|
| 162 | <briefdescription> |
---|
| 163 | </briefdescription> |
---|
| 164 | <detaileddescription> |
---|
| 165 | </detaileddescription> |
---|
| 166 | <inbodydescription> |
---|
| 167 | </inbodydescription> |
---|
| 168 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="49" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="49" bodyend="-1"/> |
---|
| 169 | </memberdef> |
---|
| 170 | <memberdef kind="variable" id="ekf_8cpp_198aa9c8e8546c3990a668fa2376bd02f" prot="public" static="yes" mutable="no"> |
---|
| 171 | <type>int</type> |
---|
| 172 | <definition>int P_est[16]</definition> |
---|
| 173 | <argsstring>[16]</argsstring> |
---|
| 174 | <name>P_est</name> |
---|
| 175 | <initializer>{0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF}</initializer> |
---|
| 176 | <briefdescription> |
---|
| 177 | </briefdescription> |
---|
| 178 | <detaileddescription> |
---|
| 179 | </detaileddescription> |
---|
| 180 | <inbodydescription> |
---|
| 181 | </inbodydescription> |
---|
| 182 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="51" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="51" bodyend="-1"/> |
---|
| 183 | </memberdef> |
---|
| 184 | <memberdef kind="variable" id="ekf_8cpp_1af019cae3f3f03d594a8f549353c99e9" prot="public" static="yes" mutable="no"> |
---|
| 185 | <type>int</type> |
---|
| 186 | <definition>int Y_mes[2]</definition> |
---|
| 187 | <argsstring>[2]</argsstring> |
---|
| 188 | <name>Y_mes</name> |
---|
| 189 | <initializer>{0,0}</initializer> |
---|
| 190 | <briefdescription> |
---|
| 191 | </briefdescription> |
---|
| 192 | <detaileddescription> |
---|
| 193 | </detaileddescription> |
---|
| 194 | <inbodydescription> |
---|
| 195 | </inbodydescription> |
---|
| 196 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="52" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="52" bodyend="-1"/> |
---|
| 197 | </memberdef> |
---|
| 198 | <memberdef kind="variable" id="ekf_8cpp_16cccd275c0b11a7fac94937f2021045b" prot="public" static="yes" mutable="no"> |
---|
| 199 | <type>int</type> |
---|
| 200 | <definition>int ukalm[2]</definition> |
---|
| 201 | <argsstring>[2]</argsstring> |
---|
| 202 | <name>ukalm</name> |
---|
| 203 | <initializer>{0,0}</initializer> |
---|
| 204 | <briefdescription> |
---|
| 205 | </briefdescription> |
---|
| 206 | <detaileddescription> |
---|
| 207 | </detaileddescription> |
---|
| 208 | <inbodydescription> |
---|
| 209 | </inbodydescription> |
---|
| 210 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="53" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="53" bodyend="-1"/> |
---|
| 211 | </memberdef> |
---|
| 212 | <memberdef kind="variable" id="ekf_8cpp_1f79d2220030336cad022d159b3f8c8bb" prot="public" static="yes" mutable="no"> |
---|
| 213 | <type>int</type> |
---|
| 214 | <definition>int Kalm[8]</definition> |
---|
| 215 | <argsstring>[8]</argsstring> |
---|
| 216 | <name>Kalm</name> |
---|
| 217 | <briefdescription> |
---|
| 218 | </briefdescription> |
---|
| 219 | <detaileddescription> |
---|
| 220 | </detaileddescription> |
---|
| 221 | <inbodydescription> |
---|
| 222 | </inbodydescription> |
---|
| 223 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="54" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="54" bodyend="-1"/> |
---|
| 224 | </memberdef> |
---|
| 225 | <memberdef kind="variable" id="ekf_8cpp_10db030b4590561a5e0a5d70223a860dd" prot="public" static="yes" mutable="no"> |
---|
| 226 | <type>int</type> |
---|
| 227 | <definition>int PSI[16]</definition> |
---|
| 228 | <argsstring>[16]</argsstring> |
---|
| 229 | <name>PSI</name> |
---|
| 230 | <initializer>{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}</initializer> |
---|
| 231 | <briefdescription> |
---|
| 232 | </briefdescription> |
---|
| 233 | <detaileddescription> |
---|
| 234 | </detaileddescription> |
---|
| 235 | <inbodydescription> |
---|
| 236 | </inbodydescription> |
---|
| 237 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="56" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="56" bodyend="-1"/> |
---|
| 238 | </memberdef> |
---|
| 239 | <memberdef kind="variable" id="ekf_8cpp_17a8fbda3ee1da60e70499a79b4d3e3df" prot="public" static="yes" mutable="no"> |
---|
| 240 | <type>int</type> |
---|
| 241 | <definition>int temp15a[16]</definition> |
---|
| 242 | <argsstring>[16]</argsstring> |
---|
| 243 | <name>temp15a</name> |
---|
| 244 | <briefdescription> |
---|
| 245 | </briefdescription> |
---|
| 246 | <detaileddescription> |
---|
| 247 | </detaileddescription> |
---|
| 248 | <inbodydescription> |
---|
| 249 | </inbodydescription> |
---|
| 250 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="59" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="59" bodyend="-1"/> |
---|
| 251 | </memberdef> |
---|
| 252 | <memberdef kind="variable" id="ekf_8cpp_1f82c96cc9810ce86d1edec2fbf1deb6d" prot="public" static="yes" mutable="no"> |
---|
| 253 | <type>int</type> |
---|
| 254 | <definition>int cA</definition> |
---|
| 255 | <argsstring></argsstring> |
---|
| 256 | <name>cA</name> |
---|
| 257 | <briefdescription> |
---|
| 258 | </briefdescription> |
---|
| 259 | <detaileddescription> |
---|
| 260 | </detaileddescription> |
---|
| 261 | <inbodydescription> |
---|
| 262 | </inbodydescription> |
---|
| 263 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="62" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="62" bodyend="-1"/> |
---|
| 264 | </memberdef> |
---|
| 265 | <memberdef kind="variable" id="ekf_8cpp_1026fd8d02ce266d7a9f9bb1e233f6242" prot="public" static="yes" mutable="no"> |
---|
| 266 | <type>int</type> |
---|
| 267 | <definition>int cB</definition> |
---|
| 268 | <argsstring></argsstring> |
---|
| 269 | <name>cB</name> |
---|
| 270 | <briefdescription> |
---|
| 271 | </briefdescription> |
---|
| 272 | <detaileddescription> |
---|
| 273 | </detaileddescription> |
---|
| 274 | <inbodydescription> |
---|
| 275 | </inbodydescription> |
---|
| 276 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="62" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="62" bodyend="-1"/> |
---|
| 277 | </memberdef> |
---|
| 278 | <memberdef kind="variable" id="ekf_8cpp_1da52f4f6674e0f993ee24e61553e2eae" prot="public" static="yes" mutable="no"> |
---|
| 279 | <type>int</type> |
---|
| 280 | <definition>int cC</definition> |
---|
| 281 | <argsstring></argsstring> |
---|
| 282 | <name>cC</name> |
---|
| 283 | <briefdescription> |
---|
| 284 | </briefdescription> |
---|
| 285 | <detaileddescription> |
---|
| 286 | </detaileddescription> |
---|
| 287 | <inbodydescription> |
---|
| 288 | </inbodydescription> |
---|
| 289 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="62" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="62" bodyend="-1"/> |
---|
| 290 | </memberdef> |
---|
| 291 | <memberdef kind="variable" id="ekf_8cpp_13a4546b83055800c4658329cb2ca0b4e" prot="public" static="yes" mutable="no"> |
---|
| 292 | <type>int</type> |
---|
| 293 | <definition>int cG</definition> |
---|
| 294 | <argsstring></argsstring> |
---|
| 295 | <name>cG</name> |
---|
| 296 | <briefdescription> |
---|
| 297 | </briefdescription> |
---|
| 298 | <detaileddescription> |
---|
| 299 | </detaileddescription> |
---|
| 300 | <inbodydescription> |
---|
| 301 | </inbodydescription> |
---|
| 302 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="62" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="62" bodyend="-1"/> |
---|
| 303 | </memberdef> |
---|
| 304 | <memberdef kind="variable" id="ekf_8cpp_119c038d1a31ea4257f137481c835ea96" prot="public" static="yes" mutable="no"> |
---|
| 305 | <type>int</type> |
---|
| 306 | <definition>int cH</definition> |
---|
| 307 | <argsstring></argsstring> |
---|
| 308 | <name>cH</name> |
---|
| 309 | <briefdescription> |
---|
| 310 | </briefdescription> |
---|
| 311 | <detaileddescription> |
---|
| 312 | </detaileddescription> |
---|
| 313 | <inbodydescription> |
---|
| 314 | </inbodydescription> |
---|
| 315 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="62" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="62" bodyend="-1"/> |
---|
| 316 | </memberdef> |
---|
| 317 | </sectiondef> |
---|
| 318 | <sectiondef kind="func"> |
---|
| 319 | <memberdef kind="function" id="ekf_8cpp_1956dd7a9713816704f8ff5760e26e903" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual"> |
---|
| 320 | <type>void</type> |
---|
| 321 | <definition>void init_ekf</definition> |
---|
| 322 | <argsstring>(double Tv, double *param)</argsstring> |
---|
| 323 | <name>init_ekf</name> |
---|
| 324 | <param> |
---|
| 325 | <type>double</type> |
---|
| 326 | <declname>Tv</declname> |
---|
| 327 | </param> |
---|
| 328 | <param> |
---|
| 329 | <type>double *</type> |
---|
| 330 | <declname>param</declname> |
---|
| 331 | </param> |
---|
| 332 | <briefdescription> |
---|
| 333 | </briefdescription> |
---|
| 334 | <detaileddescription> |
---|
| 335 | </detaileddescription> |
---|
| 336 | <inbodydescription> |
---|
| 337 | <para>KONEC inicializace pro PC - az sem vyhodit v DSP /// </para> </inbodydescription> |
---|
| 338 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="27" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="65" bodyend="125"/> |
---|
| 339 | </memberdef> |
---|
| 340 | <memberdef kind="function" id="ekf_8cpp_1267ca49dd712a2b06af569b1e108976a" prot="public" static="no" const="no" explicit="no" inline="no" virt="non-virtual"> |
---|
| 341 | <type>void</type> |
---|
| 342 | <definition>void ekf</definition> |
---|
| 343 | <argsstring>(double *x_estimation, double Umd, double beta, double Ucnd, double Ucd, double isxd, double isyd)</argsstring> |
---|
| 344 | <name>ekf</name> |
---|
| 345 | <param> |
---|
| 346 | <type>double *</type> |
---|
| 347 | <declname>x_estimation</declname> |
---|
| 348 | </param> |
---|
| 349 | <param> |
---|
| 350 | <type>double</type> |
---|
| 351 | <declname>Umd</declname> |
---|
| 352 | </param> |
---|
| 353 | <param> |
---|
| 354 | <type>double</type> |
---|
| 355 | <declname>beta</declname> |
---|
| 356 | </param> |
---|
| 357 | <param> |
---|
| 358 | <type>double</type> |
---|
| 359 | <declname>Ucnd</declname> |
---|
| 360 | </param> |
---|
| 361 | <param> |
---|
| 362 | <type>double</type> |
---|
| 363 | <declname>Ucd</declname> |
---|
| 364 | </param> |
---|
| 365 | <param> |
---|
| 366 | <type>double</type> |
---|
| 367 | <declname>isxd</declname> |
---|
| 368 | </param> |
---|
| 369 | <param> |
---|
| 370 | <type>double</type> |
---|
| 371 | <declname>isyd</declname> |
---|
| 372 | </param> |
---|
| 373 | <briefdescription> |
---|
| 374 | </briefdescription> |
---|
| 375 | <detaileddescription> |
---|
| 376 | </detaileddescription> |
---|
| 377 | <inbodydescription> |
---|
| 378 | <para>prechod ze simulace do fixed-pointu - vyradit v DSP ///////////</para><para>KONEC emulace FIXED-POINTu - az sem vyradit v DSP //////////// </para> </inbodydescription> |
---|
| 379 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="28" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="216" bodyend="254"/> |
---|
| 380 | </memberdef> |
---|
| 381 | <memberdef kind="function" id="ekf_8cpp_1bdd51eee3a5eb0a70f3d1e13b5cf2697" prot="public" static="yes" const="no" explicit="no" inline="no" virt="non-virtual"> |
---|
| 382 | <type>void</type> |
---|
| 383 | <definition>void prediction</definition> |
---|
| 384 | <argsstring>(int *ux)</argsstring> |
---|
| 385 | <name>prediction</name> |
---|
| 386 | <param> |
---|
| 387 | <type>int *</type> |
---|
| 388 | <declname>ux</declname> |
---|
| 389 | </param> |
---|
| 390 | <briefdescription> |
---|
| 391 | </briefdescription> |
---|
| 392 | <detaileddescription> |
---|
| 393 | </detaileddescription> |
---|
| 394 | <inbodydescription> |
---|
| 395 | </inbodydescription> |
---|
| 396 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="31" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="148" bodyend="172"/> |
---|
| 397 | </memberdef> |
---|
| 398 | <memberdef kind="function" id="ekf_8cpp_188642818810b8018ecbb525cad1d5c6f" prot="public" static="yes" const="no" explicit="no" inline="no" virt="non-virtual"> |
---|
| 399 | <type>void</type> |
---|
| 400 | <definition>void correction</definition> |
---|
| 401 | <argsstring>(void)</argsstring> |
---|
| 402 | <name>correction</name> |
---|
| 403 | <param> |
---|
| 404 | <type>void</type> |
---|
| 405 | </param> |
---|
| 406 | <briefdescription> |
---|
| 407 | </briefdescription> |
---|
| 408 | <detaileddescription> |
---|
| 409 | </detaileddescription> |
---|
| 410 | <inbodydescription> |
---|
| 411 | </inbodydescription> |
---|
| 412 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="32" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="174" bodyend="213"/> |
---|
| 413 | </memberdef> |
---|
| 414 | <memberdef kind="function" id="ekf_8cpp_195d9e77bdeaf5408a69d540731fd629f" prot="public" static="yes" const="no" explicit="no" inline="no" virt="non-virtual"> |
---|
| 415 | <type>void</type> |
---|
| 416 | <definition>static void update_psi</definition> |
---|
| 417 | <argsstring>(void)</argsstring> |
---|
| 418 | <name>update_psi</name> |
---|
| 419 | <param> |
---|
| 420 | <type>void</type> |
---|
| 421 | </param> |
---|
| 422 | <briefdescription> |
---|
| 423 | </briefdescription> |
---|
| 424 | <detaileddescription> |
---|
| 425 | </detaileddescription> |
---|
| 426 | <inbodydescription> |
---|
| 427 | </inbodydescription> |
---|
| 428 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" line="33" bodyfile="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp" bodystart="128" bodyend="145"/> |
---|
| 429 | </memberdef> |
---|
| 430 | </sectiondef> |
---|
| 431 | <briefdescription> |
---|
| 432 | </briefdescription> |
---|
| 433 | <detaileddescription> |
---|
| 434 | </detaileddescription> |
---|
| 435 | <programlisting> |
---|
| 436 | <codeline lineno="1"><highlight class="comment">/************************************</highlight></codeline> |
---|
| 437 | <codeline lineno="2"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Extended<sp/>Kalman<sp/>Filter</highlight></codeline> |
---|
| 438 | <codeline lineno="3"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Kalman<sp/>Observer</highlight></codeline> |
---|
| 439 | <codeline lineno="4"><highlight class="comment"></highlight></codeline> |
---|
| 440 | <codeline lineno="5"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>Z.<sp/>Peroutka</highlight></codeline> |
---|
| 441 | <codeline lineno="6"><highlight class="comment"></highlight></codeline> |
---|
| 442 | <codeline lineno="7"><highlight class="comment">Rev.<sp/>15.3.2008</highlight></codeline> |
---|
| 443 | <codeline lineno="8"><highlight class="comment"></highlight></codeline> |
---|
| 444 | <codeline lineno="9"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>EKF<sp/>pocitan<sp/>s<sp/>daty<sp/>ve<sp/>formatu<sp/>Q15,<sp/>zatimco<sp/>regulace<sp/>pracuje<sp/>v<sp/>Q13<sp/>-></highlight></codeline> |
---|
| 445 | <codeline lineno="10"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>resenim<sp/>je<sp/>vynasobit<sp/>referencni<sp/>hodnoty<sp/>v<sp/>EKF<sp/>4-mi<sp/>(tim<sp/>je<sp/>automaticky</highlight></codeline> |
---|
| 446 | <codeline lineno="11"><highlight class="comment"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>zajisten<sp/>prechod<sp/>mezi<sp/>formaty<sp/>Q13<sp/>a<sp/>Q15).<sp/>Realizovano<sp/>konstantou<sp/>K_PREVOD_FORM.</highlight></codeline> |
---|
| 447 | <codeline lineno="12"><highlight class="comment"></highlight></codeline> |
---|
| 448 | <codeline lineno="13"><highlight class="comment">15.3.2008<sp/><sp/><sp/><sp/><sp/><sp/><sp/>Kontrola<sp/>kodu<sp/>+<sp/>zamena<sp/>datovych<sp/>typu<sp/>q15->int<sp/>a<sp/>q30->long.</highlight></codeline> |
---|
| 449 | <codeline lineno="14"><highlight class="comment"></highlight></codeline> |
---|
| 450 | <codeline lineno="15"><highlight class="comment">*************************************/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 451 | <codeline lineno="16"><highlight class="normal"></highlight></codeline> |
---|
| 452 | <codeline lineno="17"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/><math.h></highlight><highlight class="normal"></highlight></codeline> |
---|
| 453 | <codeline lineno="18"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>"fixed.h"</highlight><highlight class="normal"></highlight></codeline> |
---|
| 454 | <codeline lineno="19"><highlight class="normal"></highlight><highlight class="comment">//#include<sp/>"parametry_motoru.h"<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>//<sp/>aktivovat<sp/>v<sp/>DSP</highlight><highlight class="normal"></highlight></codeline> |
---|
| 455 | <codeline lineno="20"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>"reference.h"</highlight><highlight class="normal"></highlight></codeline> |
---|
| 456 | <codeline lineno="21"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>"matrix.h"</highlight><highlight class="normal"></highlight></codeline> |
---|
| 457 | <codeline lineno="22"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/>"ekf.h"</highlight><highlight class="normal"></highlight></codeline> |
---|
| 458 | <codeline lineno="23"><highlight class="normal"></highlight></codeline> |
---|
| 459 | <codeline lineno="24"><highlight class="normal"></highlight><highlight class="preprocessor">#define<sp/>K_PREVOD_FORM<sp/><sp/><sp/>4<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>//<sp/>2^(15-Qm)</highlight></codeline> |
---|
| 460 | <codeline lineno="25"><highlight class="preprocessor"></highlight><highlight class="normal"></highlight></codeline> |
---|
| 461 | <codeline lineno="26"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Declaration<sp/>of<sp/>global<sp/>functions<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 462 | <codeline lineno="27"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>init_ekf(</highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Tv,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>*param);</highlight></codeline> |
---|
| 463 | <codeline lineno="28"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>ekf(</highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>*x_estimation,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Umd,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>beta,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Ucnd,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Ucd,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>isxd,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>isyd);</highlight></codeline> |
---|
| 464 | <codeline lineno="29"><highlight class="normal"></highlight></codeline> |
---|
| 465 | <codeline lineno="30"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Declaration<sp/>of<sp/>local<sp/>functions<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 466 | <codeline lineno="31"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>prediction(</highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>*ux);</highlight></codeline> |
---|
| 467 | <codeline lineno="32"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>correction(</highlight><highlight class="keywordtype">void</highlight><highlight class="normal">);</highlight></codeline> |
---|
| 468 | <codeline lineno="33"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>update_psi(</highlight><highlight class="keywordtype">void</highlight><highlight class="normal">);</highlight></codeline> |
---|
| 469 | <codeline lineno="34"><highlight class="normal"></highlight></codeline> |
---|
| 470 | <codeline lineno="35"><highlight class="normal"></highlight><highlight class="comment">//<sp/>global<sp/>variables</highlight><highlight class="normal"></highlight></codeline> |
---|
| 471 | <codeline lineno="36"><highlight class="normal"></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>ladeni_EKF[10];</highlight></codeline> |
---|
| 472 | <codeline lineno="37"><highlight class="normal"></highlight></codeline> |
---|
| 473 | <codeline lineno="38"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Constants<sp/>-<sp/>definovat<sp/>jako<sp/>konstanty<sp/>??<sp/>?kde<sp/>je<sp/>vyhodnejsi<sp/>aby<sp/>v<sp/>pameti<sp/>byli?*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 474 | <codeline lineno="39"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>H[8]={0x7FFF,0,0,0,0,0x7FFF,0,0};<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[2,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 475 | <codeline lineno="40"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Ht[8]={0x7FFF,0,0,0x7FFF,0,0,0,0};</highlight><highlight class="comment">/*<sp/>matrix<sp/>[4,2]<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 476 | <codeline lineno="41"><highlight class="normal"></highlight><highlight class="comment">//static<sp/>int<sp/>Q[4][4]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};<sp/>/*<sp/>matrix<sp/>[4,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 477 | <codeline lineno="42"><highlight class="normal"></highlight><highlight class="comment">//static<sp/>int<sp/>R[2][2]={0,0,0,0};<sp/>/*<sp/>matrix<sp/>[2,2]<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 478 | <codeline lineno="43"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Q[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[4,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 479 | <codeline lineno="44"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>R[4]={0,0,0,0};<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[2,2]<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 480 | <codeline lineno="45"><highlight class="normal"></highlight></codeline> |
---|
| 481 | <codeline lineno="46"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Initial<sp/>conditions<sp/>and<sp/>variables<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 482 | <codeline lineno="47"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>x_est[4]={0,0,0,0};</highlight></codeline> |
---|
| 483 | <codeline lineno="48"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>x_pred[4]={0,0,0,0};</highlight></codeline> |
---|
| 484 | <codeline lineno="49"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>P_pred[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[4,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 485 | <codeline lineno="50"><highlight class="normal"></highlight><highlight class="comment">//static<sp/>int<sp/>P_est[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};<sp/>/*<sp/>matrix<sp/>[4,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 486 | <codeline lineno="51"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>P_est[16]={0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF};<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[4,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 487 | <codeline lineno="52"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Y_mes[2]={0,0};</highlight></codeline> |
---|
| 488 | <codeline lineno="53"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>ukalm[2]={0,0};</highlight></codeline> |
---|
| 489 | <codeline lineno="54"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Kalm[8];<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[5,2]<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 490 | <codeline lineno="55"><highlight class="normal"></highlight></codeline> |
---|
| 491 | <codeline lineno="56"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>PSI[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[4,4]<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 492 | <codeline lineno="57"><highlight class="normal"></highlight></codeline> |
---|
| 493 | <codeline lineno="58"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Temporary<sp/>variables<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 494 | <codeline lineno="59"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>temp15a[16];</highlight></codeline> |
---|
| 495 | <codeline lineno="60"><highlight class="normal"></highlight></codeline> |
---|
| 496 | <codeline lineno="61"><highlight class="normal"></highlight><highlight class="comment">//<sp/>constants<sp/>of<sp/>PMSM<sp/>mathematical<sp/>model</highlight><highlight class="normal"></highlight></codeline> |
---|
| 497 | <codeline lineno="62"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>cA,<sp/>cB,<sp/>cC,<sp/>cG,<sp/>cH;<sp/><sp/></highlight><highlight class="comment">//<sp/>cD,<sp/>cE,<sp/>cF,<sp/>cI<sp/>...<sp/>nepouzivane</highlight><highlight class="normal"></highlight></codeline> |
---|
| 498 | <codeline lineno="63"><highlight class="normal"></highlight></codeline> |
---|
| 499 | <codeline lineno="65"><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>init_ekf(</highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Tv,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>*param)</highlight></codeline> |
---|
| 500 | <codeline lineno="66"><highlight class="normal">{</highlight></codeline> |
---|
| 501 | <codeline lineno="67"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>parametry<sp/>simulace<sp/>-<sp/>v<sp/>DSP<sp/>vyhodit<sp/>(nahrazeno<sp/>"parametry_motoru.h")</highlight><highlight class="normal"></highlight></codeline> |
---|
| 502 | <codeline lineno="68"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Rs,<sp/>Ls,<sp/>Fmag,<sp/>kp,<sp/>p;</highlight></codeline> |
---|
| 503 | <codeline lineno="69"><highlight class="normal"></highlight></codeline> |
---|
| 504 | <codeline lineno="70"><highlight class="normal"><sp/></highlight><highlight class="comment">//<sp/>param=[Rs,<sp/>Ls,<sp/>Fmag,<sp/>Bf,<sp/>p,<sp/>kp,<sp/>J<sp/>=<sp/>0.04?];</highlight><highlight class="normal"></highlight></codeline> |
---|
| 505 | <codeline lineno="71"><highlight class="normal"><sp/><sp/>Rs=*param;</highlight></codeline> |
---|
| 506 | <codeline lineno="72"><highlight class="normal"><sp/><sp/>Ls=*(param+1);</highlight></codeline> |
---|
| 507 | <codeline lineno="73"><highlight class="normal"><sp/><sp/>Fmag=*(param+2);</highlight></codeline> |
---|
| 508 | <codeline lineno="74"><highlight class="normal"><sp/><sp/>p=*(param+4);</highlight></codeline> |
---|
| 509 | <codeline lineno="75"><highlight class="normal"><sp/><sp/>kp=*(param+5);</highlight></codeline> |
---|
| 510 | <codeline lineno="77"><highlight class="normal"></highlight></codeline> |
---|
| 511 | <codeline lineno="78"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>Tuning<sp/>of<sp/>matrix<sp/>Q</highlight><highlight class="normal"></highlight></codeline> |
---|
| 512 | <codeline lineno="79"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/>Q[0][0]=prevod(.05,15);<sp/><sp/><sp/><sp/><sp/><sp/><sp/>//<sp/>1e-2</highlight></codeline> |
---|
| 513 | <codeline lineno="80"><highlight class="comment"><sp/><sp/>Q[1][1]=Q[0][0];</highlight></codeline> |
---|
| 514 | <codeline lineno="81"><highlight class="comment"><sp/><sp/>Q[2][2]=prevod(1e-3,15);<sp/><sp/><sp/><sp/><sp/><sp/>//<sp/>10e-5</highlight></codeline> |
---|
| 515 | <codeline lineno="82"><highlight class="comment"><sp/><sp/>Q[3][3]=prevod(1e-3,15);<sp/><sp/><sp/><sp/><sp/><sp/>//<sp/>model(ureg)<sp/>...<sp/>4e-3</highlight></codeline> |
---|
| 516 | <codeline lineno="83"><highlight class="comment"></highlight></codeline> |
---|
| 517 | <codeline lineno="84"><highlight class="comment"><sp/><sp/>//<sp/>Tuning<sp/>of<sp/>matrix<sp/>R</highlight></codeline> |
---|
| 518 | <codeline lineno="85"><highlight class="comment"><sp/><sp/>R[0][0]=prevod(0.05,15);<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>//<sp/>model(ureg)<sp/>...<sp/>0.1</highlight></codeline> |
---|
| 519 | <codeline lineno="86"><highlight class="comment"><sp/><sp/>R[1][1]=R[0][0];</highlight></codeline> |
---|
| 520 | <codeline lineno="87"><highlight class="comment">/**/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 521 | <codeline lineno="88"><highlight class="normal"></highlight></codeline> |
---|
| 522 | <codeline lineno="89"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>Tuning<sp/>of<sp/>matrix<sp/>Q</highlight><highlight class="normal"></highlight></codeline> |
---|
| 523 | <codeline lineno="90"><highlight class="normal"><sp/><sp/>Q[0]=prevod(.01,15);<sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>0.05</highlight><highlight class="normal"></highlight></codeline> |
---|
| 524 | <codeline lineno="91"><highlight class="normal"><sp/><sp/>Q[5]=Q[0];</highlight></codeline> |
---|
| 525 | <codeline lineno="92"><highlight class="normal"><sp/><sp/>Q[10]=prevod(0.0001,15);<sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>1e-3</highlight><highlight class="normal"></highlight></codeline> |
---|
| 526 | <codeline lineno="93"><highlight class="normal"><sp/><sp/>Q[15]=prevod(0.0001,15);<sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>1e-3</highlight><highlight class="normal"></highlight></codeline> |
---|
| 527 | <codeline lineno="94"><highlight class="normal"></highlight></codeline> |
---|
| 528 | <codeline lineno="95"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>Tuning<sp/>of<sp/>matrix<sp/>R</highlight><highlight class="normal"></highlight></codeline> |
---|
| 529 | <codeline lineno="96"><highlight class="normal"><sp/><sp/>R[0]=prevod(0.05,15);<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>0.05</highlight><highlight class="normal"></highlight></codeline> |
---|
| 530 | <codeline lineno="97"><highlight class="normal"><sp/><sp/>R[3]=R[0];</highlight></codeline> |
---|
| 531 | <codeline lineno="98"><highlight class="normal"></highlight></codeline> |
---|
| 532 | <codeline lineno="99"><highlight class="normal"></highlight></codeline> |
---|
| 533 | <codeline lineno="100"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">/*<sp/>Tuning<sp/>of<sp/>matrix<sp/>P_est<sp/>-<sp/>initial<sp/>values<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 534 | <codeline lineno="101"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/><sp/><sp/>P_est[0][0]=prevod(0.999,15);</highlight></codeline> |
---|
| 535 | <codeline lineno="102"><highlight class="comment"><sp/><sp/><sp/><sp/>P_est[1][1]=prevod(0.999,15);</highlight></codeline> |
---|
| 536 | <codeline lineno="103"><highlight class="comment"><sp/><sp/><sp/><sp/>P_est[2][2]=prevod(0.999,15);</highlight></codeline> |
---|
| 537 | <codeline lineno="104"><highlight class="comment"><sp/><sp/><sp/><sp/>P_est[3][3]=prevod(0.999,15);</highlight></codeline> |
---|
| 538 | <codeline lineno="105"><highlight class="comment"><sp/><sp/><sp/><sp/>P_est[4][4]=prevod(0.999,15);</highlight></codeline> |
---|
| 539 | <codeline lineno="106"><highlight class="comment">/**/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 540 | <codeline lineno="107"><highlight class="normal"></highlight></codeline> |
---|
| 541 | <codeline lineno="108"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>Motor<sp/>model<sp/>parameters<sp/></highlight><highlight class="normal"></highlight></codeline> |
---|
| 542 | <codeline lineno="109"><highlight class="normal"><sp/><sp/>cA=prevod(1-Tv*Rs/Ls,15);</highlight></codeline> |
---|
| 543 | <codeline lineno="110"><highlight class="normal"><sp/><sp/>cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);</highlight></codeline> |
---|
| 544 | <codeline lineno="111"><highlight class="normal"><sp/><sp/>cC=prevod(Tv/Ls/Iref*Uref,15);</highlight></codeline> |
---|
| 545 | <codeline lineno="112"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>cD=prevod(1-Tv*Bf/J,15);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 546 | <codeline lineno="113"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 547 | <codeline lineno="114"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>cF=prevod(p*Tv*Mref/J/Wref,15);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 548 | <codeline lineno="115"><highlight class="normal"><sp/><sp/>cG=prevod(Tv*Wref*K_PREVOD_FORM/Thetaref,15);</highlight></codeline> |
---|
| 549 | <codeline lineno="116"><highlight class="normal"><sp/><sp/>cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);</highlight></codeline> |
---|
| 550 | <codeline lineno="117"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 551 | <codeline lineno="118"><highlight class="normal"></highlight></codeline> |
---|
| 552 | <codeline lineno="119"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">/*<sp/>Init<sp/>matrix<sp/>PSI<sp/>with<sp/>permanently<sp/>constant<sp/>terms<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 553 | <codeline lineno="120"><highlight class="normal"><sp/><sp/>PSI[0]=cA;</highlight></codeline> |
---|
| 554 | <codeline lineno="121"><highlight class="normal"><sp/><sp/>PSI[5]=PSI[0];</highlight></codeline> |
---|
| 555 | <codeline lineno="122"><highlight class="normal"><sp/><sp/>PSI[10]=0x7FFF;</highlight></codeline> |
---|
| 556 | <codeline lineno="123"><highlight class="normal"><sp/><sp/>PSI[14]=cG;</highlight></codeline> |
---|
| 557 | <codeline lineno="124"><highlight class="normal"><sp/><sp/>PSI[15]=0x7FFF;</highlight></codeline> |
---|
| 558 | <codeline lineno="125"><highlight class="normal">}</highlight></codeline> |
---|
| 559 | <codeline lineno="126"><highlight class="normal"></highlight></codeline> |
---|
| 560 | <codeline lineno="127"><highlight class="normal"></highlight></codeline> |
---|
| 561 | <codeline lineno="128"><highlight class="normal"></highlight><highlight class="keyword">static</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>update_psi(</highlight><highlight class="keywordtype">void</highlight><highlight class="normal">)</highlight></codeline> |
---|
| 562 | <codeline lineno="129"><highlight class="normal">{</highlight></codeline> |
---|
| 563 | <codeline lineno="130"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>t_sin,t_cos,tmp;</highlight></codeline> |
---|
| 564 | <codeline lineno="131"><highlight class="normal"></highlight></codeline> |
---|
| 565 | <codeline lineno="132"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>PC</highlight><highlight class="normal"></highlight></codeline> |
---|
| 566 | <codeline lineno="133"><highlight class="normal"><sp/><sp/>t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);</highlight></codeline> |
---|
| 567 | <codeline lineno="134"><highlight class="normal"><sp/><sp/>t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);</highlight></codeline> |
---|
| 568 | <codeline lineno="135"><highlight class="normal"></highlight></codeline> |
---|
| 569 | <codeline lineno="136"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>DSP</highlight><highlight class="normal"></highlight></codeline> |
---|
| 570 | <codeline lineno="137"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>t_sin=qsin(x_est[3]);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 571 | <codeline lineno="138"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>t_cos=qcos(x_est[3]);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 572 | <codeline lineno="139"><highlight class="normal"></highlight></codeline> |
---|
| 573 | <codeline lineno="140"><highlight class="normal"><sp/><sp/>PSI[2]=((long)cB*t_sin)>>15;</highlight></codeline> |
---|
| 574 | <codeline lineno="141"><highlight class="normal"><sp/><sp/>tmp=((long)cH*x_est[2])>>15;</highlight></codeline> |
---|
| 575 | <codeline lineno="142"><highlight class="normal"><sp/><sp/>PSI[3]=((long)tmp*t_cos)>>15;</highlight></codeline> |
---|
| 576 | <codeline lineno="143"><highlight class="normal"><sp/><sp/>PSI[6]=-((long)cB*t_cos)>>15;</highlight></codeline> |
---|
| 577 | <codeline lineno="144"><highlight class="normal"><sp/><sp/>PSI[7]=((long)tmp*t_sin)>>15;</highlight></codeline> |
---|
| 578 | <codeline lineno="145"><highlight class="normal">}</highlight></codeline> |
---|
| 579 | <codeline lineno="146"><highlight class="normal"></highlight></codeline> |
---|
| 580 | <codeline lineno="147"><highlight class="normal"></highlight></codeline> |
---|
| 581 | <codeline lineno="148"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>prediction(</highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>*ux)</highlight></codeline> |
---|
| 582 | <codeline lineno="149"><highlight class="normal">{</highlight></codeline> |
---|
| 583 | <codeline lineno="150"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>t_sin,t_cos,<sp/>tmp;</highlight></codeline> |
---|
| 584 | <codeline lineno="151"><highlight class="normal"></highlight></codeline> |
---|
| 585 | <codeline lineno="152"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>PC</highlight><highlight class="normal"></highlight></codeline> |
---|
| 586 | <codeline lineno="153"><highlight class="normal"><sp/><sp/>t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);</highlight></codeline> |
---|
| 587 | <codeline lineno="154"><highlight class="normal"><sp/><sp/>t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);</highlight></codeline> |
---|
| 588 | <codeline lineno="155"><highlight class="normal"></highlight></codeline> |
---|
| 589 | <codeline lineno="156"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>implementace<sp/>v<sp/>DSP</highlight><highlight class="normal"></highlight></codeline> |
---|
| 590 | <codeline lineno="157"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>t_sin=qsin(x_est[3]);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 591 | <codeline lineno="158"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>t_cos=qcos(x_est[3]);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 592 | <codeline lineno="159"><highlight class="normal"></highlight></codeline> |
---|
| 593 | <codeline lineno="160"><highlight class="normal"><sp/><sp/>tmp=((long)cB*x_est[2])>>15;</highlight></codeline> |
---|
| 594 | <codeline lineno="161"><highlight class="normal"><sp/><sp/>x_pred[0]=((long)cA*x_est[0]+(</highlight><highlight class="keywordtype">long</highlight><highlight class="normal">)tmp*t_sin+(long)cC*ux[0])>>15;</highlight></codeline> |
---|
| 595 | <codeline lineno="162"><highlight class="normal"><sp/><sp/>x_pred[1]=((long)cA*x_est[1]-(</highlight><highlight class="keywordtype">long</highlight><highlight class="normal">)tmp*t_cos+(long)cC*ux[1])>>15;</highlight></codeline> |
---|
| 596 | <codeline lineno="163"><highlight class="normal"><sp/><sp/>x_pred[2]=x_est[2];</highlight></codeline> |
---|
| 597 | <codeline lineno="164"><highlight class="normal"><sp/><sp/>x_pred[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;</highlight></codeline> |
---|
| 598 | <codeline lineno="165"><highlight class="normal"></highlight></codeline> |
---|
| 599 | <codeline lineno="166"><highlight class="normal"><sp/><sp/>update_psi();</highlight></codeline> |
---|
| 600 | <codeline lineno="167"><highlight class="normal"></highlight></codeline> |
---|
| 601 | <codeline lineno="168"><highlight class="normal"><sp/><sp/>mmult(PSI,P_est,temp15a,3,3,3);</highlight></codeline> |
---|
| 602 | <codeline lineno="169"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>mtrans(PSI,temp15b,5,5);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 603 | <codeline lineno="170"><highlight class="normal"><sp/><sp/>mmultt(temp15a,PSI,P_pred,3,3,3);</highlight></codeline> |
---|
| 604 | <codeline lineno="171"><highlight class="normal"><sp/><sp/>maddD(P_pred,Q,3,3);</highlight></codeline> |
---|
| 605 | <codeline lineno="172"><highlight class="normal">}</highlight></codeline> |
---|
| 606 | <codeline lineno="173"><highlight class="normal"></highlight></codeline> |
---|
| 607 | <codeline lineno="174"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>correction(</highlight><highlight class="keywordtype">void</highlight><highlight class="normal">)</highlight></codeline> |
---|
| 608 | <codeline lineno="175"><highlight class="normal">{</highlight></codeline> |
---|
| 609 | <codeline lineno="176"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Y_error[2];</highlight></codeline> |
---|
| 610 | <codeline lineno="177"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">long</highlight><highlight class="normal"><sp/>temp30a[4];<sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>[2,2]<sp/>-<sp/>temporary<sp/>matrix<sp/>for<sp/>inversion<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 611 | <codeline lineno="178"><highlight class="normal"></highlight></codeline> |
---|
| 612 | <codeline lineno="179"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">/*<sp/>Kalman<sp/>gain<sp/>calculation<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 613 | <codeline lineno="180"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>mmult(P_pred,Ht,temp15a,5,5,2);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 614 | <codeline lineno="181"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/>mmultt(P_pred,H,temp15a,4,4,1);</highlight></codeline> |
---|
| 615 | <codeline lineno="182"><highlight class="comment"><sp/><sp/>mmult(H,temp15a,temp15b,1,4,1);<sp/><sp/><sp/><sp/><sp/><sp/><sp/>these<sp/>lines<sp/>are<sp/>replaced<sp/>by<sp/>choice_P<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 616 | <codeline lineno="183"><highlight class="normal"></highlight></codeline> |
---|
| 617 | <codeline lineno="184"><highlight class="normal"><sp/><sp/>choice_P(P_pred,temp15a,3);</highlight></codeline> |
---|
| 618 | <codeline lineno="185"><highlight class="normal"><sp/><sp/>maddD(temp15a,R,1,1);</highlight></codeline> |
---|
| 619 | <codeline lineno="186"><highlight class="normal"><sp/><sp/>minv2(temp15a,temp30a);</highlight></codeline> |
---|
| 620 | <codeline lineno="187"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/>mmultt(P_pred,H,temp15a,4,4,1);<sp/><sp/><sp/><sp/><sp/>/*<sp/>remove<sp/>this<sp/>line<sp/>if<sp/>choice_P<sp/>is<sp/>not<sp/>used<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 621 | <codeline lineno="188"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>mmultDr15(P_pred,Ht,temp15a,4,4,1,1);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 622 | <codeline lineno="189"><highlight class="normal"><sp/><sp/>mmultDr(P_pred,temp15a,3,3,1,1);</highlight></codeline> |
---|
| 623 | <codeline lineno="190"><highlight class="normal"><sp/><sp/>mmult1530(temp15a,temp30a,Kalm,3,1,1);</highlight></codeline> |
---|
| 624 | <codeline lineno="191"><highlight class="normal"></highlight></codeline> |
---|
| 625 | <codeline lineno="192"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">/*<sp/>estimate<sp/>the<sp/>state<sp/>system<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 626 | <codeline lineno="193"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>mmult(H,x_pred,temp15a,1,4,0);</highlight><highlight class="normal"></highlight></codeline> |
---|
| 627 | <codeline lineno="194"><highlight class="normal"><sp/><sp/>choice_x(x_pred,<sp/>temp15a);</highlight></codeline> |
---|
| 628 | <codeline lineno="195"><highlight class="normal"><sp/><sp/>msub(Y_mes,temp15a,Y_error,1,0);</highlight></codeline> |
---|
| 629 | <codeline lineno="196"><highlight class="normal"><sp/><sp/>mmult(Kalm,Y_error,temp15a,3,1,0);</highlight></codeline> |
---|
| 630 | <codeline lineno="197"><highlight class="normal"><sp/><sp/>madd(x_pred,temp15a,x_est,3,0);</highlight></codeline> |
---|
| 631 | <codeline lineno="198"><highlight class="normal"></highlight></codeline> |
---|
| 632 | <codeline lineno="199"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">/*<sp/>matrix<sp/>of<sp/>covariances<sp/>-<sp/>version<sp/>without<sp/>MMULTDL()<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 633 | <codeline lineno="200"><highlight class="normal"></highlight><highlight class="comment">/*<sp/><sp/>mmult(Kalm,H,temp15a,4,1,4);</highlight></codeline> |
---|
| 634 | <codeline lineno="201"><highlight class="comment"><sp/><sp/>mmult(temp15a,P_pred,P_est,4,4,4);</highlight></codeline> |
---|
| 635 | <codeline lineno="202"><highlight class="comment"><sp/><sp/>msub(P_pred,P_est,P_est,4,4);</highlight></codeline> |
---|
| 636 | <codeline lineno="203"><highlight class="comment">/*<sp/>END<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 637 | <codeline lineno="204"><highlight class="normal"></highlight></codeline> |
---|
| 638 | <codeline lineno="205"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>Version<sp/>with<sp/>MMULTDL()<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 639 | <codeline lineno="206"><highlight class="normal"><sp/><sp/>mmultDl(P_pred,temp15a,1,3,3,1);</highlight></codeline> |
---|
| 640 | <codeline lineno="207"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>if<sp/>result<sp/>matrix<sp/>has<sp/>more<sp/>terms<sp/>than<sp/>DIAG<sp/>matrix,it<sp/>is<sp/>necessary<sp/>to<sp/>enable</highlight></codeline> |
---|
| 641 | <codeline lineno="208"><highlight class="comment">erase<sp/>sequence<sp/>in<sp/>definition<sp/>of<sp/>function<sp/>MMULTDL()<sp/>that<sp/>is<sp/>currently<sp/>disabled.<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 642 | <codeline lineno="209"><highlight class="normal"></highlight></codeline> |
---|
| 643 | <codeline lineno="210"><highlight class="normal"><sp/><sp/>mmult(Kalm,temp15a,P_est,3,1,3);</highlight></codeline> |
---|
| 644 | <codeline lineno="211"><highlight class="normal"><sp/><sp/>msub(P_pred,P_est,P_est,3,3);</highlight></codeline> |
---|
| 645 | <codeline lineno="212"><highlight class="normal"></highlight><highlight class="comment">/*<sp/>END<sp/>*/</highlight><highlight class="normal"></highlight></codeline> |
---|
| 646 | <codeline lineno="213"><highlight class="normal">}</highlight></codeline> |
---|
| 647 | <codeline lineno="214"><highlight class="normal"></highlight></codeline> |
---|
| 648 | <codeline lineno="215"><highlight class="normal"></highlight></codeline> |
---|
| 649 | <codeline lineno="216"><highlight class="normal"></highlight><highlight class="keywordtype">void</highlight><highlight class="normal"><sp/>ekf(</highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>*x_estimation,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Umd,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>beta,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Ucnd,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>Ucd,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>isxd,<sp/></highlight><highlight class="keywordtype">double</highlight><highlight class="normal"><sp/>isyd)</highlight></codeline> |
---|
| 650 | <codeline lineno="217"><highlight class="normal">{</highlight></codeline> |
---|
| 651 | <codeline lineno="218"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Umk,<sp/>ua,<sp/>ub;</highlight></codeline> |
---|
| 652 | <codeline lineno="219"><highlight class="normal"></highlight></codeline> |
---|
| 653 | <codeline lineno="221"><highlight class="normal"><sp/><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal"><sp/>Um,<sp/>Ucn,<sp/>Uc,<sp/>isx,<sp/>isy,<sp/>t_cos;</highlight></codeline> |
---|
| 654 | <codeline lineno="222"><highlight class="normal"></highlight></codeline> |
---|
| 655 | <codeline lineno="223"><highlight class="normal"><sp/><sp/>Um=prevod(Umd/Uref,Qm);</highlight></codeline> |
---|
| 656 | <codeline lineno="224"><highlight class="normal"><sp/><sp/>Ucn=prevod(Ucnd/Uref,Qm);</highlight></codeline> |
---|
| 657 | <codeline lineno="225"><highlight class="normal"><sp/><sp/>Uc=prevod(Ucd/Uref,Qm);</highlight></codeline> |
---|
| 658 | <codeline lineno="226"><highlight class="normal"><sp/><sp/>isx=prevod(isxd/Iref,Qm);</highlight></codeline> |
---|
| 659 | <codeline lineno="227"><highlight class="normal"><sp/><sp/>isy=prevod(isyd/Iref,Qm);</highlight></codeline> |
---|
| 660 | <codeline lineno="229"><highlight class="normal"></highlight></codeline> |
---|
| 661 | <codeline lineno="230"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>prepocet<sp/>napeti<sp/>pro<sp/>EKF<sp/>dle<sp/>skutecneho<sp/>napeti<sp/>v<sp/>ss<sp/>obvodu</highlight><highlight class="normal"></highlight></codeline> |
---|
| 662 | <codeline lineno="231"><highlight class="normal"><sp/><sp/>Umk=((long)Um*Uc)/Ucn;</highlight></codeline> |
---|
| 663 | <codeline lineno="232"><highlight class="normal"></highlight></codeline> |
---|
| 664 | <codeline lineno="233"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>vypocet<sp/>fazovych<sp/>napeti<sp/>stridace</highlight><highlight class="normal"></highlight></codeline> |
---|
| 665 | <codeline lineno="234"><highlight class="normal"><sp/><sp/>ua=((long)Umk*prevod(cos(beta),15))>>15;</highlight></codeline> |
---|
| 666 | <codeline lineno="235"><highlight class="normal"><sp/><sp/>ub=((long)Umk*prevod(cos(beta-2./3.*M_PI),15))>>15;</highlight></codeline> |
---|
| 667 | <codeline lineno="236"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>uc=((long)Umk*prevod(cos(beta+2./3.*M_PI),15))>>15;</highlight><highlight class="normal"></highlight></codeline> |
---|
| 668 | <codeline lineno="237"><highlight class="normal"></highlight></codeline> |
---|
| 669 | <codeline lineno="238"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>vypocet<sp/>napeti<sp/>v<sp/>systemu<sp/>(x,y)</highlight><highlight class="normal"></highlight></codeline> |
---|
| 670 | <codeline lineno="239"><highlight class="normal"><sp/><sp/>ukalm[0]=ua;</highlight></codeline> |
---|
| 671 | <codeline lineno="240"><highlight class="normal"><sp/><sp/>ukalm[1]=(((long)ua+ub<<1)*18917)>>15;<sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>usy=(ua+2*ub)/sqrt(3)</highlight><highlight class="normal"></highlight></codeline> |
---|
| 672 | <codeline lineno="241"><highlight class="normal"></highlight></codeline> |
---|
| 673 | <codeline lineno="242"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>zadani<sp/>mereni</highlight><highlight class="normal"></highlight></codeline> |
---|
| 674 | <codeline lineno="243"><highlight class="normal"><sp/><sp/>Y_mes[0]=isx;</highlight></codeline> |
---|
| 675 | <codeline lineno="244"><highlight class="normal"><sp/><sp/>Y_mes[1]=isy;</highlight></codeline> |
---|
| 676 | <codeline lineno="245"><highlight class="normal"></highlight></codeline> |
---|
| 677 | <codeline lineno="247"><highlight class="normal"><sp/><sp/>prediction(ukalm);</highlight></codeline> |
---|
| 678 | <codeline lineno="248"><highlight class="normal"><sp/><sp/>correction();</highlight></codeline> |
---|
| 679 | <codeline lineno="249"><highlight class="normal"></highlight></codeline> |
---|
| 680 | <codeline lineno="250"><highlight class="normal"><sp/><sp/></highlight><highlight class="comment">//<sp/>navrat<sp/>estimovanych<sp/>hodnot<sp/>regulatoru</highlight><highlight class="normal"></highlight></codeline> |
---|
| 681 | <codeline lineno="251"><highlight class="normal"><sp/><sp/>*x_estimation=zprevod(x_est[2],Qm)*Wref;</highlight></codeline> |
---|
| 682 | <codeline lineno="252"><highlight class="normal"></highlight><highlight class="comment">//<sp/><sp/>x_est[3]=(short<sp/>int)x_est[3];</highlight><highlight class="normal"></highlight></codeline> |
---|
| 683 | <codeline lineno="253"><highlight class="normal"><sp/><sp/>*(x_estimation+1)=zprevod((</highlight><highlight class="keywordtype">short</highlight><highlight class="normal"><sp/></highlight><highlight class="keywordtype">int</highlight><highlight class="normal">)x_est[3],15)*Thetaref;</highlight></codeline> |
---|
| 684 | <codeline lineno="254"><highlight class="normal">}</highlight></codeline> |
---|
| 685 | </programlisting> |
---|
| 686 | <location file="/home/smidl/work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp"/> |
---|
| 687 | </compounddef> |
---|
| 688 | </doxygen> |
---|