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@79:3688c3a0d7f4, 2018-10-23 (annotated)

Committer:
tnhnrl
Date:
Tue Oct 23 20:35:16 2018 +0000
Revision:
79:3688c3a0d7f4
Parent:
76:c802e1da4179
Child:
82:0981b9ada820
SD card logger working

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