Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown

Dependencies:   mbed MODSERIAL FATFileSystem

ConfigFileIO/ConfigFileIO.cpp@73:f6f378311c8d, 2018-07-30 (annotated)

Committer:
tnhnrl
Date:
Mon Jul 30 16:48:48 2018 +0000
Revision:
73:f6f378311c8d
Parent:
57:ec69651c8c21
Child:
74:d281aaef9766
work in progress 7/30 12:48 pm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tnhnrl 57:ec69651c8c21 1 #include "ConfigFileIO.hpp"
tnhnrl 57:ec69651c8c21 2 #include "StaticDefs.hpp"
tnhnrl 57:ec69651c8c21 3
tnhnrl 57:ec69651c8c21 4 //configuration file io, both to read, set, and save
tnhnrl 57:ec69651c8c21 5
tnhnrl 57:ec69651c8c21 6 ConfigFileIO::ConfigFileIO() {
tnhnrl 73:f6f378311c8d 7 }
tnhnrl 57:ec69651c8c21 8
tnhnrl 73:f6f378311c8d 9 void ConfigFileIO::saveBattData(float batt_p_gain, float batt_i_gain, float batt_d_gain, float batt_zeroOffset, float batt_filter_freq, float batt_deadband) {
tnhnrl 57:ec69651c8c21 10 ConfigFile write_Batt_txt; //write to the config file
tnhnrl 57:ec69651c8c21 11
tnhnrl 57:ec69651c8c21 12 char PGain_buffer[128];
tnhnrl 57:ec69651c8c21 13 sprintf(PGain_buffer,"# configuration file for battery mover parameters\n\n#Gains\nPGain");
tnhnrl 57:ec69651c8c21 14
tnhnrl 57:ec69651c8c21 15 char string_pgain[128];
tnhnrl 73:f6f378311c8d 16 sprintf(string_pgain, "%f", batt_p_gain);
tnhnrl 57:ec69651c8c21 17 write_Batt_txt.setValue(PGain_buffer, string_pgain);
tnhnrl 57:ec69651c8c21 18
tnhnrl 57:ec69651c8c21 19 char string_igain[128];
tnhnrl 73:f6f378311c8d 20 sprintf(string_igain, "%f", batt_i_gain);
tnhnrl 57:ec69651c8c21 21 write_Batt_txt.setValue("IGain", string_igain);
tnhnrl 57:ec69651c8c21 22
tnhnrl 57:ec69651c8c21 23 char string_dgain[128];
tnhnrl 73:f6f378311c8d 24 sprintf(string_dgain, "%f", batt_d_gain);
tnhnrl 57:ec69651c8c21 25 write_Batt_txt.setValue("DGain", string_dgain);
tnhnrl 73:f6f378311c8d 26
tnhnrl 73:f6f378311c8d 27 char string_zeroCounts[128];
tnhnrl 73:f6f378311c8d 28 sprintf(string_zeroCounts, "%f", batt_zeroOffset);
tnhnrl 73:f6f378311c8d 29 write_Batt_txt.setValue("\n#string pot parameters\nzeroCounts", string_zeroCounts);
tnhnrl 73:f6f378311c8d 30
tnhnrl 73:f6f378311c8d 31 //hard-coded values
tnhnrl 57:ec69651c8c21 32 write_Batt_txt.setValue("PistonTravelLimit", "73.0");
tnhnrl 57:ec69651c8c21 33 write_Batt_txt.setValue("slope", "0.12176");
tnhnrl 73:f6f378311c8d 34
tnhnrl 73:f6f378311c8d 35 char string_filter_freq[128];
tnhnrl 73:f6f378311c8d 36 sprintf(string_filter_freq, "%f", batt_filter_freq);
tnhnrl 73:f6f378311c8d 37 write_Batt_txt.setValue("filterWn", string_filter_freq);
tnhnrl 73:f6f378311c8d 38
tnhnrl 73:f6f378311c8d 39 char string_deadband[128];
tnhnrl 73:f6f378311c8d 40 sprintf(string_deadband, "%f", batt_deadband);
tnhnrl 73:f6f378311c8d 41 write_Batt_txt.setValue("deadband", string_deadband);
tnhnrl 57:ec69651c8c21 42
tnhnrl 57:ec69651c8c21 43 //SAVE THE DATA!
tnhnrl 57:ec69651c8c21 44 pc().printf("Saving BATTERY MOVER PID data!");
tnhnrl 57:ec69651c8c21 45
tnhnrl 57:ec69651c8c21 46 if (!write_Batt_txt.write("/local/batt.txt")) {
tnhnrl 57:ec69651c8c21 47 pc().printf("\n\rERROR: (SAVE)Failure to write batt.txt file.");
tnhnrl 57:ec69651c8c21 48 }
tnhnrl 57:ec69651c8c21 49 else {
tnhnrl 57:ec69651c8c21 50 pc().printf("\n\rFile batt.txt successful written.\n\r");
tnhnrl 57:ec69651c8c21 51 }
tnhnrl 57:ec69651c8c21 52 }
tnhnrl 57:ec69651c8c21 53
tnhnrl 73:f6f378311c8d 54 void ConfigFileIO::savePitchData(float pitch_p_gain, float pitch_i_gain, float pitch_d_gain, float pitch_zeroOffset, float pitch_filter_freq, float pitch_deadband) {
tnhnrl 57:ec69651c8c21 55 ConfigFile write_pitch_txt; //write to the config file
tnhnrl 57:ec69651c8c21 56
tnhnrl 57:ec69651c8c21 57 char PGain_buffer[128];
tnhnrl 57:ec69651c8c21 58 sprintf(PGain_buffer,"# pitch outer loop parameters\n\n#Gains\nPGain");
tnhnrl 57:ec69651c8c21 59
tnhnrl 57:ec69651c8c21 60 char string_pgain[128];
tnhnrl 73:f6f378311c8d 61 sprintf(string_pgain, "%f", pitch_p_gain);
tnhnrl 57:ec69651c8c21 62 write_pitch_txt.setValue(PGain_buffer, string_pgain);
tnhnrl 57:ec69651c8c21 63
tnhnrl 57:ec69651c8c21 64 char string_igain[128];
tnhnrl 73:f6f378311c8d 65 sprintf(string_igain, "%f", pitch_i_gain);
tnhnrl 57:ec69651c8c21 66 write_pitch_txt.setValue("IGain", string_igain);
tnhnrl 57:ec69651c8c21 67
tnhnrl 57:ec69651c8c21 68 char string_dgain[128];
tnhnrl 73:f6f378311c8d 69 sprintf(string_dgain, "%f", pitch_d_gain);
tnhnrl 57:ec69651c8c21 70 write_pitch_txt.setValue("DGain", string_dgain);
tnhnrl 73:f6f378311c8d 71
tnhnrl 73:f6f378311c8d 72 char string_filter_freq[128];
tnhnrl 73:f6f378311c8d 73 sprintf(string_filter_freq, "%f", pitch_filter_freq);
tnhnrl 73:f6f378311c8d 74 write_pitch_txt.setValue("\n#Pitch sensor filter parameters\nfilterWn", string_filter_freq);
tnhnrl 73:f6f378311c8d 75
tnhnrl 73:f6f378311c8d 76 char string_deadband[128];
tnhnrl 73:f6f378311c8d 77 sprintf(string_deadband, "%f", pitch_deadband);
tnhnrl 73:f6f378311c8d 78 write_pitch_txt.setValue("deadband", string_deadband);
tnhnrl 57:ec69651c8c21 79
tnhnrl 57:ec69651c8c21 80 char string_zeroOffset[128];
tnhnrl 73:f6f378311c8d 81 sprintf(string_zeroOffset, "%f", pitch_zeroOffset);
tnhnrl 57:ec69651c8c21 82 //bce setting was 41 mm during LASR experiments
tnhnrl 57:ec69651c8c21 83 write_pitch_txt.setValue("\n#Offset for neutral (default: 41)\nzeroOffset", string_zeroOffset);
tnhnrl 57:ec69651c8c21 84
tnhnrl 57:ec69651c8c21 85 //SAVE THE DATA!
tnhnrl 57:ec69651c8c21 86 pc().printf("Saving Buoyancy Engine Neutral Buoyancy Positions!");
tnhnrl 57:ec69651c8c21 87
tnhnrl 57:ec69651c8c21 88 if (!write_pitch_txt.write("/local/pitch.txt")) {
tnhnrl 57:ec69651c8c21 89 pc().printf("\n\rERROR: (SAVE)Failure to write depth.txt file.");
tnhnrl 57:ec69651c8c21 90 }
tnhnrl 57:ec69651c8c21 91 else {
tnhnrl 57:ec69651c8c21 92 pc().printf("\n\rFile pitch.txt successful written.\n\r");
tnhnrl 57:ec69651c8c21 93 }
tnhnrl 57:ec69651c8c21 94 }
tnhnrl 57:ec69651c8c21 95
tnhnrl 73:f6f378311c8d 96 void ConfigFileIO::saveBCEData(float bce_p_gain, float bce_i_gain, float bce_d_gain, float bce_zeroOffset, float bce_filter_freq, float bce_deadband) {
tnhnrl 57:ec69651c8c21 97 ConfigFile write_BCE_txt; //write to the config file
tnhnrl 57:ec69651c8c21 98
tnhnrl 57:ec69651c8c21 99 char PGain_buffer[128];
tnhnrl 57:ec69651c8c21 100 sprintf(PGain_buffer,"# configuration file for BCE parameters\n\n#Gains\nPGain");
tnhnrl 57:ec69651c8c21 101
tnhnrl 57:ec69651c8c21 102 char string_pgain[128];
tnhnrl 73:f6f378311c8d 103 sprintf(string_pgain, "%f", bce_p_gain);
tnhnrl 57:ec69651c8c21 104 write_BCE_txt.setValue(PGain_buffer, string_pgain);
tnhnrl 57:ec69651c8c21 105
tnhnrl 57:ec69651c8c21 106 char string_igain[128];
tnhnrl 73:f6f378311c8d 107 sprintf(string_igain, "%f", bce_i_gain);
tnhnrl 57:ec69651c8c21 108 write_BCE_txt.setValue("IGain", string_igain);
tnhnrl 57:ec69651c8c21 109
tnhnrl 57:ec69651c8c21 110 char string_dgain[128];
tnhnrl 73:f6f378311c8d 111 sprintf(string_dgain, "%f", bce_d_gain);
tnhnrl 57:ec69651c8c21 112 write_BCE_txt.setValue("DGain", string_dgain);
tnhnrl 73:f6f378311c8d 113
tnhnrl 73:f6f378311c8d 114 char string_zeroCounts[128];
tnhnrl 73:f6f378311c8d 115 sprintf(string_zeroCounts, "%f", bce_zeroOffset);
tnhnrl 73:f6f378311c8d 116 write_BCE_txt.setValue("\n#string pot parameters\nzeroCounts", string_zeroCounts);
tnhnrl 73:f6f378311c8d 117
tnhnrl 73:f6f378311c8d 118 //hard-coded values
tnhnrl 57:ec69651c8c21 119 write_BCE_txt.setValue("PistonTravelLimit", "320.0");
tnhnrl 57:ec69651c8c21 120 write_BCE_txt.setValue("slope", "0.12176");
tnhnrl 73:f6f378311c8d 121
tnhnrl 73:f6f378311c8d 122 char string_filter_freq[128];
tnhnrl 73:f6f378311c8d 123 sprintf(string_filter_freq, "%f", bce_filter_freq);
tnhnrl 73:f6f378311c8d 124 write_BCE_txt.setValue("filterWn", string_filter_freq);
tnhnrl 73:f6f378311c8d 125
tnhnrl 73:f6f378311c8d 126 char string_deadband[128];
tnhnrl 73:f6f378311c8d 127 sprintf(string_deadband, "%f", bce_deadband);
tnhnrl 73:f6f378311c8d 128 write_BCE_txt.setValue("deadband", string_deadband);
tnhnrl 57:ec69651c8c21 129
tnhnrl 57:ec69651c8c21 130 //SAVE THE DATA!
tnhnrl 57:ec69651c8c21 131 pc().printf("Saving BCE PID data!");
tnhnrl 57:ec69651c8c21 132
tnhnrl 57:ec69651c8c21 133 if (!write_BCE_txt.write("/local/bce.txt")) {
tnhnrl 57:ec69651c8c21 134 pc().printf("\n\rERROR: (SAVE)Failure to write bce.txt file.");
tnhnrl 57:ec69651c8c21 135 }
tnhnrl 57:ec69651c8c21 136 else {
tnhnrl 57:ec69651c8c21 137 pc().printf("\n\rFile bce.txt successful written.\n\r");
tnhnrl 57:ec69651c8c21 138 }
tnhnrl 57:ec69651c8c21 139 }
tnhnrl 57:ec69651c8c21 140
tnhnrl 73:f6f378311c8d 141 void ConfigFileIO::saveDepthData(float depth_p_gain, float depth_i_gain, float depth_d_gain, float depth_zeroOffset, float depth_filter_freq, float depth_deadband) {
tnhnrl 57:ec69651c8c21 142 ConfigFile write_depth_txt; //write to the config file
tnhnrl 57:ec69651c8c21 143
tnhnrl 57:ec69651c8c21 144 char PGain_buffer[128];
tnhnrl 57:ec69651c8c21 145 sprintf(PGain_buffer,"# depth outer loop parameters\n\n#Gains\nPGain");
tnhnrl 57:ec69651c8c21 146
tnhnrl 57:ec69651c8c21 147 char string_pgain[128];
tnhnrl 73:f6f378311c8d 148 sprintf(string_pgain, "%f", depth_p_gain);
tnhnrl 57:ec69651c8c21 149 write_depth_txt.setValue(PGain_buffer, string_pgain);
tnhnrl 57:ec69651c8c21 150
tnhnrl 57:ec69651c8c21 151 char string_igain[128];
tnhnrl 73:f6f378311c8d 152 sprintf(string_igain, "%f", depth_i_gain);
tnhnrl 57:ec69651c8c21 153 write_depth_txt.setValue("IGain", string_igain);
tnhnrl 57:ec69651c8c21 154
tnhnrl 57:ec69651c8c21 155 char string_dgain[128];
tnhnrl 73:f6f378311c8d 156 sprintf(string_dgain, "%f", depth_d_gain);
tnhnrl 57:ec69651c8c21 157 write_depth_txt.setValue("DGain", string_dgain);
tnhnrl 73:f6f378311c8d 158
tnhnrl 73:f6f378311c8d 159 char string_filter_freq[128];
tnhnrl 73:f6f378311c8d 160 sprintf(string_filter_freq, "%f", depth_filter_freq);
tnhnrl 73:f6f378311c8d 161 write_depth_txt.setValue("\n#Depth sensor filter parameters\nfilterWn", string_filter_freq);
tnhnrl 73:f6f378311c8d 162
tnhnrl 73:f6f378311c8d 163 char string_deadband[128];
tnhnrl 73:f6f378311c8d 164 sprintf(string_deadband, "%f", depth_deadband);
tnhnrl 73:f6f378311c8d 165 write_depth_txt.setValue("deadband", string_deadband);
tnhnrl 57:ec69651c8c21 166
tnhnrl 57:ec69651c8c21 167 char string_zeroOffset[128];
tnhnrl 73:f6f378311c8d 168 sprintf(string_zeroOffset, "%f", depth_zeroOffset);
tnhnrl 57:ec69651c8c21 169 //bce setting was 240 mm during LASR experiments
tnhnrl 57:ec69651c8c21 170 write_depth_txt.setValue("\n#Offset for neutral (default: 240)\nzeroOffset", string_zeroOffset);
tnhnrl 57:ec69651c8c21 171
tnhnrl 57:ec69651c8c21 172 //SAVE THE DATA!
tnhnrl 57:ec69651c8c21 173 pc().printf("Saving Buoyancy Engine Neutral Buoyancy Positions!");
tnhnrl 57:ec69651c8c21 174
tnhnrl 57:ec69651c8c21 175 if (!write_depth_txt.write("/local/depth.txt")) {
tnhnrl 57:ec69651c8c21 176 pc().printf("\n\rERROR: (SAVE)Failure to write depth.txt file.");
tnhnrl 57:ec69651c8c21 177 }
tnhnrl 57:ec69651c8c21 178 else {
tnhnrl 57:ec69651c8c21 179 pc().printf("\n\rFile depth.txt successful written.\n\r");
tnhnrl 57:ec69651c8c21 180 }
tnhnrl 57:ec69651c8c21 181 }
tnhnrl 57:ec69651c8c21 182
tnhnrl 57:ec69651c8c21 183 //write rudder (servo driver) file rudder.txt
tnhnrl 57:ec69651c8c21 184 void ConfigFileIO::saveRudderData(float setMinDeg, float setMaxDeg, float setCenterPWM, float setMinPWM, float setMaxPWM) {
tnhnrl 57:ec69651c8c21 185 ConfigFile rudder_txt;
tnhnrl 57:ec69651c8c21 186
tnhnrl 57:ec69651c8c21 187 char header[128];
tnhnrl 57:ec69651c8c21 188 sprintf(header,"# rudder (servo) inner loop (heading outer loop uses this)");
tnhnrl 57:ec69651c8c21 189
tnhnrl 57:ec69651c8c21 190 char string_min_deg[128];
tnhnrl 57:ec69651c8c21 191 sprintf(string_min_deg, "%f", setMinDeg);
tnhnrl 57:ec69651c8c21 192 rudder_txt.setValue("setMinDeg", string_min_deg);
tnhnrl 57:ec69651c8c21 193
tnhnrl 57:ec69651c8c21 194 char string_max_deg[128];
tnhnrl 57:ec69651c8c21 195 sprintf(string_max_deg, "%f", setMaxDeg);
tnhnrl 57:ec69651c8c21 196 rudder_txt.setValue("setMaxDeg", string_max_deg);
tnhnrl 57:ec69651c8c21 197
tnhnrl 57:ec69651c8c21 198 char string_ctr_pwm[128];
tnhnrl 57:ec69651c8c21 199 sprintf(string_ctr_pwm, "%f", setCenterPWM);
tnhnrl 57:ec69651c8c21 200 rudder_txt.setValue("setCenterPWM", string_ctr_pwm);
tnhnrl 57:ec69651c8c21 201
tnhnrl 57:ec69651c8c21 202 char string_min_pwm[128];
tnhnrl 57:ec69651c8c21 203 sprintf(string_min_pwm, "%f", setMinPWM);
tnhnrl 57:ec69651c8c21 204 rudder_txt.setValue("setMinPWM", string_min_pwm);
tnhnrl 57:ec69651c8c21 205
tnhnrl 57:ec69651c8c21 206 char string_max_pwm[128];
tnhnrl 57:ec69651c8c21 207 sprintf(string_max_pwm, "%f", setMaxPWM);
tnhnrl 57:ec69651c8c21 208 rudder_txt.setValue("setMaxPWM", string_max_pwm);
tnhnrl 57:ec69651c8c21 209
tnhnrl 57:ec69651c8c21 210 //SAVE THE DATA!
tnhnrl 57:ec69651c8c21 211 pc().printf("Saving RUDDER DATA!");
tnhnrl 57:ec69651c8c21 212
tnhnrl 57:ec69651c8c21 213 if (!rudder_txt.write("/local/rudder.txt")) {
tnhnrl 57:ec69651c8c21 214 pc().printf("\n\rERROR: (SAVE)Failure to write rudder.txt file.");
tnhnrl 57:ec69651c8c21 215 }
tnhnrl 57:ec69651c8c21 216 else {
tnhnrl 57:ec69651c8c21 217 pc().printf("\n\rFile rudder.txt successful written.\n\r");
tnhnrl 57:ec69651c8c21 218 }
tnhnrl 57:ec69651c8c21 219 }
tnhnrl 57:ec69651c8c21 220
tnhnrl 57:ec69651c8c21 221 int ConfigFileIO::load_BCE_config() {
tnhnrl 57:ec69651c8c21 222 ConfigFile cfg;
tnhnrl 57:ec69651c8c21 223 int count = 0;
tnhnrl 57:ec69651c8c21 224 if (!cfg.read("/local/bce.txt")) {
tnhnrl 57:ec69651c8c21 225 error("File Read Error");
tnhnrl 57:ec69651c8c21 226 }
tnhnrl 57:ec69651c8c21 227 char value[BUFSIZ];
tnhnrl 57:ec69651c8c21 228
tnhnrl 57:ec69651c8c21 229 if (cfg.getValue("PGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 230 bce().setControllerP(atof(value));
tnhnrl 57:ec69651c8c21 231 count++;
tnhnrl 57:ec69651c8c21 232 }
tnhnrl 57:ec69651c8c21 233 if (cfg.getValue("IGain", &value[0] ,sizeof(value))) {
tnhnrl 57:ec69651c8c21 234 bce().setControllerI(atof(value));
tnhnrl 57:ec69651c8c21 235 count++;
tnhnrl 57:ec69651c8c21 236 }
tnhnrl 57:ec69651c8c21 237 if (cfg.getValue("DGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 238 bce().setControllerD(atof(value));
tnhnrl 57:ec69651c8c21 239 count++;
tnhnrl 57:ec69651c8c21 240 }
tnhnrl 57:ec69651c8c21 241 if (cfg.getValue("zeroCounts", &value[0],sizeof(value))) {
tnhnrl 57:ec69651c8c21 242 bce().setZeroCounts(atoi(value));
tnhnrl 57:ec69651c8c21 243 count++;
tnhnrl 57:ec69651c8c21 244 }
tnhnrl 57:ec69651c8c21 245 if (cfg.getValue("PistonTravelLimit", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 246 bce().setTravelLimit(atof(value));
tnhnrl 57:ec69651c8c21 247 count++;
tnhnrl 57:ec69651c8c21 248 }
tnhnrl 57:ec69651c8c21 249 if (cfg.getValue("slope", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 250 bce().setPotSlope(atof(value));
tnhnrl 57:ec69651c8c21 251 count++;
tnhnrl 57:ec69651c8c21 252 }
tnhnrl 57:ec69651c8c21 253 if (cfg.getValue("filterWn", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 254 bce().setFilterFrequency(atof(value));
tnhnrl 57:ec69651c8c21 255 count++;
tnhnrl 57:ec69651c8c21 256 }
tnhnrl 57:ec69651c8c21 257 if (cfg.getValue("deadband", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 258 bce().setDeadband(atof(value));
tnhnrl 57:ec69651c8c21 259 count++;
tnhnrl 57:ec69651c8c21 260 }
tnhnrl 57:ec69651c8c21 261
tnhnrl 57:ec69651c8c21 262 return count;
tnhnrl 57:ec69651c8c21 263 }
tnhnrl 57:ec69651c8c21 264
tnhnrl 57:ec69651c8c21 265 //write to heading.txt
tnhnrl 73:f6f378311c8d 266 void ConfigFileIO::saveHeadingData(float heading_p_gain, float heading_i_gain, float heading_d_gain, float heading_zeroOffset, float heading_filter_freq, float heading_deadband) {
tnhnrl 57:ec69651c8c21 267 ConfigFile heading_txt;
tnhnrl 57:ec69651c8c21 268
tnhnrl 57:ec69651c8c21 269 char PGain_buffer[128];
tnhnrl 57:ec69651c8c21 270 sprintf(PGain_buffer,"# HEADING (rudder outer loop) parameters\n\n#Gains\nPGain");
tnhnrl 57:ec69651c8c21 271
tnhnrl 57:ec69651c8c21 272 //convert input numbers into text and save them to text file
tnhnrl 57:ec69651c8c21 273 char string_pgain[128];
tnhnrl 57:ec69651c8c21 274 sprintf(string_pgain, "%f", heading_p_gain);
tnhnrl 57:ec69651c8c21 275 heading_txt.setValue(PGain_buffer, string_pgain);
tnhnrl 57:ec69651c8c21 276
tnhnrl 57:ec69651c8c21 277 char string_igain[128];
tnhnrl 57:ec69651c8c21 278 sprintf(string_igain, "%f", heading_i_gain);
tnhnrl 57:ec69651c8c21 279 heading_txt.setValue("IGain", string_igain);
tnhnrl 57:ec69651c8c21 280
tnhnrl 57:ec69651c8c21 281 char string_dgain[128];
tnhnrl 57:ec69651c8c21 282 sprintf(string_dgain, "%f", heading_d_gain);
tnhnrl 57:ec69651c8c21 283 heading_txt.setValue("DGain", string_dgain);
tnhnrl 57:ec69651c8c21 284
tnhnrl 57:ec69651c8c21 285 heading_txt.setValue("\n# HEADING sensor filter parameters\nfilterWn", "6.0");
tnhnrl 57:ec69651c8c21 286 heading_txt.setValue("deadband", "0.5");
tnhnrl 57:ec69651c8c21 287
tnhnrl 57:ec69651c8c21 288 char string_zeroOffset[128];
tnhnrl 73:f6f378311c8d 289 sprintf(string_zeroOffset, "%f", heading_zeroOffset);
tnhnrl 57:ec69651c8c21 290 heading_txt.setValue("\n#HEADING offset for dive (no default!)\nzeroOffset", string_zeroOffset);
tnhnrl 57:ec69651c8c21 291
tnhnrl 57:ec69651c8c21 292 //SAVE THE DATA!
tnhnrl 57:ec69651c8c21 293 pc().printf("(ConfigFileIO) Saving HEADING parameters!");
tnhnrl 57:ec69651c8c21 294
tnhnrl 57:ec69651c8c21 295 if (!heading_txt.write("/local/heading.txt")) {
tnhnrl 57:ec69651c8c21 296 pc().printf("\n\rERROR: (SAVE) Failure to write heading.txt file.");
tnhnrl 57:ec69651c8c21 297 }
tnhnrl 57:ec69651c8c21 298 else {
tnhnrl 57:ec69651c8c21 299 pc().printf("\n\rFile heading.txt successful written.\n\r");
tnhnrl 57:ec69651c8c21 300 }
tnhnrl 57:ec69651c8c21 301 }
tnhnrl 57:ec69651c8c21 302
tnhnrl 57:ec69651c8c21 303 int ConfigFileIO::load_BATT_config() {
tnhnrl 57:ec69651c8c21 304 ConfigFile cfg;
tnhnrl 57:ec69651c8c21 305 int count = 0;
tnhnrl 57:ec69651c8c21 306 if (!cfg.read("/local/batt.txt")) {
tnhnrl 57:ec69651c8c21 307 error("BATT File Read Error");
tnhnrl 57:ec69651c8c21 308 }
tnhnrl 57:ec69651c8c21 309 char value[BUFSIZ];
tnhnrl 57:ec69651c8c21 310
tnhnrl 57:ec69651c8c21 311
tnhnrl 57:ec69651c8c21 312 if (cfg.getValue("PGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 313 batt().setControllerP(atof(value));
tnhnrl 57:ec69651c8c21 314 count++;
tnhnrl 57:ec69651c8c21 315 }
tnhnrl 57:ec69651c8c21 316 if (cfg.getValue("IGain", &value[0] ,sizeof(value))) {
tnhnrl 57:ec69651c8c21 317 batt().setControllerI(atof(value));
tnhnrl 57:ec69651c8c21 318 count++;
tnhnrl 57:ec69651c8c21 319 }
tnhnrl 57:ec69651c8c21 320 if (cfg.getValue("DGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 321 batt().setControllerD(atof(value));
tnhnrl 57:ec69651c8c21 322 count++;
tnhnrl 57:ec69651c8c21 323 }
tnhnrl 57:ec69651c8c21 324 if (cfg.getValue("zeroCounts", &value[0],sizeof(value))) {
tnhnrl 57:ec69651c8c21 325 batt().setZeroCounts(atoi(value));
tnhnrl 57:ec69651c8c21 326 count++;
tnhnrl 57:ec69651c8c21 327 }
tnhnrl 57:ec69651c8c21 328 if (cfg.getValue("PistonTravelLimit", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 329 batt().setTravelLimit(atof(value));
tnhnrl 57:ec69651c8c21 330 count++;
tnhnrl 57:ec69651c8c21 331 }
tnhnrl 57:ec69651c8c21 332 if (cfg.getValue("slope", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 333 batt().setPotSlope(atof(value));
tnhnrl 57:ec69651c8c21 334 count++;
tnhnrl 57:ec69651c8c21 335 }
tnhnrl 57:ec69651c8c21 336 if (cfg.getValue("filterWn", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 337 batt().setFilterFrequency(atof(value));
tnhnrl 57:ec69651c8c21 338 count++;
tnhnrl 57:ec69651c8c21 339 }
tnhnrl 57:ec69651c8c21 340 if (cfg.getValue("deadband", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 341 batt().setDeadband(atof(value));
tnhnrl 57:ec69651c8c21 342 count++;
tnhnrl 57:ec69651c8c21 343 }
tnhnrl 57:ec69651c8c21 344
tnhnrl 57:ec69651c8c21 345 return count;
tnhnrl 57:ec69651c8c21 346 }
tnhnrl 57:ec69651c8c21 347
tnhnrl 57:ec69651c8c21 348 int ConfigFileIO::load_DEPTH_config() {
tnhnrl 57:ec69651c8c21 349 ConfigFile cfg;
tnhnrl 57:ec69651c8c21 350 int count = 0;
tnhnrl 57:ec69651c8c21 351 if (!cfg.read("/local/depth.txt")) {
tnhnrl 57:ec69651c8c21 352 error("DEPTH File Read Error");
tnhnrl 57:ec69651c8c21 353 }
tnhnrl 57:ec69651c8c21 354 char value[BUFSIZ];
tnhnrl 57:ec69651c8c21 355
tnhnrl 57:ec69651c8c21 356 if (cfg.getValue("PGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 357 depthLoop().setControllerP(atof(value));
tnhnrl 57:ec69651c8c21 358 count++;
tnhnrl 57:ec69651c8c21 359 }
tnhnrl 57:ec69651c8c21 360 if (cfg.getValue("IGain", &value[0] ,sizeof(value))) {
tnhnrl 57:ec69651c8c21 361 depthLoop().setControllerI(atof(value));
tnhnrl 57:ec69651c8c21 362 count++;
tnhnrl 57:ec69651c8c21 363 }
tnhnrl 57:ec69651c8c21 364 if (cfg.getValue("DGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 365 depthLoop().setControllerD(atof(value));
tnhnrl 57:ec69651c8c21 366 count++;
tnhnrl 57:ec69651c8c21 367 }
tnhnrl 57:ec69651c8c21 368 if (cfg.getValue("filterWn", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 369 depthLoop().setFilterFrequency(atof(value));
tnhnrl 57:ec69651c8c21 370 count++;
tnhnrl 57:ec69651c8c21 371 }
tnhnrl 57:ec69651c8c21 372 if (cfg.getValue("deadband", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 373 depthLoop().setDeadband(atof(value));
tnhnrl 57:ec69651c8c21 374 count++;
tnhnrl 57:ec69651c8c21 375 }
tnhnrl 57:ec69651c8c21 376 if (cfg.getValue("zeroOffset", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 377 depthLoop().setOutputOffset(atof(value));
tnhnrl 57:ec69651c8c21 378 count++;
tnhnrl 57:ec69651c8c21 379 }
tnhnrl 57:ec69651c8c21 380 return count;
tnhnrl 57:ec69651c8c21 381 }
tnhnrl 57:ec69651c8c21 382
tnhnrl 57:ec69651c8c21 383 int ConfigFileIO::load_PITCH_config() {
tnhnrl 57:ec69651c8c21 384 ConfigFile cfg;
tnhnrl 57:ec69651c8c21 385 int count = 0;
tnhnrl 57:ec69651c8c21 386 if (!cfg.read("/local/pitch.txt")){
tnhnrl 57:ec69651c8c21 387 error("PITCH File Read Error");
tnhnrl 57:ec69651c8c21 388 }
tnhnrl 57:ec69651c8c21 389 char value[BUFSIZ];
tnhnrl 57:ec69651c8c21 390
tnhnrl 57:ec69651c8c21 391 if (cfg.getValue("PGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 392 pitchLoop().setControllerP(atof(value));
tnhnrl 57:ec69651c8c21 393 count++;
tnhnrl 57:ec69651c8c21 394 }
tnhnrl 57:ec69651c8c21 395 if (cfg.getValue("IGain", &value[0] ,sizeof(value))) {
tnhnrl 57:ec69651c8c21 396 pitchLoop().setControllerI(atof(value));
tnhnrl 57:ec69651c8c21 397 count++;
tnhnrl 57:ec69651c8c21 398 }
tnhnrl 57:ec69651c8c21 399 if (cfg.getValue("DGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 400 pitchLoop().setControllerD(atof(value));
tnhnrl 57:ec69651c8c21 401 count++;
tnhnrl 57:ec69651c8c21 402 }
tnhnrl 57:ec69651c8c21 403 if (cfg.getValue("filterWn", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 404 pitchLoop().setFilterFrequency(atof(value));
tnhnrl 57:ec69651c8c21 405 count++;
tnhnrl 57:ec69651c8c21 406 }
tnhnrl 57:ec69651c8c21 407 if (cfg.getValue("deadband", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 408 pitchLoop().setDeadband(atof(value));
tnhnrl 57:ec69651c8c21 409 count++;
tnhnrl 57:ec69651c8c21 410 }
tnhnrl 57:ec69651c8c21 411
tnhnrl 57:ec69651c8c21 412 if (cfg.getValue("zeroOffset", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 413 pitchLoop().setOutputOffset(atof(value));
tnhnrl 57:ec69651c8c21 414 count++;
tnhnrl 57:ec69651c8c21 415 }
tnhnrl 57:ec69651c8c21 416 return count;
tnhnrl 57:ec69651c8c21 417 }
tnhnrl 57:ec69651c8c21 418
tnhnrl 57:ec69651c8c21 419 int ConfigFileIO::load_HEADING_config() {
tnhnrl 57:ec69651c8c21 420 ConfigFile cfg;
tnhnrl 57:ec69651c8c21 421 int count = 0;
tnhnrl 57:ec69651c8c21 422 if (!cfg.read("/local/heading.txt")){
tnhnrl 57:ec69651c8c21 423 error("HEADING File Read Error");
tnhnrl 57:ec69651c8c21 424 }
tnhnrl 57:ec69651c8c21 425 char value[BUFSIZ];
tnhnrl 57:ec69651c8c21 426
tnhnrl 57:ec69651c8c21 427 if (cfg.getValue("PGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 428 headingLoop().setControllerP(atof(value));
tnhnrl 57:ec69651c8c21 429 count++;
tnhnrl 57:ec69651c8c21 430 }
tnhnrl 57:ec69651c8c21 431 if (cfg.getValue("IGain", &value[0] ,sizeof(value))) {
tnhnrl 57:ec69651c8c21 432 headingLoop().setControllerI(atof(value));
tnhnrl 57:ec69651c8c21 433 count++;
tnhnrl 57:ec69651c8c21 434 }
tnhnrl 57:ec69651c8c21 435 if (cfg.getValue("DGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 436 headingLoop().setControllerD(atof(value));
tnhnrl 57:ec69651c8c21 437 count++;
tnhnrl 57:ec69651c8c21 438 }
tnhnrl 57:ec69651c8c21 439 if (cfg.getValue("filterWn", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 440 headingLoop().setFilterFrequency(atof(value));
tnhnrl 57:ec69651c8c21 441 count++;
tnhnrl 57:ec69651c8c21 442 }
tnhnrl 57:ec69651c8c21 443 if (cfg.getValue("deadband", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 444 headingLoop().setDeadband(atof(value));
tnhnrl 57:ec69651c8c21 445 count++;
tnhnrl 57:ec69651c8c21 446 }
tnhnrl 57:ec69651c8c21 447
tnhnrl 57:ec69651c8c21 448 if (cfg.getValue("zeroOffset", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 449 headingLoop().setOutputOffset(atof(value));
tnhnrl 57:ec69651c8c21 450 count++;
tnhnrl 57:ec69651c8c21 451 }
tnhnrl 57:ec69651c8c21 452 return count;
tnhnrl 57:ec69651c8c21 453 }
tnhnrl 57:ec69651c8c21 454
tnhnrl 57:ec69651c8c21 455 int ConfigFileIO::load_RUDDER_config() {
tnhnrl 57:ec69651c8c21 456 ConfigFile cfg;
tnhnrl 57:ec69651c8c21 457 int count = 0;
tnhnrl 57:ec69651c8c21 458 if (!cfg.read("/local/rudder.txt")){
tnhnrl 57:ec69651c8c21 459 error("RUDDER File Read Error");
tnhnrl 57:ec69651c8c21 460 }
tnhnrl 57:ec69651c8c21 461 char value[BUFSIZ];
tnhnrl 57:ec69651c8c21 462
tnhnrl 57:ec69651c8c21 463 //float values below
tnhnrl 57:ec69651c8c21 464 if (cfg.getValue("setMinDeg", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 465 rudder().setMinDeg(atof(value));
tnhnrl 57:ec69651c8c21 466 count++;
tnhnrl 57:ec69651c8c21 467 }
tnhnrl 57:ec69651c8c21 468 if (cfg.getValue("setMaxDeg", &value[0] ,sizeof(value))) {
tnhnrl 57:ec69651c8c21 469 rudder().setMaxDeg(atof(value));
tnhnrl 57:ec69651c8c21 470 count++;
tnhnrl 57:ec69651c8c21 471 }
tnhnrl 57:ec69651c8c21 472
tnhnrl 57:ec69651c8c21 473 //integer values below
tnhnrl 57:ec69651c8c21 474 if (cfg.getValue("setCenterPWM", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 475 rudder().setCenterPWM(atof(value));
tnhnrl 57:ec69651c8c21 476 count++;
tnhnrl 57:ec69651c8c21 477 }
tnhnrl 57:ec69651c8c21 478 if (cfg.getValue("setMinPWM", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 479 rudder().setMinPWM(atof(value));
tnhnrl 57:ec69651c8c21 480 count++;
tnhnrl 57:ec69651c8c21 481 }
tnhnrl 57:ec69651c8c21 482 if (cfg.getValue("setMaxPWM", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 483 rudder().setMaxPWM(atof(value));
tnhnrl 57:ec69651c8c21 484 count++;
tnhnrl 57:ec69651c8c21 485 }
tnhnrl 57:ec69651c8c21 486 return count;
tnhnrl 57:ec69651c8c21 487 }