977 lines
64 KiB
C++
977 lines
64 KiB
C++
#ifndef ACSPARAMETERS_H_
|
|
#define ACSPARAMETERS_H_
|
|
|
|
#include <fsfw/parameters/HasParametersIF.h>
|
|
|
|
#include <cmath>
|
|
|
|
typedef unsigned char uint8_t;
|
|
|
|
class AcsParameters : public HasParametersIF {
|
|
private:
|
|
static constexpr double DEG2RAD = M_PI / 180.;
|
|
|
|
public:
|
|
AcsParameters();
|
|
virtual ~AcsParameters();
|
|
|
|
ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
|
|
ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues,
|
|
uint16_t startAtIndex) override;
|
|
|
|
struct OnBoardParams {
|
|
double sampleTime = 0.4; // [s]
|
|
uint16_t ptgCtrlLostTimer = 750;
|
|
uint8_t fusedRateSafeDuringEclipse = true;
|
|
uint8_t fusedRateFromStr = true;
|
|
uint8_t fusedRateFromQuest = true;
|
|
double questFilterWeight = 0.9;
|
|
} onBoardParams;
|
|
|
|
struct InertiaEIVE {
|
|
/* Possible inertia matrices
|
|
* 2023-04-14
|
|
* all matrices derived from the CAD model with CAD mass of 8.72 kg
|
|
* all matrices are scaled by final measured mass of 8.756 kg
|
|
* all matrices are in [kg m^2]
|
|
*/
|
|
double inertiaMatrixDeployed[3][3] = {
|
|
{0.128333461640235, -0.000151805020803, -0.004178414952517},
|
|
{-0.000151805020803, 0.141791062870050, 0.000395791231451},
|
|
{-0.004178414952517, 0.000395791231451, 0.069793610830099}};
|
|
double inertiaMatrixUndeployed[3][3] = {
|
|
{0.102082611845982, -0.000149885620646, -0.004174050971653},
|
|
{-0.000149885620646, 0.135214836333048, 0.000396374487363},
|
|
{-0.004174050971653, 0.000396374487363, 0.050118987572848}};
|
|
double inertiaMatrixPanel1[3][3] = {{0.115207454653725, -0.000153027308288, -0.004177193002842},
|
|
{-0.000153027308288, 0.138513727361148, 0.006962185987503},
|
|
{-0.004177193002842, 0.006962185987503, 0.059944939352491}};
|
|
double inertiaMatrixPanel3[3][3] = {
|
|
{0.115208618832493, -0.000148663333161, -0.004175272921328},
|
|
{-0.000148663333161, 0.138492171841952, -0.006170020268690},
|
|
{-0.004175272921328, -0.006170020268690, 0.059967659050454}};
|
|
} inertiaEIVE;
|
|
|
|
struct MgmHandlingParameters {
|
|
float mgm0orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
|
|
float mgm1orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}};
|
|
float mgm2orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
|
|
float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}};
|
|
float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
|
|
|
|
float mgm0hardIronOffset[3] = {6.116487, 6.796264, -19.188060};
|
|
float mgm1hardIronOffset[3] = {-1.077152, 2.080583, 1.974483};
|
|
float mgm2hardIronOffset[3] = {-19.285857, 5.401821, -16.096297};
|
|
float mgm3hardIronOffset[3] = {-0.634033, 2.787695, 0.092036};
|
|
float mgm4hardIronOffset[3] = {2.702743, 5.236043, 0.726229};
|
|
|
|
float mgm0softIronInverse[3][3] = {{0.910192, -0.188413, -0.161522},
|
|
{-0.188413, 1.642303, -0.033184},
|
|
{-0.161522, -0.033184, 0.943904}};
|
|
float mgm1softIronInverse[3][3] = {{1.053508, -0.170225, -0.041678},
|
|
{-0.170225, 1.274465, -0.040231},
|
|
{-0.041678, -0.040231, 1.086352}};
|
|
float mgm2softIronInverse[3][3] = {{0.931086, 0.172675, -0.043084},
|
|
{0.172675, 1.541296, 0.065489},
|
|
{-0.043084, 0.065489, 1.001238}};
|
|
float mgm3softIronInverse[3][3] = {{1.073353, 0.177266, -0.058832},
|
|
{0.177266, 1.262156, 0.010478},
|
|
{-0.058832, 0.010478, 1.068345}};
|
|
float mgm4softIronInverse[3][3] = {{1.114887, -0.007534, -0.037072},
|
|
{-0.007534, 1.253879, 0.006812},
|
|
{-0.037072, 0.006812, 1.313158}};
|
|
|
|
double mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
|
|
double mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
|
|
double mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
|
|
float mgmVectorFilterWeight = 0.85;
|
|
float mgmDerivativeFilterWeight = 0.99;
|
|
uint8_t useMgm4 = false;
|
|
} mgmHandlingParameters;
|
|
|
|
struct SusHandlingParameters {
|
|
float sus0orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM10
|
|
float sus1orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; // FM06
|
|
float sus2orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; // FM13
|
|
float sus3orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; // FM14
|
|
float sus4orientationMatrix[3][3] = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}; // FM05
|
|
float sus5orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM02
|
|
float sus6orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM07
|
|
float sus7orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; // FM01
|
|
float sus8orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; // FM03
|
|
float sus9orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; // FM11
|
|
float sus10orientationMatrix[3][3] = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}; // FM09
|
|
float sus11orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM08
|
|
|
|
float sus0coeffAlpha[9][10] = {
|
|
{13.0465222152293, 0.0639132159808454, 2.98083557560227, -0.0773202212713293,
|
|
0.0949075412003712, 0.0503055998355815, -0.00104133434256204, 0.000633099036136146,
|
|
0.00091428505258307, 0.000259857066722932},
|
|
{1.66740227859888, 1.55804368674744, 0.209274741749388, 0.0123798418560859,
|
|
0.00724950517167516, -0.000577445375457582, 8.94374551545955e-05, 6.94513586221567e-05,
|
|
-1.06065583714065e-05, -1.43899892666699e-05},
|
|
{8.71610925597519, 1.42112818752419, -0.549859300501301, 0.0374581774684577,
|
|
0.0617635595955198, 0.0447491072679598, 0.00069998577106559, 0.00101018723225412,
|
|
-4.88501228194031e-06, -0.000434861113274231},
|
|
{-2.3555601314395, 1.29430213886389, 0.179499593411187, 0.00440896450927253,
|
|
0.00352052300927628, 0.00434187143967281, -9.66615195654703e-05, 3.64923075694275e-05,
|
|
6.09619017310129e-05, 4.23908862836885e-05},
|
|
{-0.858019663974047, 1.10138705956076, 0.278789852526915, -0.000199798507752607,
|
|
0.00112092406838628, -0.00177346866231588, 0.000217816070307086, -0.000240713988238257,
|
|
0.000150795563555828, -0.000279246491927943},
|
|
{7.93661480471297, 1.33902098855997, -0.64010306493848, -0.00307944184518557,
|
|
-0.00511421127083497, 0.0204008636376403, -9.50042323904954e-05, 6.01530207062221e-05,
|
|
9.13233708460098e-05, -0.000206717750924323},
|
|
{16.2658124154565, 0.191301571705827, 1.02390350838635, 0.0258487436355216,
|
|
-0.0219752092833362, 0.0236916776412211, -0.000350496453661261, -0.000123849795280597,
|
|
-0.000532190902882765, 9.36018171121253e-05},
|
|
{-1.53023612303052, 1.29132951637076, 0.181159073530008, -0.0023490608317645,
|
|
-0.00370741703297037, -0.000229071300377431, -1.6634455407558e-05, 1.11387154630828e-05,
|
|
1.02609175615251e-05, -9.64717658954667e-06},
|
|
{-32.9918791079688, 0.093536793089853, 4.76858627395571, 0.0595845684553358,
|
|
-0.054845749101257, -0.133247382500001, -0.000688999201915199, 7.67286265747961e-05,
|
|
0.000868163357631254, 0.00120099606910313}};
|
|
float sus0coeffBeta[9][10] = {
|
|
{12.7380220453847, -0.6087309901836, 2.60957722462363, -0.0415319939920917,
|
|
0.0444944768824276, 0.0223231464060241, -0.000421503508733887, -9.39560038638717e-05,
|
|
0.000821479971871302, -4.5330528329465e-05},
|
|
{1.96846333975847, -0.33921438143463, 1.23957110477613, -0.00948832495296823,
|
|
0.00107211134687287, -0.00410820045700199, -9.33679611473279e-05, 3.72984782145427e-05,
|
|
-4.04514487800062e-05, -7.6296149087237e-05},
|
|
{5.7454444934481, -1.58476383793609, -0.418479494289251, -0.0985177320630941,
|
|
-0.0862179276808015, 0.0126762052037897, -0.00118207758271301, -0.000190361442918412,
|
|
0.0011723869613426, 0.000122882034141316},
|
|
{2.11042287406433, -0.225942746245056, 1.18084080712528, -0.00103013931607172,
|
|
-0.00675606790663387, -0.00106646109062746, 1.7708839355979e-05, -3.13642668374253e-05,
|
|
-5.87601932564404e-05, -3.92033314627704e-05},
|
|
{2.96049248725882, -0.286261455028255, 1.09122556181319, -0.000672369023155898,
|
|
0.000574446975796023, 0.000120303729680796, 0.000292285799270644, 0.000170497873487264,
|
|
0.000259925974231328, 0.000222437797823852},
|
|
{1.65218061201483, -0.19535446105784, 1.39609640918411, 0.000961524354787167,
|
|
0.00592400381724333, -0.0078500192096718, -7.02791628080906e-07, -2.07197580883822e-05,
|
|
-4.33518182614169e-05, 4.66993119419691e-05},
|
|
{-19.56673237415, 1.06558565338761, 0.151160448373445, -0.0252628659378108,
|
|
0.0281230551050938, -0.0217328869907185, 0.000241309440918385, -0.000116449585258429,
|
|
0.000401546410974577, -0.000147563886502726},
|
|
{1.56167171538684, -0.155299366654736, 1.20084049723279, 0.00457348893890231,
|
|
0.00118888040006052, 0.0029920178735941, -5.583448120596e-05, -2.34496315691865e-05,
|
|
-5.3309466243918e-05, 6.20289310356821e-06},
|
|
{1.95050549495182, -2.74909818412705, 3.80268788018641, 0.0629242254381785,
|
|
0.0581479035315726, -0.111361283351269, -0.00047845777495158, -0.00075354297736741,
|
|
-0.000186887396585446, 0.00119710704771344}};
|
|
float sus1coeffAlpha[9][10] = {
|
|
{-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084,
|
|
-0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176,
|
|
-0.000178087038629721, 4.09208968678946e-05},
|
|
{2.24803085641869, 1.42938692406645, 0.30104994020693, 0.00756499999397385,
|
|
0.0117765927439368, -0.000743685980641362, 4.69920803836194e-05, 0.000129815636957956,
|
|
-9.10792250542345e-06, -2.03870119873411e-05},
|
|
{26.9943033817917, 0.147791175366868, -3.48256070200564, -0.0303332422478656,
|
|
0.0183377266255394, 0.124593616125966, -0.000466003049304431, -0.000272000698791331,
|
|
-0.00063621309529853, -0.00158363678978767},
|
|
{-0.221893380318465, 1.29919955307083, 0.21872487901019, 0.0049448219667127,
|
|
0.00291224091529189, 0.00654651987282984, -9.86842469311185e-05, 8.20057454706638e-05,
|
|
6.42331081725944e-05, 7.11656918299053e-05},
|
|
{1.40178843964621, 1.1733111455249, 0.287485528779234, -0.000793970428759834,
|
|
0.000170529273905818, -0.00268807864923086, 9.09553964483881e-05, -0.000271892733575409,
|
|
8.52016306311741e-05, -0.000291797625433646},
|
|
{0.65549617899457, 1.25716478394514, 0.301396415134214, -0.00357289640403958,
|
|
-0.000473416364133431, -0.010760332636205, -9.77220176481185e-05, 4.40798040046875e-05,
|
|
2.84958344955681e-05, 0.000128583400693359},
|
|
{6.20958048145025, 1.9528406481596, 1.32915657614139, -0.0326944423378284,
|
|
-0.0158258335207969, 0.0328249756354635, 0.00027113042931131, -0.000133980867173428,
|
|
-0.000357964552318811, 0.000224235061786191},
|
|
{2.46222812180944, 1.1731834908026, 0.17440330925151, -0.00132279581980401,
|
|
-0.00447202005426964, -0.000804321602550913, -1.59526570766446e-05, 2.62946483533391e-05,
|
|
3.28466749016414e-05, -6.63837547601294e-06},
|
|
{42.615758859473, 2.46617281707273, -5.742515881283, -0.131942799763164, 0.20250702826603,
|
|
0.0981562802911027, 0.00189939440077981, -0.0018591621618441, -0.00161121179693977,
|
|
-0.00058814458116749}};
|
|
float sus1coeffBeta[9][10] = {
|
|
{-12.300032617206, -1.06640894101328, 0.33950802247214, -0.00890867870617722,
|
|
-0.04872758086642, -0.0114263851027856, 0.000141061196404012, -0.000675469545483099,
|
|
-0.000138249928781575, -0.000138871036200597},
|
|
{10.1631114109768, 0.261654603839785, 1.2376413405181, 0.00888558138614535,
|
|
0.00151674939001532, -0.00534577602313027, 9.19430013005559e-05, 5.39804599087081e-05,
|
|
-4.15760162347772e-05, -7.60797902457032e-05},
|
|
{-30.142329062199, 1.26939195100229, 6.14467186367471, 0.0464163689935328,
|
|
0.00379001947505376, -0.165444163648109, 0.000516545385538741, 1.56053219154647e-05,
|
|
-5.58651971370719e-05, 0.00173185063955313},
|
|
{12.1454103989862, -0.243589095509132, 2.02543716988677, -0.000857989774598331,
|
|
-0.00705278543432513, 0.0250580538307654, 3.50683653081847e-05, -2.63093897408875e-05,
|
|
-5.67352645830913e-05, 0.000232270832022029},
|
|
{4.4338108906594, -0.305276965994378, 1.17293558142526, 0.000152618994429577,
|
|
0.00134432642920902, -0.00104036813342885, 0.000334476082056995, 6.74826804343671e-05,
|
|
0.000275311897725414, 7.58157740577916e-05},
|
|
{3.47680700379043, -0.154163381023597, 1.389579838768, 0.000799705880026268,
|
|
0.00401980026462874, -0.00915311817354667, -2.54817301605075e-06, -2.27422984169921e-05,
|
|
-2.61224817848938e-05, 6.00381132540332e-05},
|
|
{29.469181543703, -0.722888948550437, 3.3623377135197, 0.00148445490093232,
|
|
-0.0474780142430845, 0.0486755575785462, 0.000126295091963757, 0.000526632230895258,
|
|
-0.000259305985126003, 0.000412751148048724},
|
|
{2.67029041722834, -0.0837968038501666, 1.37628504937018, 0.00165061312885753,
|
|
-0.00953813055064273, 0.0032433005486936, -1.6522452172598e-05, 0.000144574078261271,
|
|
-8.47348746872376e-05, -1.92509604512729e-06},
|
|
{-20.959201441285, -2.23605897639125, 5.73044624806043, 0.0354141964763815,
|
|
0.0887545371234514, -0.193862330062381, 0.000216532998121618, -0.00207707610520973,
|
|
0.000552928905346826, 0.00190182163597828}};
|
|
float sus2coeffAlpha[9][10] = {
|
|
{6.51602979328333, 0.690575501042577, 1.18185457002269, -0.0153161662266588,
|
|
0.00145972227341484, 0.0351496474730776, -0.000172645571366945, -6.04213053580018e-05,
|
|
9.74494676304114e-05, 0.000334122888261002},
|
|
{0.954398509323963, 1.10996214782069, 0.313314231563221, -0.00367553051112208,
|
|
0.0110290193380194, 0.000240079475656232, -6.93444423181303e-05, 0.000107433381295167,
|
|
1.30750132315838e-05, -2.43580795300515e-05},
|
|
{-55.1159841655056, -1.47449655191106, 3.40106264596874, -0.0621428271456258,
|
|
0.0659788065633613, -0.0791732068323335, -0.000524264070592741, 0.000582093651418709,
|
|
-0.000586102213707195, 0.000658133691098817},
|
|
{1.98614148820353, 1.32058724763677, 0.156843003413303, 0.002748082456053,
|
|
0.00202677073171519, 0.00382360695862248, -0.000122364309010211, 5.33354637965168e-05,
|
|
3.93641210098335e-05, 4.06398431916703e-05},
|
|
{3.41223117010734, 1.1597568029329, 0.31881674291653, -0.000382400010917784,
|
|
-0.000754945672515052, -0.00079200882313927, 0.000145713118224563, -0.00026910957285589,
|
|
0.000137876961532787, -0.000326798596746712},
|
|
{6.23333031852853, 1.24902998148103, -0.0162317540018123, -0.00338184464699201,
|
|
0.000420329743164687, 0.00202038442335185, -7.10435889754986e-05, -6.04039458988991e-06,
|
|
7.25318569569788e-06, -2.5930447720704e-05},
|
|
{191.759784636909, -10.5228276216193, 8.48306234734519, 0.243240262512846,
|
|
-0.344226468125615, 0.126267158197535, -0.00186612281541009, 0.00304415728817747,
|
|
-0.00304958575196089, 0.000457236034569107},
|
|
{5.61375025356727, 1.1692295110657, 0.224665256727786, -0.00230481633344849,
|
|
-0.00746693012026367, -0.00172583925345173, -7.00823444553058e-06, 7.31362778266959e-05,
|
|
5.81988007269583e-05, 1.3723604109425e-05},
|
|
{98.0250669452855, -2.18500123986039, -6.68238707939385, 0.000754807832106659,
|
|
0.256133336978808, 0.110826583415768, 0.000457663127670018, -0.00197655629847616,
|
|
-0.00254305206375073, -0.000466731538082995}};
|
|
float sus2coeffBeta[9][10] = {
|
|
{41.1102358678699, 2.3034699186519, 2.74551448799899, 0.061701310929235, 0.0317074142089495,
|
|
0.0308171492962288, 0.00049453042200054, 0.000519222896270701, 2.85420168881716e-05,
|
|
0.000259197384126413},
|
|
{4.46821725251333, 0.0125273331991983, 1.32640678842532, 0.000543566569079156,
|
|
0.00396616601484022, -0.00488408099728387, -3.05734704054868e-06, 7.3424831303621e-05,
|
|
-5.49439160235527e-05, -8.30708110469922e-05},
|
|
{64.773396165255, 2.97057686090134, -1.90770757709096, 0.062747116236773,
|
|
-0.077990648565002, 0.0613989204238974, 0.00055512113297293, -0.000347045533958329,
|
|
0.00104059576098392, -0.000348638726253297},
|
|
{3.10352939390402, -0.2376108554276, 1.60523925160222, 0.00116454605680723,
|
|
-0.0067958260462381, 0.0136561370875238, 2.59929059167486e-05, 3.33825895937897e-05,
|
|
-5.55828531601728e-05, 0.000109833374761172},
|
|
{0.156052891975873, -0.320721597024578, 1.15208488414874, 0.00164743688819939,
|
|
0.000534718891498932, 0.000469870758457642, 0.000308432468885207, 0.00011789470679678,
|
|
0.000292373398965513, 0.000183599033441813},
|
|
{2.84967971406268, -0.21374251183113, 1.09938586447269, 2.34894704600407e-05,
|
|
0.00588345375399262, 0.00296966835738407, 8.42707308834155e-06, 2.81870099202641e-06,
|
|
-3.56732787246631e-05, -7.04534663356379e-05},
|
|
{-7.59892007483895, 0.358662160515702, 0.805137646978357, 0.00222144376998348,
|
|
0.0464438387809707, 0.00847551828841782, 3.24805702347551e-05, 5.45500807838332e-05,
|
|
0.000941378089367713, 0.000353137737023192},
|
|
{-4.65367165487109, 0.201306010390421, 1.19135575710523, -0.00692801521395975,
|
|
0.00394118754078443, 0.00426360093528599, 6.297683536736e-05, -7.15794236895102e-05,
|
|
-7.47076172176468e-05, -1.94516917836346e-05},
|
|
{-59.5882618930651, 3.84530212586425, 3.50497032358686, -0.116100453177197,
|
|
-0.0380997421813177, -0.0581898335691109, 0.00111464935006159, 0.000559313074537689,
|
|
0.000168067749764069, 0.000563224178849256}};
|
|
float sus3coeffAlpha[9][10] = {
|
|
{-174.687021034355, -7.53454036765748, -9.33798316371397, -0.18212338430986,
|
|
-0.242523652239734, -0.202086838965846, -0.00138648793335223, -0.00225430176012882,
|
|
-0.00198887215340364, -0.00160678535160774},
|
|
{6.92009692410602, 1.8192037428209, 0.254908171908415, 0.0179273243472017,
|
|
0.00894059238779664, -0.000436952529644, 0.000138070523903458, 9.22759645920339e-05,
|
|
-9.4312261303588e-06, -1.76647897892869e-05},
|
|
{-17.9720006944368, 0.230510201259892, 1.10751755772907, -0.00973621304161327,
|
|
0.0554450499198677, -0.00590970792122449, -2.92393772526592e-05, 0.000444329929586969,
|
|
-0.000436055839773919, -9.5869891049503e-05},
|
|
{-4.9880829382985, 1.33627775121504, -0.330382157073243, 0.00306744056311184,
|
|
0.00376353074674973, -0.0107453978169225, -0.00010680477021693, 5.17225535432745e-05,
|
|
7.4423443938376e-05, -0.000107927900087035},
|
|
{0.952867982900728, 1.14513280899596, 0.307744203675505, 0.000404669974219378,
|
|
-0.000737988606997615, 0.00120218232577844, 0.000191147653645603, -0.000275058867995882,
|
|
0.000137187356620739, -0.000320202731145004},
|
|
{8.076706574364, 1.31338618710295, -0.334634356394277, -0.00209719438033295,
|
|
-0.00381753503582303, 0.0100347823323616, -7.00550548221671e-05, -1.97626956996069e-05,
|
|
7.80079707003333e-05, -8.95904360920744e-05},
|
|
{-82.4748312650249, 8.63074484663009, -0.949295700187556, -0.178618807265278,
|
|
0.130143669167547, 0.0284326533865768, 0.00149831261351137, -0.0011583692969717,
|
|
0.0010560778729661, 0.000635404380970666},
|
|
{3.34457857521978, 1.09242517408071, 0.241722402244944, 0.00381629887587041,
|
|
-0.00863580122530851, 0.00137050492069702, -8.91046701171713e-05, 8.44169683308007e-05,
|
|
-3.54608413548779e-05, 8.54042677832451e-06},
|
|
{78.1540457908649, -1.30266922193303, -5.33605443700115, 0.0184226131926499,
|
|
0.146629920899062, 0.110698519952472, 6.64041537651749e-05, -0.00120174584530713,
|
|
-0.00133177694921411, -0.000796422644338886}};
|
|
float sus3coeffBeta[9][10] = {
|
|
{-31.5704266802979, -5.10700699133189, 2.84549700473812, -0.122701561048957,
|
|
-0.11257100034746, 0.102120576206517, -0.000796645106694696, -0.00192211266325167,
|
|
-4.99981232866237e-05, 0.00104036677004523},
|
|
{-0.734294938181273, -0.0694317595592039, 1.34746975389878, -0.00103465544451119,
|
|
0.00389798465946559, -0.00308561832194191, -2.91843250099708e-06, 7.59634622232999e-05,
|
|
-6.54571602919161e-05, -0.000104146832644606},
|
|
{24.2649069708536, 3.08145095664586, 1.88975821636026, 0.0767528234206466,
|
|
-0.0526971951753399, -0.0477053831942802, 0.000613806533422364, -0.000631628059238499,
|
|
0.00026217621127941, 0.000555307997961608},
|
|
{0.62884078560034, -0.152668817824194, 1.70304497205574, 0.000894387499536142,
|
|
-0.00306495168098874, 0.0180087418010658, 1.74990847586174e-05, 3.1263263531046e-05,
|
|
-7.1643235604579e-06, 0.000147876621100347},
|
|
{-3.05400297018165, -0.316256447664344, 1.14841722699638, 0.000671621084688467,
|
|
-0.000906765726598906, 0.000687041032077189, 0.000323419818039841, 0.000128019308781935,
|
|
0.000286018723737538, 0.000192248693306256},
|
|
{-4.39855066935163, -0.322858945262125, 1.44405016355615, -4.93181749911261e-05,
|
|
0.0127396834052722, -0.00523149676786941, 2.56561922352657e-05, 7.61202764874326e-06,
|
|
-0.00014623717850039, 8.12219846932013e-06},
|
|
{110.820397525173, -10.9497307382094, 2.48939759290446, 0.296585618718034,
|
|
-0.142611297893517, -0.0141810186612052, -0.00275127095595919, 0.00160686698368569,
|
|
-0.000872029428758877, -0.000410522437887563},
|
|
{-7.15740446281205, 0.104233532313688, 1.13155893729292, -0.00350418544400852,
|
|
0.00532058598508803, 0.00459314980222008, 3.09155436939349e-05, -7.60935741692174e-05,
|
|
-5.87922606348196e-05, 2.56146268588382e-05},
|
|
{44.8818060495112, -7.94729992210875, 3.59286389225051, 0.217944601088562,
|
|
0.108087933176612, -0.116711715153385, -0.00194260120960441, -0.0015752762498594,
|
|
-0.000331129410732722, 0.00125896996438418}};
|
|
float sus4coeffAlpha[9][10] = {
|
|
{-12.4581187126738, 0.398038572289047, -0.438887880988151, -0.00965382887938283,
|
|
-0.0309322349328842, -0.00359106522420111, -7.79546112299913e-06, -0.000432733997178497,
|
|
-9.79031907635314e-05, -1.49299384451257e-05},
|
|
{8.41054378583447, 1.87462327360707, 0.266809999719952, 0.0216455385250676,
|
|
0.00879426079919981, -0.00142295319820553, 0.000194819780653264, 8.57549705064449e-05,
|
|
-3.56478452552367e-05, -1.65680920554434e-05},
|
|
{16.4141780945815, 2.57697842088604, 0.373972171754278, 0.0498264199400303,
|
|
0.0183175817756131, -0.008545409848878, 0.000422696533006382, -0.000268245978898508,
|
|
-0.000663188021815416, -7.51144017137827e-05},
|
|
{0.796692054977593, 1.26773229735266, 0.247715261673662, 0.00358183885438128,
|
|
0.00216435175662881, 0.00713732829335305, -0.000110129715615857, 3.56051594182427e-05,
|
|
5.03074365340535e-05, 8.40279146176271e-05},
|
|
{2.37491588500165, 1.05997969088519, 0.309540461340971, -0.000405047711742513,
|
|
0.000462224730316111, -0.00201887171945793, 0.000260159805167265, -0.000282867209803598,
|
|
0.000201613303652666, -0.000277796442847579},
|
|
{6.36749007598708, 1.31659760017973, -0.122724934153231, -0.00328808937096891,
|
|
-0.00577347207798776, 0.00403172074457999, -7.45676459772001e-05, 1.79838644222274e-05,
|
|
0.000104552066440564, -2.78115121929346e-05},
|
|
{-47.9667098848496, 3.97703197139796, -1.96403894754299, -0.0577989657406978,
|
|
0.0634225576208007, -0.0346023445055141, 0.00045886475369098, -0.000326132951996844,
|
|
0.000716490441845967, -0.000136132038635483},
|
|
{6.21505474256094, 0.871830486201601, 0.286906473833627, 0.007875292606045,
|
|
-0.00974634725746389, 0.00128416935792136, -0.000111796743751489, 0.000102016719989187,
|
|
-3.3503088289589e-05, -1.03874407813931e-05},
|
|
{102.09801265482, -4.12715152309748, -5.04594403360339, 0.075499959116996,
|
|
0.216574192561683, 0.0750031215784663, -0.000147358932612646, -0.0023710703422108,
|
|
-0.00143310719642393, -0.000431914403446768}};
|
|
float sus4coeffBeta[9][10] = {
|
|
{-21.5077132684032, -1.60004839699939, -0.0298995033958561, -0.0315563250430659,
|
|
-0.0424403625879891, -0.0245426225510417, -0.000209861203016225, -0.000422150973104431,
|
|
-0.00030514398458781, -0.000211986731019738},
|
|
{9.07644247897601, 0.207457289788099, 1.26735366597312, 0.00768477352180427,
|
|
0.00429230749575816, -0.00514802326062087, 7.56149591998578e-05, 8.42794730840662e-05,
|
|
-3.62215715492783e-05, -5.24384190165239e-05},
|
|
{-33.5225408043693, -3.11167857248829, 1.91760591695775, -0.0963752386435729,
|
|
0.00026620241534153, -0.0256680391021823, -0.00102188712837393, 2.63753563968978e-05,
|
|
0.000113172463974702, 0.000271939918507175},
|
|
{19.1379025029401, -0.225979661987912, 2.72337120022998, -0.00136982412154458,
|
|
-0.00447301210555274, 0.046496718064139, 2.09123846958985e-05, -4.30383094864847e-05,
|
|
-1.22808643520768e-05, 0.000440555709696048},
|
|
{2.957867714783, -0.316069593806939, 1.06379930645214, 0.00103244713047271,
|
|
0.00148059212230411, 0.000557885068990542, 0.000288633931072557, 0.000172775380291659,
|
|
0.000269738457990237, 0.000254577019084984},
|
|
{2.04155199929521, -0.318303488378033, 1.37820715117028, 0.00114788656817743,
|
|
0.0130051117909245, -0.00743109928493789, 1.22403390396844e-05, -3.19245785131217e-05,
|
|
-0.000156735218010879, 3.81458400945988e-05},
|
|
{27.314954181241, -1.43916155634084, 2.48967706992348, 0.0278695408478388,
|
|
-0.0341141456915131, 0.0281959785297513, -0.000252996164135396, 0.000163365679366542,
|
|
-0.000380129463154642, 0.000159350154429114},
|
|
{-0.274693278266294, 0.0199711721436635, 1.26676843352524, -0.0006713759238817,
|
|
-0.00389715205101059, 0.00294298337610857, -9.58643121413979e-06, 6.30700938550725e-05,
|
|
-6.07188867796123e-05, 7.72199861279611e-06},
|
|
{-74.1601853968901, 2.55641628908672, 6.38533530714782, -0.0582345132980647,
|
|
-0.0653804553172819, -0.138850555683872, 0.000489364157827405, 0.000469559629292745,
|
|
0.000698140692952438, 0.00123017528239406}};
|
|
float sus5coeffAlpha[9][10] = {
|
|
{-12.1398741236355, 1.99425442858125, -1.9303044815802, 0.0418421279520049,
|
|
-0.0309683799946315, -0.0562201123081437, 0.000522607299552916, -0.000375386573815007,
|
|
-0.000183899715035788, -0.000600349486293698},
|
|
{4.51862054729553, 1.72396080253297, 0.274562680698765, 0.0162681383591035,
|
|
0.0108410181586673, -0.000272215427359511, 0.000124164068046579, 0.000125586897851351,
|
|
-1.24082224214974e-05, -1.63339067540159e-05},
|
|
{63.0100748193658, 7.78014670478172, 0.327263471268564, 0.181264302704374,
|
|
-0.0652454854214506, -0.03906716801285, 0.00166924078925478, -0.000749939315526625,
|
|
0.000320696101132374, 0.000499934751180042},
|
|
{-2.14377722994325, 1.33617641673436, 0.0973465660282871, 0.00389526886867845,
|
|
0.00526064997381395, 0.00244964888333519, -8.59416490903541e-05, 4.58871931007681e-05,
|
|
8.6123353128647e-05, 2.85447259858337e-05},
|
|
{0.164792977301912, 1.17541977248641, 0.348838798760518, -0.000180865118417534,
|
|
0.000331789515553421, -0.000734333865631793, 9.76677859410759e-05, -0.000324347075049525,
|
|
8.66683396011167e-05, -0.000385839566009832},
|
|
{-0.228934187493575, 1.30552820143752, 0.306779576899158, -0.00508763741184706,
|
|
-0.00318524263093038, -0.00878095392529144, -6.59040013073836e-05, 8.69122529321691e-05,
|
|
5.73853071731283e-05, 8.56628414466758e-05},
|
|
{22.6047744510684, -0.591739857860868, 0.566728856847393, 0.0498124268150265,
|
|
-0.0214126910277926, 0.00538091942017912, -0.000391517685229849, 0.000554321668236216,
|
|
0.000191004410219065, 0.000102775124022018},
|
|
{4.54704081104052, 0.844841244606025, 0.181355971462193, 0.0109743851006749,
|
|
-0.00363467884122547, 0.00108873046814694, -0.000153236888951059, 3.14623342713789e-06,
|
|
-2.78503202185463e-05, 3.99983788680736e-06},
|
|
{-30.878359404848, 5.20536009886854, -0.674455093700773, -0.10801865891189,
|
|
-0.0514805639475938, 0.0503660452068572, 0.00072776817295273, 0.00120288537038655,
|
|
-0.000301602375634166, -0.000477098479809266}};
|
|
float sus5coeffBeta[9][10] = {
|
|
{16.8155737032787, 0.65475660868259, 1.95532810363711, 0.000295624718662669,
|
|
0.0426379914736747, 0.00192544771588337, -4.94534888281508e-05, 8.32299142575155e-05,
|
|
0.000645497238623369, -0.000234155227840799},
|
|
{9.48268090632318, 0.528942263930744, 1.34030963800712, 0.0173605129814363,
|
|
0.00581086655972212, -0.00365006277801141, 0.000180048140973223, 0.000102002650672644,
|
|
-4.10833110241736e-05, -8.7810396165556e-05},
|
|
{-47.8325489165383, -4.78262055949503, 1.66912859871505, -0.143518014673292,
|
|
0.0288441527062856, -0.00322823115861497, -0.00148509910480755, 0.000284265179004289,
|
|
-0.000175299737313045, -7.04175618676909e-05},
|
|
{3.70510151312723, -0.272200626024415, 1.5527519845099, 0.000589727630962265,
|
|
-0.00889682554869096, 0.0109857452472628, 3.05876215574877e-05, 2.09194236165814e-05,
|
|
-8.33769024439277e-05, 6.90991113575066e-05},
|
|
{0.820199776906695, -0.355683467192776, 1.17142130858009, -0.000160174871610729,
|
|
4.09723480153701e-05, 0.000209103751629257, 0.000390331989170637, 6.45642836249667e-05,
|
|
0.000318092703362044, 0.000107158633760141},
|
|
{5.52084497768914, -0.227775345312466, 0.845897282556327, 0.00157426476122436,
|
|
0.00657189797805861, 0.0103797665963117, 2.51479848048895e-05, -4.78371400399983e-05,
|
|
-5.20221896473413e-05, -0.000143840492906166},
|
|
{-33.4875689683454, 0.937557276329106, -1.02741065470967, -0.0140023273976314,
|
|
0.0401908729477037, -0.0512457211360142, 7.05537967426573e-05, -0.00027521752411122,
|
|
0.000407657552700476, -0.000458411000693613},
|
|
{0.931346887326171, -0.320804452025793, 1.28866325376154, 0.00912456151698805,
|
|
-0.00404367403569981, 0.00477543659981282, -9.43987917474817e-05, 4.66464249533497e-05,
|
|
-7.89362487264572e-05, -1.0951496495443e-05},
|
|
{-38.3689359928435, 3.8540516906071, 1.26391725545116, -0.108584643500539,
|
|
-0.0542697403292778, 0.0285360568428252, 0.000845084580479371, 0.00114184315411245,
|
|
-0.000169538153750085, -0.000336529204350355}};
|
|
float sus6coeffAlpha[9][10] = {
|
|
{10.4400948050067, 1.38202655603079, 0.975299591736672, 0.0172133914423707,
|
|
-0.0163482459492803, 0.035730152619911, 0.00021725657060767, -0.000181685375645396,
|
|
-0.000124096561459262, 0.00040790566176981},
|
|
{6.38281281805793, 1.81388255990089, 0.28679524291736, 0.0218036823758417,
|
|
0.010516766426651, 0.000446101708841615, 0.00020187044149361, 0.000114957457831415,
|
|
1.63114413539632e-05, -2.0187452317724e-05},
|
|
{-29.3049094555, -0.506844002611835, 1.64911970541112, -0.0336282997119334,
|
|
0.053185806861685, -0.028164943139695, -0.00021098074590512, 0.000643681643489995,
|
|
-0.000249094601806692, 0.000231466668650876},
|
|
{-4.76233790255328, 1.1780710601961, -0.194257188545164, 0.00471817228628384,
|
|
-0.00183773644319332, -0.00570261621182479, -7.99203367291902e-05, 7.75752247926601e-05,
|
|
-9.78534772816957e-06, -4.72083745991256e-05},
|
|
{0.692159025649028, 1.11895461388667, 0.341706834956496, 0.000237989648019541,
|
|
-0.000188322779563912, 0.000227310789253953, 0.000133001646828401, -0.000305810826248463,
|
|
0.00010150571088124, -0.000367705461590854},
|
|
{3.38094203317731, 1.24778838596815, 0.067807236112956, -0.00379395536123526,
|
|
-0.00339180589343601, -0.00188754615986649, -7.52406312245606e-05, 4.58398750278147e-05,
|
|
6.97244631313601e-05, 2.50519145070895e-05},
|
|
{-7.10546287716029, 0.459472977452686, -1.12251049944014, 0.0175406972371191,
|
|
-0.0310525406867782, -0.0531315970690727, -0.000121107664597462, 0.000544665437051928,
|
|
-1.78466217018177e-05, -0.00058976234038192},
|
|
{1.60633684055984, 1.1975095485662, 0.180159204664965, -0.00259157601062089,
|
|
-0.0038106317634397, 0.000956686555225968, 4.28416721502134e-06, 5.84532336259517e-06,
|
|
-2.73407888222758e-05, 5.45131881032866e-06},
|
|
{43.3732235586222, 0.528096786861784, -3.41255850703983, -0.0161629934278675,
|
|
0.0790998053536612, 0.0743822668655928, 0.000237176965460634, -0.000426691336904078,
|
|
-0.000889196131314391, -0.000509766491897672}};
|
|
float sus6coeffBeta[9][10] = {
|
|
{1.03872648284911, -0.213507239271552, 1.43193059498181, -0.000972717820830235,
|
|
-0.00661046096415371, 0.00974284211491888, 2.96098456891215e-05, -8.2933115634257e-05,
|
|
-5.52178824394723e-06, 5.73935295303589e-05},
|
|
{3.42242235823356, 0.0848392511283237, 1.24574390342586, 0.00356248195980133,
|
|
0.00100415659893053, -0.00460120247716139, 3.84891005422427e-05, 2.70236417852327e-05,
|
|
-7.58501977656551e-05, -8.79809730730992e-05},
|
|
{14.0092526123741, 1.03126714946215, 1.0611008563785, 0.04076462444523, 0.0114106419194518,
|
|
0.00746959159048058, 0.000388033225774727, -0.000124645014888926, -0.000296639947532341,
|
|
-0.00020861690864945},
|
|
{1.3562422681189, -0.241585615891602, 1.49170424068611, 0.000179184170448335,
|
|
-0.00712399257616284, 0.0121433526723498, 3.29770580642447e-05, 8.78960210966787e-06,
|
|
-6.00508568552101e-05, 0.000101583822589461},
|
|
{-0.718855428908583, -0.344067476078684, 1.12397093701762, 0.000236505431484729,
|
|
-0.000406441415248947, 0.00032834991502413, 0.000359422093285086, 8.18895560425272e-05,
|
|
0.000316835483508523, 0.000151442890664899},
|
|
{-0.268764016434841, -0.275272048639511, 1.26239753050527, -0.000511224336925231,
|
|
0.0095628568270856, -0.00397960092451418, 1.39587366293607e-05, 1.31409051361129e-05,
|
|
-9.83662017231755e-05, 1.87078667116619e-05},
|
|
{27.168106989145, -2.43346872338192, 1.91135512970771, 0.0553180826818016,
|
|
-0.0481878292619383, 0.0052773235604729, -0.000428011927975304, 0.000528018208222772,
|
|
-0.000285438191474895, -5.71327627917386e-05},
|
|
{-0.169494136517622, -0.350851545482921, 1.19922076033643, 0.0101120903675328,
|
|
-0.00151674465424115, 0.00548694086125656, -0.000108240000970513, 1.57202185024105e-05,
|
|
-9.77555098179959e-05, 2.09624089449761e-05},
|
|
{-32.3807957489507, 1.8271436443167, 2.51530814328123, -0.0532334586403461,
|
|
-0.0355980127727253, -0.0213373892796204, 0.00045506092539885, 0.000545065581027688,
|
|
0.000141998709314758, 0.000101051304611037}};
|
|
float sus7coeffAlpha[9][10] = {
|
|
{-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523,
|
|
-0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283,
|
|
-0.00119021101127241, -0.000460110780350978},
|
|
{1.60822506792345, 1.12993931449931, 0.300781032865641, -0.00405149856360946,
|
|
0.0116663280665617, -0.000746071920075153, -8.36092173253351e-05, 0.000126762041147563,
|
|
-1.57820750462019e-05, -2.13840141586661e-05},
|
|
{-151.403952985468, -5.77049222793992, 9.71132757422642, -0.113259116970462,
|
|
0.284142453949027, -0.198625061659164, -0.000836450164210354, 0.00174062771509636,
|
|
-0.00323746390757859, 0.00124721932086258},
|
|
{3.47391964888809, 1.28788318973591, 0.358380140281919, 0.0033863520864927,
|
|
0.00154601909793475, 0.0103457296050314, -9.56426572270873e-05, 5.48838958555808e-05,
|
|
2.97537427220847e-05, 0.000104735911514185},
|
|
{3.32650947866065, 1.16701012685798, 0.293514063672376, -0.00065850791542434,
|
|
-8.61746510464303e-05, -0.00212038990772211, 0.00010377123197, -0.000262818127593837,
|
|
0.000103360882478383, -0.000296739688930329},
|
|
{-0.440176043435378, 1.18923278867097, 0.519516382652818, -0.00138846714677511,
|
|
0.00266491699926247, -0.014254675949624, -4.20279929822439e-05, -5.49260281515447e-05,
|
|
-1.00328708454487e-05, 0.000138142092498215},
|
|
{9.54962966738358, 1.83809145920811, 1.82162819067959, -0.0116786627338505,
|
|
-0.00496037444422313, 0.0590883547819332, 7.48465315787857e-05, 0.000221693951602584,
|
|
7.96466345174136e-06, 0.000638822537725177},
|
|
{7.04862901290925, 0.876813777672465, 0.16368093989381, 0.00928717461441627,
|
|
-0.00276538956293246, 0.00117995419940653, -0.000141511492474493, -6.09796031786385e-06,
|
|
-2.62114930414747e-05, -2.88713611443788e-06},
|
|
{135.349147631811, -7.21933296299596, -6.02379024934871, 0.19557354282067,
|
|
0.207680233512614, 0.12880101618361, -0.00169832076532024, -0.00192216719797732,
|
|
-0.00188763612041332, -0.00103101801961442}};
|
|
float sus7coeffBeta[9][10] = {
|
|
{-12.7115487367622, -1.08890790360556, 0.0579616268854079, -0.0212303293514951,
|
|
-0.0395948453851818, -0.0275564242614342, -0.000228652851842222, -0.000148106159109458,
|
|
-0.000555136649469199, -0.000198260004582737},
|
|
{-0.988147625946871, -0.759018567468546, 1.20998292002818, -0.0241231836977845,
|
|
-0.000572110443300516, -0.00294835038249426, -0.00026533039022186, 6.82250069765274e-06,
|
|
7.21038415209318e-06, -6.54881435118179e-05},
|
|
{98.0979345921564, 4.27381413621355, -4.39956005193548, 0.0709109587666745,
|
|
-0.172774236139236, 0.107243391488741, 0.000421832640471043, -0.00140450884710288,
|
|
0.00158019019392239, -0.00078512547169536},
|
|
{4.10892685652543, -0.229301778557857, 1.33380992987117, -0.000250095848720304,
|
|
-0.00555205065514645, 0.00355052914398176, 1.62727119770752e-05, -1.26026527654764e-05,
|
|
-3.25505031810898e-05, 5.79970895921158e-06},
|
|
{3.09432502337258, -0.300556003790433, 1.17085811008124, 0.00128679594824324,
|
|
0.00148229981422985, 9.15267474159147e-05, 0.000300497843413856, 6.31378865575566e-05,
|
|
0.000258447032558814, 9.79142983264352e-05},
|
|
{8.92336134924575, -0.197306981784312, 0.659908505354084, 0.00175572239373996,
|
|
0.006801023678097, 0.0189775987436792, 9.2187857727721e-06, -4.8706332690626e-05,
|
|
-6.887009887486e-05, -0.000266455617735054},
|
|
{-52.0734887320227, 2.64822385560272, -1.72387600304694, -0.0383944891609251,
|
|
0.110873671161269, -0.0475247245070445, 0.000194652401328063, -0.000697307928990137,
|
|
0.00124021816001, -0.000194213899980878},
|
|
{2.08203985879155, -0.127503525368396, 1.17628056094647, 0.00283288065938444,
|
|
0.00394668214608305, 0.00314868636161131, -2.99504350569853e-05, -7.11070816314279e-05,
|
|
-6.30148122529749e-05, 2.28114298989664e-05},
|
|
{191.321181158032, -12.2449557187473, -7.21933741885107, 0.267954293388644,
|
|
0.331529493933124, 0.149867703984027, -0.00222279201444128, -0.00284724570619954,
|
|
-0.00298774060233964, -0.000988903783752156}};
|
|
float sus8coeffAlpha[9][10] = {
|
|
{5.46354311880959, 1.15370126035432, 0.568432485840475, -0.00105094692478431,
|
|
-0.000472899673842554, 0.015581320536192, 2.26460844314248e-05, -0.000254397947062058,
|
|
0.000198938007250408, 0.000102026690279006},
|
|
{8.8976133108173, 1.89502416095352, 0.268670471819199, 0.0217013413241972,
|
|
0.00973925295182384, -0.00116357269193765, 0.000185865842232419, 0.000103311614912702,
|
|
-2.46539447920969e-05, -2.06292928734686e-05},
|
|
{-45.4550803910752, 1.27220123406993, 5.21483855848504, 0.0315791081623634,
|
|
0.0725172355124129, -0.13947591535243, 0.000412577580637848, 0.000434545096994917,
|
|
-0.000840043932292312, 0.00126857487044307},
|
|
{1.81302768546433, 1.20563501267535, 0.344815267182167, 0.00546879453241056,
|
|
-0.00115382996865884, 0.010597876132341, -7.75885604486581e-05, 8.99568815949154e-05,
|
|
-2.98129544974679e-06, 0.000108913239345604},
|
|
{2.19111439539173, 1.06951675598148, 0.283707798607213, 0.00016478588207518,
|
|
0.000196086067268121, -0.00214980231173703, 0.000237820475654357, -0.000256402967908595,
|
|
0.000165966620658577, -0.000268394081675921},
|
|
{15.0858674915897, 1.27922724811168, -1.0803137812576, -0.00184009775302466,
|
|
-0.00458792284209219, 0.0359393555418547, -6.05121024079603e-05, -1.2288384024143e-05,
|
|
8.55484605384438e-05, -0.000379241348638065},
|
|
{-14.9594190080906, 1.79473182195746, -1.00830704063572, 0.000890685410857856,
|
|
0.0408932029176081, -0.0165460857151619, -0.000170544299916973, -0.000370901607010145,
|
|
0.000324089709129097, -9.33010240878062e-05},
|
|
{0.867614491733251, 1.38248194737027, 0.233408537422123, -0.00772942878114575,
|
|
-0.00783126068079782, -0.000413713955432221, 4.5775750146291e-05, 6.97323029940275e-05,
|
|
1.70664456940787e-05, 6.75517901233086e-06},
|
|
{2.34474364146174, -0.777275400251477, 2.09531381577911, 0.0170780716714389,
|
|
0.102855060371092, -0.1203441505925, 0.000187004964420911, -0.00141720441050986,
|
|
-0.000336251285258365, 0.00145175125888695}};
|
|
float sus8coeffBeta[9][10] = {
|
|
{28.3033101237397, 1.77504446792811, 1.70758838986317, 0.0307800697044683,
|
|
0.0598759344275936, -0.014461432284373, 0.000128415617799076, 0.000664419128546701,
|
|
0.000312923304130995, -0.000269026446641855},
|
|
{7.73040563051023, 0.0267291479555493, 1.16189582308493, 0.000611047892976521,
|
|
-0.00213680506915073, -0.00517435586596902, -3.60304406049766e-06, -1.74452976404459e-05,
|
|
-3.95396925228538e-05, -7.01948519410633e-05},
|
|
{-48.0766126130725, -3.77981206700298, 3.03482861087335, -0.0678496412519532,
|
|
0.115260678424016, -0.0109681510065038, -0.000438011443691466, 0.00097230136258486,
|
|
-0.000930875177732769, -0.000203144239955507},
|
|
{12.1881935626341, -0.234345089308583, 2.01134619426134, 0.000181529284001169,
|
|
-0.00642848065105061, 0.0243985799415726, 2.0224042581776e-05, 5.22503286757285e-06,
|
|
-4.75196303016323e-05, 0.000221160482364556},
|
|
{3.49559433498742, -0.294995112674766, 1.07892379698257, 0.000861664794052587,
|
|
0.00138978933062055, 0.000436385106465176, 0.000288095124755908, 0.000147259769247883,
|
|
0.000256686898599516, 0.000198982412957039},
|
|
{9.36663996178607, -0.171266136751803, 0.799869891484541, -0.000896305696610864,
|
|
0.00477919972789653, 0.0077876110326094, 9.16475263625076e-06, 3.02461250100473e-05,
|
|
-3.63917701783264e-05, -0.000101376940843402},
|
|
{9.93372683055145, 1.02056557854246, 3.01635426591734, -0.0477113881140277,
|
|
-0.0280426434406976, 0.0438813017696874, 0.000470431190169998, -7.55753674679743e-05,
|
|
-0.000516444906323815, 0.000253844418223843},
|
|
{4.12868774589741, -0.305711790187688, 1.15739216407191, 0.00942395115281056,
|
|
0.00264758462357433, 0.00227985850688479, -0.000107537164019682, -4.91396736189963e-05,
|
|
-5.3803493814502e-05, 6.80587059728718e-06},
|
|
{64.9193383444005, -1.57724255547465, -3.82166532626293, 0.0104712238987591,
|
|
0.0898786950946473, 0.128910456296131, -8.27123227422217e-05, -0.000143979624107479,
|
|
-0.00146684876653306, -0.00102226799570239}};
|
|
float sus9coeffAlpha[9][10] = {
|
|
{65.8975109449121, 2.19115342242175, 6.11069527811832, -0.0219884864133703,
|
|
0.119985456538482, 0.142746712551924, -0.000465882328687976, 0.000606525132125852,
|
|
0.00141667074621881, 0.00109715845894006},
|
|
{5.70337356029945, 1.86705636976809, 0.235584190291708, 0.0194937327615426,
|
|
0.00973291465247784, -0.00155675297510773, 0.000147099297988423, 0.000115708967219349,
|
|
-4.1462310493722e-05, -9.80097031103588e-06},
|
|
{138.221145997284, 6.07665575619595, -9.08085914250542, 0.0839801072927519,
|
|
-0.143071750033303, 0.237868300719915, 0.000626693630444932, -0.000579788170871402,
|
|
0.00181740650944343, -0.00207086879728281},
|
|
{-7.78295582666151, 1.37506685179192, -0.507596181420042, 0.00350118305456038,
|
|
0.00380814310115541, -0.0174012437563343, -0.000124801268056815, 2.96314830184492e-05,
|
|
6.3416992450033e-05, -0.000190177262510221},
|
|
{0.13102597129751, 1.24228303845143, 0.328808873447393, 2.6858679536165e-05,
|
|
0.000231428138164498, -0.000584089095259736, 5.5322167970451e-05, -0.000322205709821716,
|
|
7.71348293209208e-05, -0.000393885990364776},
|
|
{4.64571633968935, 1.2668223691397, -0.158952088650432, -0.0038344859267428,
|
|
0.0030051503726095, 0.00455578826025588, -9.42520993914957e-05, 5.81633314412289e-05,
|
|
-4.43545804544095e-05, -4.83524454851519e-05},
|
|
{99.2385930314563, -3.65569343617926, 5.54203926675588, 0.0975630395981933,
|
|
-0.15701634159692, 0.107834711298836, -0.000885326636237814, 0.000960753844480462,
|
|
-0.00179894024848343, 0.000583066757644971},
|
|
{2.82671549736619, 1.11214198870501, 0.214735318432744, 0.00284415167563662,
|
|
-0.00743289575690122, 0.000382705440762292, -7.43232442872501e-05, 6.96994098083348e-05,
|
|
-4.15108111710131e-06, 1.33520085213482e-05},
|
|
{36.9013743125415, -0.522392401546163, -1.52452843963663, 0.0261375433218879,
|
|
0.060573568610239, 0.0182582125221054, -0.000244373383911157, -0.000271385147292484,
|
|
-0.000723799969427732, 6.76324880239196e-05}};
|
|
float sus9coeffBeta[9][10] = {
|
|
{128.70886435409, 7.27355509732751, 7.18142203531244, 0.1536100459329, 0.199455846541636,
|
|
0.101824964939793, 0.00116666116789421, 0.00181595584079788, 0.00159271319494017,
|
|
0.000556768406475719},
|
|
{-7.07933839681024, -0.979062424441878, 1.21792546815617, -0.0295740143783226,
|
|
-0.00442780611714201, -0.00329612819203176, -0.000291373125216143, -7.47259350176359e-05,
|
|
-4.87265282482212e-05, -7.87490350444332e-05},
|
|
{41.1357193180502, 2.75138456414254, -0.0160889117718198, 0.0274001112562423,
|
|
-0.145644717742057, -0.0316076203283094, -0.000136443337244472, -0.00153945199081365,
|
|
0.000938960439977633, 0.000599987111822885},
|
|
{2.7980384746608, -0.234741037383589, 1.5342193016705, -0.000993791566721689,
|
|
-0.00787533639513478, 0.00927468655141365, 2.63308697896639e-05, -3.42816267184975e-05,
|
|
-8.48879419798771e-05, 3.84043821333798e-05},
|
|
{0.427687530667804, -0.346076633694936, 1.22968527483851, -4.95098138311122e-05,
|
|
0.000298245372198029, 0.000332756250024796, 0.00040375986210644, 5.20675972504572e-05,
|
|
0.000327042170278218, 5.93011568264671e-05},
|
|
{4.50337810133314, -0.279364254817202, 0.945812187846199, 0.000116182663432306,
|
|
0.0115646046622083, 0.00908289960302886, 1.90394667311541e-05, -4.4360223646434e-06,
|
|
-0.000131398914898614, -0.000145568992865512},
|
|
{-36.3377213654193, 2.21047221783626, 0.0609982245149821, -0.0670546774988572,
|
|
0.016827777144747, -0.0277834084058314, 0.000778301409125556, 0.000135846745194401,
|
|
0.00043261858797068, -0.00021172728254561},
|
|
{-0.737678205841529, -0.217352122193475, 1.23494846329297, 0.00748173441779792,
|
|
0.0019595873704705, 0.00567253723266176, -8.34768773292938e-05, -3.50608394184873e-05,
|
|
-0.000107500091550635, -5.1379722947632e-07},
|
|
{-36.6150844777671, 3.24952006904945, 1.7222457840185, -0.0846362445435584,
|
|
-0.0625549615377418, 0.019178365782485, 0.000664877496455304, 0.000942971403881222,
|
|
0.000190754698755098, -0.000372226659190439}};
|
|
float sus10coeffAlpha[9][10] = {
|
|
{14.4562393748324, 0.669162330324919, 2.13895255446541, -0.0161997097021299,
|
|
0.00185995785065838, 0.0621351118528379, -0.000278999272493087, 0.000238469666491965,
|
|
-0.000279407497782961, 0.000726904943739837},
|
|
{-4.45678285887022, 0.92869611919737, 0.186752102727282, -0.00706160758952316,
|
|
0.00532680276723634, -0.00119102617674229, -0.000105283880098953, 3.90673052334419e-05,
|
|
-3.13338277344246e-05, 5.32977236959767e-06},
|
|
{30.4255268053197, 3.00991076401191, -1.4855621363519, 0.033934286288413,
|
|
-0.0553588742704929, 0.0299275582316466, 0.000167915322354466, -0.00050925078118232,
|
|
0.000463662961330962, -0.000232919143454163},
|
|
{2.45076465343337, 1.30206564388838, 0.635121046212765, 0.00517109639797675,
|
|
0.00360579544364496, 0.0198490668911362, -9.31556816982662e-05, 6.7313653707875e-05,
|
|
6.4669137025142e-05, 0.000209727581169138},
|
|
{-0.784841314851562, 1.10058314980836, 0.314063830836532, 0.000583003703415889,
|
|
0.000312635453606579, -0.000183738114552387, 0.000214096205760617, -0.000286744686021244,
|
|
0.000159157597180407, -0.00032235099420715},
|
|
{7.19568036510586, 1.33307479701657, -0.465585141952456, -0.0031910726544199,
|
|
-0.00546273504371797, 0.0145494754402526, -7.9863949693769e-05, 4.83681329120104e-05,
|
|
8.85844309936609e-05, -0.000143217870916994},
|
|
{-12.8344546267449, 1.36023633150143, -0.728527724854506, 0.019982118403416,
|
|
0.0385056413989437, -0.00468598272326268, -0.000303957957649245, -6.37783846968216e-05,
|
|
0.000514049116643205, 0.000112015427600697},
|
|
{-2.58279031298065, 1.42167821629586, 0.208769467511292, -0.00640190372145885,
|
|
-0.0056405289717473, 0.000509611313918708, 2.23310562107823e-05, 3.23685469522147e-05,
|
|
-7.55982776243849e-06, 2.78417756661088e-06},
|
|
{-29.7178996143914, 2.636972251183, 1.97316329325243, -0.03983524158327,
|
|
-0.0193152048730234, -0.0600902798379509, 0.00031786916010672, 0.000162178988605602,
|
|
0.000224550786416246, 0.000614337977361927}};
|
|
float sus10coeffBeta[9][10] = {
|
|
{12.4771349792459, 1.44317849705414, 0.975637226331561, 0.0430284146301043,
|
|
0.0220810531548995, -0.0220926906772, 0.000310052324529521, 0.000658151808869523,
|
|
-0.000288026365111098, -0.000214619731807045},
|
|
{-0.113203260140131, -0.272424061092191, 1.27704377191184, -0.00791746619331075,
|
|
0.00278646694862191, -0.00398881099259934, -8.09569694307212e-05, 5.99617384829016e-05,
|
|
-5.4550919751855e-05, -8.6314530565085e-05},
|
|
{-48.585664295448, -2.04899787231903, 4.48757129623549, -0.0226180460431321,
|
|
0.090326735447661, -0.0722998813632622, -6.77623771415477e-05, 0.000562585419036509,
|
|
-0.000956171370931993, 0.000491554402311223},
|
|
{-1.20986884955482, -0.215604107185474, 1.22123198786617, 0.000256508527822089,
|
|
-0.00625056735692847, 0.00262961582224303, 2.27433984698861e-05, 1.60471509861372e-05,
|
|
-4.85061736834262e-05, -1.8387092782907e-06},
|
|
{-0.250205907903409, -0.315819331560782, 1.09018364376391, -0.000521787614293089,
|
|
-0.000500747760913489, 2.48184307342838e-05, 0.000313799238640988, 0.000136669146368744,
|
|
0.000278914324565192, 0.000218512838469476},
|
|
{-1.38512578184076, -0.240456589364121, 1.34170304231345, 0.00017499230372669,
|
|
0.0070862275911073, -0.00460640844814105, 1.27594111036696e-05, -4.73855624902052e-06,
|
|
-5.41141037812903e-05, 8.50767021818388e-06},
|
|
{58.9917559342216, -2.28705697628345, 5.35995190407842, 0.0214721399750612,
|
|
-0.112195722921667, 0.0890150265857542, -0.000100675657768708, 0.000493488022135339,
|
|
-0.00137672908303878, 0.000518683157694955},
|
|
{3.18905073365834, -0.633376549706314, 1.17190259811174, 0.0188817945597344,
|
|
0.00107470708915782, 0.00400880471375267, -0.000197312295539118, -2.46543035998379e-05,
|
|
-6.07871064300252e-05, 1.91822310311955e-05},
|
|
{-21.6881499304099, -0.563186103920008, 3.70747028664292, 0.021112883967427,
|
|
-0.00650020689049325, -0.0778533644688476, -0.000131921888670268, -0.000402754836445439,
|
|
0.000551249824375055, 0.00062236627391337}};
|
|
float sus11coeffAlpha[9][10] = {
|
|
{-5.23569698615548, -1.45500092391928, 2.7643243644756, -0.0762912296128707,
|
|
-0.0201645929971608, 0.0997226845779083, -0.000741669441569556, -0.000485368004931528,
|
|
0.000166230212359982, 0.00103455037278067},
|
|
{-7.7405077383712, 0.892040861541276, 0.39014957203484, -0.00952030929935314,
|
|
0.0185577462685363, 0.000500600568760257, -0.000151227821554572, 0.000245334737283439,
|
|
1.89380065823205e-05, 1.83481122973969e-07},
|
|
{-27.462143709831, -1.68192474287299, 0.689411302961069, -0.0146021086710062,
|
|
0.227153492753605, 0.0815806579791421, 2.92919049429149e-05, 0.00153760357651792,
|
|
-0.00247865821192621, -0.00166333309739387},
|
|
{-6.74664748624368, 1.43279156053015, 0.0212787292914553, 0.00764792230365732,
|
|
0.00796410301290615, 0.0014384998868733, -8.95239151813685e-05, 9.55245417090909e-05,
|
|
0.000127251739461239, 3.26943341606363e-05},
|
|
{-2.20391533717674, 1.32902400478083, 0.38633027011889, 0.00104660852197061,
|
|
0.00105228824412283, -0.00242067551428214, -6.98346290136652e-05, -0.000369075232184835,
|
|
-1.59510520000704e-05, -0.000448565104826966},
|
|
{-5.29476778147188, 1.4549556336236, 0.561334186252557, -0.00260896342687109,
|
|
-0.00855934179001141, -0.0182515354646452, -8.79385828606048e-05, 5.98357681659175e-05,
|
|
0.000146570207542509, 0.000201596912834554},
|
|
{-45.7906613832612, 3.77630104475902, -1.67494598155515, -0.0545433897761635,
|
|
0.047897938410221, -0.0355687158405231, 0.000374738707508583, -0.000448453494537518,
|
|
0.000377784972619365, -0.000276573228333836},
|
|
{-9.11681182090372, 2.06933872940742, 0.26131496122122, -0.0259534033367855,
|
|
-0.00777266937872862, -0.00262135395843891, 0.000223790782655445, 6.40488537928934e-05,
|
|
7.75581514100296e-05, -9.25934285039627e-06},
|
|
{183.243883340634, -8.02281039502717, -10.0756951652703, 0.168750521462303,
|
|
0.314006821405967, 0.200264755034566, -0.0011895153717447, -0.00253812476819896,
|
|
-0.00291324393641628, -0.00140062522117514}};
|
|
float sus11coeffBeta[9][10] = {
|
|
{34.4499366074013, -0.438583698052091, 4.72111001451028, -0.041810050989433,
|
|
0.0562461093661426, 0.0856849892524893, -0.000477813051406167, -3.16404257494464e-05,
|
|
0.00102633196865105, 0.000552974013759876},
|
|
{7.6366298088699, 0.150314752958302, 1.31364679484924, 0.00557696667395871,
|
|
0.00163731860604376, -0.00454759608980269, 5.83979683674572e-05, 4.45944881220665e-05,
|
|
-4.27874746147066e-05, -8.77418673597557e-05},
|
|
{130.156215011893, 1.85759000444524, -10.986892391833, -0.00686275191260681,
|
|
-0.188837138116058, 0.346177462085361, -0.000183276946352264, -0.000702183496893294,
|
|
0.00293145272693857, -0.00318194442670715},
|
|
{-1.67854820161036, -0.358899332859806, 0.956690839640595, -4.93862910503803e-05,
|
|
-0.0136134783014874, -0.00848731301504507, 3.75950499927045e-05, 1.35374694383289e-06,
|
|
-0.000156596507890443, -0.000123254220377897},
|
|
{3.67569209537767, -0.387260959713287, 1.31343215605952, -0.00206444615206506,
|
|
0.00145334813110285, -0.00151259497696238, 0.000449492568365603, 6.95883968949488e-07,
|
|
0.000368585523744765, -6.3420715525635e-05},
|
|
{14.3296323024886, -0.182979476956897, 0.306817119309235, -0.00022212115978293,
|
|
0.00463485302909649, 0.0277574953550035, 1.1422454625565e-05, 1.06053257479502e-05,
|
|
-2.05720000720608e-05, -0.000338584671430337},
|
|
{-18.7534921817754, 1.14272710923224, 0.460498062012866, -0.00995826989278202,
|
|
0.0658502318647112, 0.00616942819937029, -7.70857153768402e-05, -0.000641755741925561,
|
|
0.00047849204592989, 0.000158509018296766},
|
|
{1.26543621388607, -0.176674379740481, 1.38814920935488, 0.00545485262295305,
|
|
-0.00499775616702264, 0.0038057039142173, -6.59604252054511e-05, 6.40211116049053e-05,
|
|
-6.74778593434431e-05, -2.81973589469059e-05},
|
|
{116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136,
|
|
0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
|
|
-0.000889232196185857, -0.00168429567131815}};
|
|
float susBrightnessThreshold = 0.7;
|
|
float susVectorFilterWeight = .95;
|
|
float susRateFilterWeight = .99;
|
|
} susHandlingParameters;
|
|
|
|
struct GyrHandlingParameters {
|
|
double gyr0orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}};
|
|
double gyr1orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
|
|
double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}};
|
|
double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
|
|
|
|
double gyr0bias[3] = {0.0, 0.4, -0.1};
|
|
double gyr1bias[3] = {0.0956745283018868, 2.0854575471698116, 1.2505990566037737};
|
|
double gyr2bias[3] = {0.1, 0.7, -0.2};
|
|
double gyr3bias[3] = {-0.10721698113207549, -0.6111650943396226, 0.1716462264150944};
|
|
|
|
/* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
|
|
* assumed to be equal for the same class of sensors */
|
|
double gyr02variance[3] = {pow(4.6e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
|
|
pow(4.6e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
|
|
pow(6.1e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
|
|
double gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
|
|
uint8_t preferAdis = false;
|
|
float gyrFilterWeight = 0.6;
|
|
} gyrHandlingParameters;
|
|
|
|
struct RwHandlingParameters {
|
|
double inertiaWheel = 0.000028198;
|
|
double maxTrq = 0.0032; // 3.2 [mNm]
|
|
int32_t maxRwSpeed = 65000; // 0.1 RPM
|
|
int32_t stictionSpeed = 1000; // 0.1 RPM
|
|
int32_t stictionReleaseSpeed = 1000; // 0.1 RPM
|
|
double stictionTorque = 0.0006;
|
|
|
|
uint16_t rampTime = 10;
|
|
|
|
uint32_t multipleRwInvalidTimeout = 25;
|
|
} rwHandlingParameters;
|
|
|
|
struct RwMatrices {
|
|
double alignmentMatrix[3][4] = {{-0.9205, 0.0000, 0.9205, 0.0000},
|
|
{0.0000, 0.9205, 0.0000, -0.9205},
|
|
{-0.3907, -0.3907, -0.3907, -0.3907}};
|
|
double pseudoInverse[4][3] = {
|
|
{-0.5432, 0, -0.6399}, {0, 0.5432, -0.6399}, {0.5432, 0, -0.6399}, {0, -0.5432, -0.6399}};
|
|
double pseudoInverseWithoutRW1[4][3] = {
|
|
{0, 0, 0}, {-0.5432, 0.5432, -1.2798}, {1.0864, 0, 0}, {-0.5432, -0.5432, -1.2798}};
|
|
double pseudoInverseWithoutRW2[4][3] = {
|
|
{-0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, -1.0864, 0}};
|
|
double pseudoInverseWithoutRW3[4][3] = {
|
|
{-1.0864, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, -0.5432, -1.2798}};
|
|
double pseudoInverseWithoutRW4[4][3] = {
|
|
{-0.5432, -0.5432, -1.2798}, {0, 1.0864, 0}, {0.5432, -0.5432, -1.2798}, {0, 0, 0}};
|
|
double nullspaceVector[4] = {-1, 1, -1, 1};
|
|
} rwMatrices;
|
|
|
|
struct SafeModeControllerParameters {
|
|
double k_orthoMekf = 4.4e-3;
|
|
double k_alignMekf = 4.0e-5;
|
|
double k_parallelMekf = 3.75e-4;
|
|
|
|
double k_orthoGyr = 4.4e-3;
|
|
double k_alignGyr = 4.0e-5;
|
|
double k_parallelGyr = 3.75e-4;
|
|
|
|
double k_orthoSusMgm = 4.4e-3;
|
|
double k_alignSusMgm = 4.0e-5;
|
|
double k_parallelSusMgm = 3.75e-4;
|
|
|
|
double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)};
|
|
double sunTargetDir[3] = {0, 0, 1};
|
|
|
|
uint8_t useMekf = false;
|
|
uint8_t useGyr = false;
|
|
uint8_t dampingDuringEclipse = true;
|
|
|
|
float sineLimitSunRotRate = 0.24;
|
|
} safeModeControllerParameters;
|
|
|
|
struct PointingLawParameters {
|
|
double zeta = 0.3;
|
|
double om = 0.3;
|
|
double omMax = 1 * DEG2RAD;
|
|
double qiMin = 0.1;
|
|
|
|
double gainNullspace = 0.01;
|
|
double nullspaceSpeed = 32500; // 0.1 RPM
|
|
|
|
double desatMomentumRef[3] = {0, 0, 0};
|
|
double deSatGainFactor = 1000;
|
|
uint8_t desatOn = true;
|
|
uint8_t useMekf = true;
|
|
} pointingLawParameters;
|
|
|
|
struct IdleModeControllerParameters : PointingLawParameters {
|
|
} idleModeControllerParameters;
|
|
|
|
struct TargetModeControllerParameters : PointingLawParameters {
|
|
double refDirection[3] = {-1, 0, 0}; // Antenna
|
|
double refRotRate[3] = {0, 0, 0};
|
|
double quatRef[4] = {0, 0, 0, 1};
|
|
uint8_t timeElapsedMax = 10; // rot rate calculations
|
|
|
|
// Default is Stuttgart GS
|
|
double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude
|
|
double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude
|
|
double altitudeTgt = 500; // [m]
|
|
|
|
// For one-axis control:
|
|
uint8_t avoidBlindStr = true;
|
|
double blindAvoidStart = 1.5;
|
|
double blindAvoidStop = 2.5;
|
|
double blindRotRate = 1. * DEG2RAD;
|
|
} targetModeControllerParameters;
|
|
|
|
struct GsTargetModeControllerParameters : PointingLawParameters {
|
|
double refDirection[3] = {-1, 0, 0}; // Antenna
|
|
uint8_t timeElapsedMax = 10; // rot rate calculations
|
|
|
|
// Default is Stuttgart GS
|
|
double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude
|
|
double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude
|
|
double altitudeTgt = 500; // [m]
|
|
double rotRateLimit = .75 * DEG2RAD;
|
|
} gsTargetModeControllerParameters;
|
|
|
|
struct NadirModeControllerParameters : PointingLawParameters {
|
|
double refDirection[3] = {-1, 0, 0}; // Antenna
|
|
double quatRef[4] = {0, 0, 0, 1};
|
|
double refRotRate[3] = {0, 0, 0};
|
|
uint8_t timeElapsedMax = 10; // rot rate calculations
|
|
} nadirModeControllerParameters;
|
|
|
|
struct InertialModeControllerParameters : PointingLawParameters {
|
|
double tgtQuat[4] = {0, 0, 0, 1};
|
|
double refRotRate[3] = {0, 0, 0};
|
|
double quatRef[4] = {0, 0, 0, 1};
|
|
} inertialModeControllerParameters;
|
|
|
|
struct StrParameters {
|
|
double exclusionAngle = 20. * DEG2RAD;
|
|
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // body rf
|
|
} strParameters;
|
|
|
|
struct GpsParameters {
|
|
double timeDiffVelocityMax = 30; // [s]
|
|
double minimumFdirAltitude = 475 * 1e3; // [m]
|
|
double maximumFdirAltitude = 575 * 1e3; // [m]
|
|
double fdirAltitude = 525 * 1e3; // [m]
|
|
uint8_t useSpg4 = true;
|
|
} gpsParameters;
|
|
|
|
struct SunModelParameters {
|
|
float domega = 36000.771;
|
|
float omega_0 = 280.46 * DEG2RAD; // RAAN plus argument of
|
|
// perigee
|
|
float m_0 = 357.5277; // coefficients for mean anomaly
|
|
float dm = 35999.049; // coefficients for mean anomaly
|
|
float e = 23.4392911 * DEG2RAD; // angle of earth's rotation axis
|
|
float e1 = 0.74508 * DEG2RAD;
|
|
|
|
float p1 = 6892. / 3600. * DEG2RAD; // some parameter
|
|
float p2 = 72. / 3600. * DEG2RAD; // some parameter
|
|
} sunModelParameters;
|
|
|
|
struct KalmanFilterParameters {
|
|
double sensorNoiseStr = 0.1 * DEG2RAD;
|
|
double sensorNoiseSus = 8. * DEG2RAD;
|
|
double sensorNoiseMgm = 4. * DEG2RAD;
|
|
double sensorNoiseGyr = 0.1 * DEG2RAD;
|
|
|
|
double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD; // Angular Random Walk
|
|
double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD; // Bias Stability
|
|
|
|
uint8_t allowStr = false;
|
|
} kalmanFilterParameters;
|
|
|
|
struct MagnetorquerParameter {
|
|
double mtq0orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}};
|
|
double mtq1orientationMatrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
|
|
double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}};
|
|
double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
|
|
double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}};
|
|
double dipoleMax = 0.2; // [Am^2]
|
|
|
|
uint16_t torqueDuration = 300; // [ms]
|
|
} magnetorquerParameter;
|
|
|
|
struct DetumbleParameter {
|
|
uint8_t detumblecounter = 75; // 30 s
|
|
double omegaDetumbleStart = 2 * DEG2RAD;
|
|
double omegaDetumbleEnd = 1 * DEG2RAD;
|
|
double gainBdot = pow(10.0, -3.3);
|
|
double gainFull = pow(10.0, -2.3);
|
|
uint8_t useFullDetumbleLaw = false;
|
|
} detumbleParameter;
|
|
};
|
|
|
|
#endif /* ACSPARAMETERS_H_ */
|