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@74:d281aaef9766, 2018-08-14 (annotated)

Committer:
tnhnrl
Date:
Tue Aug 14 21:06:48 2018 +0000
Revision:
74:d281aaef9766
Parent:
73:f6f378311c8d
Child:
76:c802e1da4179
1) Fixed deadband on float broadcast ; 2) Fixed find neutral commands

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 74:d281aaef9766 44 xbee().printf("Saving BATTERY MOVER PID data!");
tnhnrl 57:ec69651c8c21 45
tnhnrl 57:ec69651c8c21 46 if (!write_Batt_txt.write("/local/batt.txt")) {
tnhnrl 74:d281aaef9766 47 xbee().printf("\n\rERROR: (SAVE)Failure to write batt.txt file.");
tnhnrl 57:ec69651c8c21 48 }
tnhnrl 57:ec69651c8c21 49 else {
tnhnrl 74:d281aaef9766 50 xbee().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 74:d281aaef9766 86 xbee().printf("Saving Buoyancy Engine Neutral Buoyancy Positions!");
tnhnrl 57:ec69651c8c21 87
tnhnrl 57:ec69651c8c21 88 if (!write_pitch_txt.write("/local/pitch.txt")) {
tnhnrl 74:d281aaef9766 89 xbee().printf("\n\rERROR: (SAVE)Failure to write depth.txt file.");
tnhnrl 57:ec69651c8c21 90 }
tnhnrl 57:ec69651c8c21 91 else {
tnhnrl 74:d281aaef9766 92 xbee().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 74:d281aaef9766 131 xbee().printf("Saving BCE PID data!");
tnhnrl 57:ec69651c8c21 132
tnhnrl 57:ec69651c8c21 133 if (!write_BCE_txt.write("/local/bce.txt")) {
tnhnrl 74:d281aaef9766 134 xbee().printf("\n\rERROR: (SAVE)Failure to write bce.txt file.");
tnhnrl 57:ec69651c8c21 135 }
tnhnrl 57:ec69651c8c21 136 else {
tnhnrl 74:d281aaef9766 137 xbee().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 74:d281aaef9766 173 xbee().printf("Saving Buoyancy Engine Neutral Buoyancy Positions!");
tnhnrl 57:ec69651c8c21 174
tnhnrl 57:ec69651c8c21 175 if (!write_depth_txt.write("/local/depth.txt")) {
tnhnrl 74:d281aaef9766 176 xbee().printf("\n\rERROR: (SAVE)Failure to write depth.txt file.");
tnhnrl 57:ec69651c8c21 177 }
tnhnrl 57:ec69651c8c21 178 else {
tnhnrl 74:d281aaef9766 179 xbee().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 74:d281aaef9766 211 xbee().printf("Saving RUDDER DATA!");
tnhnrl 57:ec69651c8c21 212
tnhnrl 57:ec69651c8c21 213 if (!rudder_txt.write("/local/rudder.txt")) {
tnhnrl 74:d281aaef9766 214 xbee().printf("\n\rERROR: (SAVE)Failure to write rudder.txt file.");
tnhnrl 57:ec69651c8c21 215 }
tnhnrl 57:ec69651c8c21 216 else {
tnhnrl 74:d281aaef9766 217 xbee().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 74:d281aaef9766 284
tnhnrl 74:d281aaef9766 285 char string_heading_freq[128];
tnhnrl 74:d281aaef9766 286 sprintf(string_heading_freq, "%f", heading_filter_freq);
tnhnrl 74:d281aaef9766 287 heading_txt.setValue("\n# HEADING sensor filter parameters\nfilterWn", string_heading_freq);
tnhnrl 74:d281aaef9766 288
tnhnrl 74:d281aaef9766 289 char string_heading_db[128];
tnhnrl 74:d281aaef9766 290 sprintf(string_heading_db, "%f", heading_deadband);
tnhnrl 74:d281aaef9766 291 heading_txt.setValue("deadband",string_heading_db);
tnhnrl 57:ec69651c8c21 292
tnhnrl 57:ec69651c8c21 293 char string_zeroOffset[128];
tnhnrl 73:f6f378311c8d 294 sprintf(string_zeroOffset, "%f", heading_zeroOffset);
tnhnrl 74:d281aaef9766 295 heading_txt.setValue("\n#HEADING offset\nzeroOffset", string_zeroOffset);
tnhnrl 57:ec69651c8c21 296
tnhnrl 57:ec69651c8c21 297 //SAVE THE DATA!
tnhnrl 74:d281aaef9766 298 xbee().printf("(ConfigFileIO) Saving HEADING parameters!");
tnhnrl 57:ec69651c8c21 299
tnhnrl 57:ec69651c8c21 300 if (!heading_txt.write("/local/heading.txt")) {
tnhnrl 74:d281aaef9766 301 xbee().printf("\n\rERROR: (SAVE) Failure to write heading.txt file.");
tnhnrl 57:ec69651c8c21 302 }
tnhnrl 57:ec69651c8c21 303 else {
tnhnrl 74:d281aaef9766 304 xbee().printf("\n\rFile heading.txt successful written.\n\r");
tnhnrl 57:ec69651c8c21 305 }
tnhnrl 57:ec69651c8c21 306 }
tnhnrl 57:ec69651c8c21 307
tnhnrl 57:ec69651c8c21 308 int ConfigFileIO::load_BATT_config() {
tnhnrl 57:ec69651c8c21 309 ConfigFile cfg;
tnhnrl 57:ec69651c8c21 310 int count = 0;
tnhnrl 57:ec69651c8c21 311 if (!cfg.read("/local/batt.txt")) {
tnhnrl 57:ec69651c8c21 312 error("BATT File Read Error");
tnhnrl 57:ec69651c8c21 313 }
tnhnrl 57:ec69651c8c21 314 char value[BUFSIZ];
tnhnrl 57:ec69651c8c21 315
tnhnrl 57:ec69651c8c21 316
tnhnrl 57:ec69651c8c21 317 if (cfg.getValue("PGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 318 batt().setControllerP(atof(value));
tnhnrl 57:ec69651c8c21 319 count++;
tnhnrl 57:ec69651c8c21 320 }
tnhnrl 57:ec69651c8c21 321 if (cfg.getValue("IGain", &value[0] ,sizeof(value))) {
tnhnrl 57:ec69651c8c21 322 batt().setControllerI(atof(value));
tnhnrl 57:ec69651c8c21 323 count++;
tnhnrl 57:ec69651c8c21 324 }
tnhnrl 57:ec69651c8c21 325 if (cfg.getValue("DGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 326 batt().setControllerD(atof(value));
tnhnrl 57:ec69651c8c21 327 count++;
tnhnrl 57:ec69651c8c21 328 }
tnhnrl 57:ec69651c8c21 329 if (cfg.getValue("zeroCounts", &value[0],sizeof(value))) {
tnhnrl 57:ec69651c8c21 330 batt().setZeroCounts(atoi(value));
tnhnrl 57:ec69651c8c21 331 count++;
tnhnrl 57:ec69651c8c21 332 }
tnhnrl 57:ec69651c8c21 333 if (cfg.getValue("PistonTravelLimit", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 334 batt().setTravelLimit(atof(value));
tnhnrl 57:ec69651c8c21 335 count++;
tnhnrl 57:ec69651c8c21 336 }
tnhnrl 57:ec69651c8c21 337 if (cfg.getValue("slope", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 338 batt().setPotSlope(atof(value));
tnhnrl 57:ec69651c8c21 339 count++;
tnhnrl 57:ec69651c8c21 340 }
tnhnrl 57:ec69651c8c21 341 if (cfg.getValue("filterWn", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 342 batt().setFilterFrequency(atof(value));
tnhnrl 57:ec69651c8c21 343 count++;
tnhnrl 57:ec69651c8c21 344 }
tnhnrl 57:ec69651c8c21 345 if (cfg.getValue("deadband", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 346 batt().setDeadband(atof(value));
tnhnrl 57:ec69651c8c21 347 count++;
tnhnrl 57:ec69651c8c21 348 }
tnhnrl 57:ec69651c8c21 349
tnhnrl 57:ec69651c8c21 350 return count;
tnhnrl 57:ec69651c8c21 351 }
tnhnrl 57:ec69651c8c21 352
tnhnrl 57:ec69651c8c21 353 int ConfigFileIO::load_DEPTH_config() {
tnhnrl 57:ec69651c8c21 354 ConfigFile cfg;
tnhnrl 57:ec69651c8c21 355 int count = 0;
tnhnrl 57:ec69651c8c21 356 if (!cfg.read("/local/depth.txt")) {
tnhnrl 57:ec69651c8c21 357 error("DEPTH File Read Error");
tnhnrl 57:ec69651c8c21 358 }
tnhnrl 57:ec69651c8c21 359 char value[BUFSIZ];
tnhnrl 57:ec69651c8c21 360
tnhnrl 57:ec69651c8c21 361 if (cfg.getValue("PGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 362 depthLoop().setControllerP(atof(value));
tnhnrl 57:ec69651c8c21 363 count++;
tnhnrl 57:ec69651c8c21 364 }
tnhnrl 57:ec69651c8c21 365 if (cfg.getValue("IGain", &value[0] ,sizeof(value))) {
tnhnrl 57:ec69651c8c21 366 depthLoop().setControllerI(atof(value));
tnhnrl 57:ec69651c8c21 367 count++;
tnhnrl 57:ec69651c8c21 368 }
tnhnrl 57:ec69651c8c21 369 if (cfg.getValue("DGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 370 depthLoop().setControllerD(atof(value));
tnhnrl 57:ec69651c8c21 371 count++;
tnhnrl 57:ec69651c8c21 372 }
tnhnrl 57:ec69651c8c21 373 if (cfg.getValue("filterWn", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 374 depthLoop().setFilterFrequency(atof(value));
tnhnrl 57:ec69651c8c21 375 count++;
tnhnrl 57:ec69651c8c21 376 }
tnhnrl 57:ec69651c8c21 377 if (cfg.getValue("deadband", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 378 depthLoop().setDeadband(atof(value));
tnhnrl 57:ec69651c8c21 379 count++;
tnhnrl 57:ec69651c8c21 380 }
tnhnrl 57:ec69651c8c21 381 if (cfg.getValue("zeroOffset", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 382 depthLoop().setOutputOffset(atof(value));
tnhnrl 57:ec69651c8c21 383 count++;
tnhnrl 57:ec69651c8c21 384 }
tnhnrl 57:ec69651c8c21 385 return count;
tnhnrl 57:ec69651c8c21 386 }
tnhnrl 57:ec69651c8c21 387
tnhnrl 57:ec69651c8c21 388 int ConfigFileIO::load_PITCH_config() {
tnhnrl 57:ec69651c8c21 389 ConfigFile cfg;
tnhnrl 57:ec69651c8c21 390 int count = 0;
tnhnrl 57:ec69651c8c21 391 if (!cfg.read("/local/pitch.txt")){
tnhnrl 57:ec69651c8c21 392 error("PITCH File Read Error");
tnhnrl 57:ec69651c8c21 393 }
tnhnrl 57:ec69651c8c21 394 char value[BUFSIZ];
tnhnrl 57:ec69651c8c21 395
tnhnrl 57:ec69651c8c21 396 if (cfg.getValue("PGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 397 pitchLoop().setControllerP(atof(value));
tnhnrl 57:ec69651c8c21 398 count++;
tnhnrl 57:ec69651c8c21 399 }
tnhnrl 57:ec69651c8c21 400 if (cfg.getValue("IGain", &value[0] ,sizeof(value))) {
tnhnrl 57:ec69651c8c21 401 pitchLoop().setControllerI(atof(value));
tnhnrl 57:ec69651c8c21 402 count++;
tnhnrl 57:ec69651c8c21 403 }
tnhnrl 57:ec69651c8c21 404 if (cfg.getValue("DGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 405 pitchLoop().setControllerD(atof(value));
tnhnrl 57:ec69651c8c21 406 count++;
tnhnrl 57:ec69651c8c21 407 }
tnhnrl 57:ec69651c8c21 408 if (cfg.getValue("filterWn", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 409 pitchLoop().setFilterFrequency(atof(value));
tnhnrl 57:ec69651c8c21 410 count++;
tnhnrl 57:ec69651c8c21 411 }
tnhnrl 57:ec69651c8c21 412 if (cfg.getValue("deadband", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 413 pitchLoop().setDeadband(atof(value));
tnhnrl 57:ec69651c8c21 414 count++;
tnhnrl 57:ec69651c8c21 415 }
tnhnrl 57:ec69651c8c21 416
tnhnrl 57:ec69651c8c21 417 if (cfg.getValue("zeroOffset", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 418 pitchLoop().setOutputOffset(atof(value));
tnhnrl 57:ec69651c8c21 419 count++;
tnhnrl 57:ec69651c8c21 420 }
tnhnrl 57:ec69651c8c21 421 return count;
tnhnrl 57:ec69651c8c21 422 }
tnhnrl 57:ec69651c8c21 423
tnhnrl 57:ec69651c8c21 424 int ConfigFileIO::load_HEADING_config() {
tnhnrl 57:ec69651c8c21 425 ConfigFile cfg;
tnhnrl 57:ec69651c8c21 426 int count = 0;
tnhnrl 57:ec69651c8c21 427 if (!cfg.read("/local/heading.txt")){
tnhnrl 57:ec69651c8c21 428 error("HEADING File Read Error");
tnhnrl 57:ec69651c8c21 429 }
tnhnrl 57:ec69651c8c21 430 char value[BUFSIZ];
tnhnrl 57:ec69651c8c21 431
tnhnrl 57:ec69651c8c21 432 if (cfg.getValue("PGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 433 headingLoop().setControllerP(atof(value));
tnhnrl 57:ec69651c8c21 434 count++;
tnhnrl 57:ec69651c8c21 435 }
tnhnrl 57:ec69651c8c21 436 if (cfg.getValue("IGain", &value[0] ,sizeof(value))) {
tnhnrl 57:ec69651c8c21 437 headingLoop().setControllerI(atof(value));
tnhnrl 57:ec69651c8c21 438 count++;
tnhnrl 57:ec69651c8c21 439 }
tnhnrl 57:ec69651c8c21 440 if (cfg.getValue("DGain", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 441 headingLoop().setControllerD(atof(value));
tnhnrl 57:ec69651c8c21 442 count++;
tnhnrl 57:ec69651c8c21 443 }
tnhnrl 57:ec69651c8c21 444 if (cfg.getValue("filterWn", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 445 headingLoop().setFilterFrequency(atof(value));
tnhnrl 57:ec69651c8c21 446 count++;
tnhnrl 57:ec69651c8c21 447 }
tnhnrl 57:ec69651c8c21 448 if (cfg.getValue("deadband", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 449 headingLoop().setDeadband(atof(value));
tnhnrl 57:ec69651c8c21 450 count++;
tnhnrl 57:ec69651c8c21 451 }
tnhnrl 57:ec69651c8c21 452
tnhnrl 57:ec69651c8c21 453 if (cfg.getValue("zeroOffset", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 454 headingLoop().setOutputOffset(atof(value));
tnhnrl 57:ec69651c8c21 455 count++;
tnhnrl 57:ec69651c8c21 456 }
tnhnrl 57:ec69651c8c21 457 return count;
tnhnrl 57:ec69651c8c21 458 }
tnhnrl 57:ec69651c8c21 459
tnhnrl 57:ec69651c8c21 460 int ConfigFileIO::load_RUDDER_config() {
tnhnrl 57:ec69651c8c21 461 ConfigFile cfg;
tnhnrl 57:ec69651c8c21 462 int count = 0;
tnhnrl 57:ec69651c8c21 463 if (!cfg.read("/local/rudder.txt")){
tnhnrl 57:ec69651c8c21 464 error("RUDDER File Read Error");
tnhnrl 57:ec69651c8c21 465 }
tnhnrl 57:ec69651c8c21 466 char value[BUFSIZ];
tnhnrl 57:ec69651c8c21 467
tnhnrl 57:ec69651c8c21 468 //float values below
tnhnrl 57:ec69651c8c21 469 if (cfg.getValue("setMinDeg", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 470 rudder().setMinDeg(atof(value));
tnhnrl 57:ec69651c8c21 471 count++;
tnhnrl 57:ec69651c8c21 472 }
tnhnrl 57:ec69651c8c21 473 if (cfg.getValue("setMaxDeg", &value[0] ,sizeof(value))) {
tnhnrl 57:ec69651c8c21 474 rudder().setMaxDeg(atof(value));
tnhnrl 57:ec69651c8c21 475 count++;
tnhnrl 57:ec69651c8c21 476 }
tnhnrl 57:ec69651c8c21 477
tnhnrl 57:ec69651c8c21 478 //integer values below
tnhnrl 57:ec69651c8c21 479 if (cfg.getValue("setCenterPWM", &value[0] , sizeof(value))) {
tnhnrl 57:ec69651c8c21 480 rudder().setCenterPWM(atof(value));
tnhnrl 57:ec69651c8c21 481 count++;
tnhnrl 57:ec69651c8c21 482 }
tnhnrl 57:ec69651c8c21 483 if (cfg.getValue("setMinPWM", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 484 rudder().setMinPWM(atof(value));
tnhnrl 57:ec69651c8c21 485 count++;
tnhnrl 57:ec69651c8c21 486 }
tnhnrl 57:ec69651c8c21 487 if (cfg.getValue("setMaxPWM", &value[0], sizeof(value))) {
tnhnrl 57:ec69651c8c21 488 rudder().setMaxPWM(atof(value));
tnhnrl 57:ec69651c8c21 489 count++;
tnhnrl 57:ec69651c8c21 490 }
tnhnrl 57:ec69651c8c21 491 return count;
tnhnrl 57:ec69651c8c21 492 }