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
Diff: MbedLogger/MbedLogger.cpp
- Revision:
- 73:f6f378311c8d
- Parent:
- 72:250b2665755c
- Child:
- 74:d281aaef9766
--- a/MbedLogger/MbedLogger.cpp Mon Jul 02 14:28:22 2018 +0000 +++ b/MbedLogger/MbedLogger.cpp Mon Jul 30 16:48:48 2018 +0000 @@ -17,11 +17,18 @@ _log_file_line_counter = 0; //used to set timer in finite state machine based on size of log - _heading_string = "state_string,state_ID,timer,depth_cmd,depth_ft,pitch_cmd,pitch_deg,bce_cmd,bce_mm,batt_cmd,batt_mm,pitchRate_degs,depthRate_fps,sys_amps,sys_volts,int_press_PSI\n"; +//NEXT VERSION WILL ELIMINATE LENGTH REQUIREMENT + //_heading_string = "state_string,state_ID,timer,depth_cmd,depth_ft,pitch_cmd,pitch_deg,bce_cmd,bce_mm,batt_cmd,batt_mm,pitchRate_degs,depthRate_fps,sys_amps,sys_volts,int_press_PSI\n"; + +//NEW ALTIMETER READINGS! + + _heading_string = "state_string,state_number,timer_sec,depth_cmd,depth_ft,pitch_cmd,pitch_deg,rudder_cmd_deg,heading_deg,bce_cmd,bce_mm,batt_cmd,batt_mm,pitch_rate_degs,depth_rate_fps,system_amps,system_volts,altimeter_or_intPSI,BCE_p,i,d,BATT_p,i,d,DEPTH_p,i,d,PITCH_p,i,d\n"; _transmit_packet_num = 0; _fsm_transmit_complete = false; + + _end_transmit_packet = false; } //this function has to be called for the time to function correctly @@ -39,30 +46,42 @@ void MbedLogger::recordData(int current_state) { int data_log_int = mbedLogger().getSystemTime(); //read the system timer to get unix timestamp - //_data_log[0] = mbedLogger().getSystemTime(); //read the system timer to get unix timestamp + _data_log[1] = depthLoop().getCommand(); //depth command - _data_log[2] = depthLoop().getPosition(); //depth reading + _data_log[2] = depthLoop().getPosition(); //depth reading (filtered depth) _data_log[3] = pitchLoop().getCommand(); //pitch command - _data_log[4] = pitchLoop().getPosition(); //pitch reading - _data_log[5] = bce().getSetPosition_mm(); //BCE command - _data_log[6] = bce().getPosition_mm(); //BCE reading - _data_log[7] = batt().getSetPosition_mm(); //Batt command - _data_log[8] = batt().getPosition_mm(); //Batt reading - _data_log[9] = pitchLoop().getVelocity(); // pitchRate_degs (degrees per second) - _data_log[10] = depthLoop().getVelocity(); // depthRate_fps (feet per second) + _data_log[4] = pitchLoop().getPosition(); //pitch reading (filtered pitch) + _data_log[5] = rudder().getPosition_deg(); //rudder command + _data_log[6] = headingLoop().getPosition(); //heading reading (filtered heading) + + _data_log[7] = bce().getSetPosition_mm(); //BCE command + _data_log[8] = bce().getPosition_mm(); //BCE reading + _data_log[9] = batt().getSetPosition_mm(); //Batt command + _data_log[10] = batt().getPosition_mm(); //Batt reading + _data_log[11] = pitchLoop().getVelocity(); // pitchRate_degs (degrees per second) + _data_log[12] = depthLoop().getVelocity(); // depthRate_fps (feet per second) - // extrapolated from graph if V_s = 5.0 - // https://www.nxp.com/docs/en/data-sheet/MPXA6115A.pdf - float press_xducer_PSI = (22.029*(5.0*adc().readCh5()/4095.0) + 10.884) * 0.145038; // Press_Xducer (on-board) - float voltage_input = (adc().readCh6()/4095.0) * 5.0 * 7.8; //Vin_Mon on PCB schematic (system input voltage from power supply) - float current_input = adc().readCh7()/4095.0; // I_mon (appears to be one-to-one from the schematic) - // presssure onboard + _data_log[13] = sensors().getCurrentInput(); // i_in + _data_log[14] = sensors().getVoltageInput(); // v_in + _data_log[15] = sensors().getInternalPressurePSI(); // int_press_PSI + + //BCE_p,i,d + _data_log[16] = bce().getControllerP(); + _data_log[17] = bce().getControllerI(); + _data_log[18] = bce().getControllerD(); + _data_log[19] = batt().getControllerP(); + _data_log[20] = batt().getControllerI(); + _data_log[21] = batt().getControllerD(); - _data_log[11] = current_input; // i_in - _data_log[12] = voltage_input; // v_in - _data_log[13] = press_xducer_PSI; // int_press_PSI + _data_log[22] = depthLoop().getControllerP(); + _data_log[23] = depthLoop().getControllerI(); + _data_log[24] = depthLoop().getControllerD(); + _data_log[25] = pitchLoop().getControllerP(); + _data_log[26] = pitchLoop().getControllerI(); + _data_log[27] = pitchLoop().getControllerD(); + //check what the current state is and create that string string string_state; if (current_state == SIT_IDLE) @@ -100,14 +119,18 @@ string blank_space = ""; //to get consistent spacing in the file //record the string state, integer state, and then the data - //_heading_string is 160 characters long + + //_heading_string is 254 characters long + + //packet size is 254 characters long //make function that checks length of header versus string below, add padding fprintf(_fp, "%16s,%.2d,",string_state.c_str(),current_state); - fprintf(_fp, "%11d,",data_log_int); //length 10 - fprintf(_fp, "%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.3f,%6.2f,%6.1f%38s\n",_data_log[1], - _data_log[2],_data_log[3],_data_log[4],_data_log[5],_data_log[6],_data_log[7],_data_log[8],_data_log[9],_data_log[10],_data_log[11],_data_log[12],_data_log[13],blank_space.c_str()); + fprintf(_fp, "%11d,",data_log_int); //length 10 //right here + fprintf(_fp, "%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.3f,%6.2f,%6.1f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f%41s\n",_data_log[1], + _data_log[2],_data_log[3],_data_log[4],_data_log[5],_data_log[6],_data_log[7],_data_log[8],_data_log[9],_data_log[10],_data_log[11],_data_log[12],_data_log[13],_data_log[14],_data_log[15], + _data_log[16],_data_log[17],_data_log[18],_data_log[19],_data_log[20],_data_log[21],_data_log[22],_data_log[23],_data_log[24],_data_log[25],_data_log[26],_data_log[27],blank_space.c_str()); //each line in the file is 160 characters long text-wise, check this with a file read } @@ -184,6 +207,37 @@ pc().printf("\n\rLog file closed. Lines in log file: %d.\n\r", _log_file_line_counter); } +void MbedLogger::blastData() { + pc().printf("blastData FUNCTION\n\r"); + +//OPEN FILE FOR READING + + string file_name_string = _file_system_string + "LOG000.csv"; + + _fp = fopen(file_name_string.c_str(), "r"); + +/******************************/ + while(1) { + wait(0.05); + if (!feof(_fp)) { //check for end of file + //based on the internal packet number of the class //createDataPacket //transmit data packet + transmitPacketNumber(_packet_number); + pc().printf("\r"); // for proper spacing + + _packet_number++; + } + else { + _packet_number = 0; //reset packet number + break; + } + } +/******************************/ + +//CLOSE THE FILE + closeLogFile(); + pc().printf("\n\rblastData: Log file closed. %d.\n\r"); +} + void MbedLogger::openFileForTransmit() { pc().printf("\n\ropenFileForTransmit\n\r"); //open the current file to read the contents @@ -315,38 +369,29 @@ led4() = !led4(); } -bool MbedLogger::fsmTransmitData() { //using the finite state machine - //pc().printf("debug fsmTransmitData (%d) (tpn: %d)\n\r",_fp,_transmit_packet_num); - - if (!feof(_fp) and (!_fsm_transmit_complete)) { //check for end of file - - //based on the internal packet number of the class - //createDataPacket - //transmit data packet +void MbedLogger::fsmTransmitData() { //using the finite state machine + if ( !feof(_fp) and (!_fsm_transmit_complete) ) { //check for end of file + //based on the internal packet number of the class //createDataPacket //transmit data packet transmitPacketNumber(_packet_number); pc().printf("\r"); // for proper spacing - + _packet_number++; - led2() = !led2(); - return true; + led2() = !led2(); } - else { - //is there a better way to do this? - //this flag was required because the additional requests kept generating two packets through the FSM - // by calling mbedLogger().fsmTransmitData(); - //and mbedLogger().checkForPythonTransmitRequest(); + _packet_number = 0; //reset packet number + _fsm_transmit_complete = true; - - return false; + + led3() = !led3(); } } //transmitting log file with fixed length of characters to receiver program void MbedLogger::transmitPacketNumber(int line_number) { - int line_size = 160; //length of lines in the log file, EVERY LINE MUST BE THE SAME LENGTH + int line_size = 254; //length of lines in the log file, EVERY LINE MUST BE THE SAME LENGTH fseek(_fp,(line_size+1)*line_number,SEEK_SET); //fseek must use the +1 to get the newline character @@ -368,191 +413,249 @@ } -//sample packet -//1E 1F 00 01 2E F6 //packet 1 -//1E 1F 00 02 2F B6 //packet 2 //receive correct data packet from python, immediately send a transmit packet from the MBED -void MbedLogger::checkForPythonTransmitRequest() { +void MbedLogger::transmitOnePacket() { + pc().printf("transmitOnePacket\n"); + static int transmit_state = HEADER_117; //state in switch statement - int incoming_byte = -1; //reset each time a character is read + //int incoming_byte = -1; //reset each time a character is read int requested_packet_number = -1; //reset each time a character is read static int input_packet[4]; //changed from char in previous iteration 03/28/2018 static int transmit_crc_one = 0; //hold crc values until they're reset with calculations static int transmit_crc_two = 0; - - while (pc().readable()) { - incoming_byte = pc().getc(); + + int incoming_byte[6]; + + static int bytes_received = 0; + + int req_packet_number = -1; - switch(transmit_state) { - case HEADER_117: - //continue processing - if (incoming_byte == 30){ //1E - transmit_state = HEADER_101; - } - //did not receive byte 1 - else { - transmit_state = HEADER_117; //go back to checking the first packet - } - break; - - case HEADER_101: - if(incoming_byte == 31) { //1F - transmit_state = TRANSMIT_PACKET_1; - } - break; + while (1) { + incoming_byte[bytes_received] = pc().getc(); + pc().printf("<%d> ", incoming_byte[bytes_received]); - case TRANSMIT_PACKET_1: - if (incoming_byte >= 0) { - input_packet[0] = 30; - input_packet[1] = 31; - input_packet[2] = incoming_byte; - - transmit_state = TRANSMIT_PACKET_2; - //_reply_byte3 = incoming_byte; - } - break; + bytes_received++; + + if (bytes_received > 5) { - case TRANSMIT_PACKET_2: + req_packet_number = incoming_byte[2] * 256 + incoming_byte[3]; - if (incoming_byte >= 0) { - input_packet[3] = incoming_byte; - //_reply_byte4 = incoming_byte; - } - transmit_state = PACKET_CRC_ONE; - - break; + pc().printf("req_packet_number = %d\n\r", req_packet_number); - case (PACKET_CRC_ONE): - transmit_crc_one = calcCrcOneArray(input_packet, 4); //calc CRC 1 from the input packet (size 4) - - if (incoming_byte == transmit_crc_one) { - transmit_state = PACKET_CRC_TWO; - } - - else - transmit_state = HEADER_117; - //or state remains the same? - + bytes_received = 0; break; - - case (PACKET_CRC_TWO): - transmit_state = HEADER_117; - transmit_crc_two = calcCrcTwoArray(input_packet, 4); //calc CRC 2 from the input packet (size 4) - - //check if CRC TWO is correct (then send full packet) - if (incoming_byte == transmit_crc_two) { - requested_packet_number = input_packet[2] * 256 + input_packet[3]; //compute the numbers 0 through 65535 with the two bytes - - //receive correct checksum, immediately send this packet - transmitPacketNumber(requested_packet_number); - - //fseek(_fp, 0, SEEK_END); //reset _fp (this was causing errors with the other function) - led3() = !led3(); - - } //end of checksum (incoming_byte) if statement - - break; - } //switch statement complete - } //while statement complete + } + } + + setTransmitPacketNumber(0); + + //open the file + string file_name_string = _file_system_string + "LOG000.csv"; + _fp = fopen(file_name_string.c_str(), "r"); + + //receive correct checksum, immediately send this packet + transmitPacketNumber(req_packet_number); + + fclose(_fp); } -void MbedLogger::readTransmitPacketOneChar() { +void MbedLogger::transmitMultiplePackets() { + pc().printf("transmitMultiplePackets\n"); + static int transmit_state = HEADER_117; //state in switch statement - int incoming_byte = -1; //reset each time a character is read + //int incoming_byte = -1; //reset each time a character is read int requested_packet_number = -1; //reset each time a character is read static int input_packet[4]; //changed from char in previous iteration 03/28/2018 static int transmit_crc_one = 0; //hold crc values until they're reset with calculations static int transmit_crc_two = 0; + int incoming_byte[6]; - while (pc().readable()) { - incoming_byte = pc().getc(); + static int bytes_received = 0; + + int req_packet_number = -1; + + + //open the file + string file_name_string = _file_system_string + "LOG000.csv"; + _fp = fopen(file_name_string.c_str(), "r"); + + while (1) { + //INCOMING BYTE + incoming_byte[bytes_received] = pc().getc(); + pc().printf("<%d> ", incoming_byte[bytes_received]); + + //EXIT IF YOU RECEIVE... + if (incoming_byte[bytes_received] == 16) { + //quick and dirty + if (pc().getc() == 17) { + if (pc().getc() == 18) { + if (pc().getc() == 19) { + //exit if you get all of these packets hex 10 11 12 13, dec 16 17 18 19 + pc().printf("END TRANSMISSION REQUEST RECEIVED!\n\r"); + break; + } + } + } + } + bytes_received++; + + if (bytes_received > 5) { + + req_packet_number = incoming_byte[2] * 256 + incoming_byte[3]; + + pc().printf("req_packet_number = %d\n\r", req_packet_number); + + //receive correct checksum, immediately send this packet + transmitPacketNumber(req_packet_number); + + + //reset bytes_received + bytes_received = 0; + } + } + + //CLOSE THE FILE + fclose(_fp); +} + +void MbedLogger::checkForPythonTransmitRequest() { + //pc().printf("checkForPythonTransmitRequest\n"); + + if ( pc().readable() ) { + +//PC READABLE DOES NOT WORK HERE?! + + led2() = !led2(); led3() = !led3(); - switch(transmit_state) { - case HEADER_117: - //continue processing - if (incoming_byte == 30){ //1E - transmit_state = HEADER_101; - } - //did not receive byte 1 - else { - transmit_state = HEADER_117; //go back to checking the first packet - } - break; - - case HEADER_101: - if(incoming_byte == 31) { //1F - transmit_state = TRANSMIT_PACKET_1; - } - break; - - case TRANSMIT_PACKET_1: - if (incoming_byte >= 0) { - input_packet[0] = 30; - input_packet[1] = 31; - input_packet[2] = incoming_byte; - - transmit_state = TRANSMIT_PACKET_2; - //_reply_byte3 = incoming_byte; - } - break; - - case TRANSMIT_PACKET_2: - - if (incoming_byte >= 0) { - input_packet[3] = incoming_byte; - //_reply_byte4 = incoming_byte; - } - transmit_state = PACKET_CRC_ONE; - - break; - - case (PACKET_CRC_ONE): - transmit_crc_one = calcCrcOneArray(input_packet, 4); //calc CRC 1 from the input packet (size 4) - - if (incoming_byte == transmit_crc_one) { - transmit_state = PACKET_CRC_TWO; - } - - else - transmit_state = HEADER_117; - //or state remains the same? - - break; - - case (PACKET_CRC_TWO): - transmit_state = HEADER_117; - transmit_crc_two = calcCrcTwoArray(input_packet, 4); //calc CRC 2 from the input packet (size 4) - - //check if CRC TWO is correct (then send full packet) - if (incoming_byte == transmit_crc_two) { - requested_packet_number = input_packet[2] * 256 + input_packet[3]; //compute the numbers 0 through 65535 with the two bytes - - if (requested_packet_number != _previous_reply_byte) { //CHANGE THE NAME TO SOMETHING NOT BYTE! - //MUST SET THE PACKET NUMBER - _packet_number = requested_packet_number; - - // DO NOT READ IF THE CURRENT REQUEST IS PAST THE TOTAL # OF PACKETS (lines of the file) - if (_packet_number <= _total_number_of_packets) { - readPacketInSeries(); - createDataPacket(); - } - - _previous_reply_byte = requested_packet_number; //RECORD THIS BYTE to prevent new packets being created - - transmitDataPacket(); //continuously transmit current packet until it tells you to do the next packet (response from Python program) - } - - else - transmitDataPacket(); //if you get the same packet request, send the same packet - - } //end of checksum (incoming_byte) if statement - - break; - } //switch statement complete - } //while statement complete + pc().printf("########################\n"); + //pc().printf("%d", pc().getc()); + } + +// static int transmit_state = HEADER_117; //state in switch statement +// //int incoming_byte = -1; //reset each time a character is read +// int requested_packet_number = -1; //reset each time a character is read +// static int input_packet[4]; //changed from char in previous iteration 03/28/2018 +// static int transmit_crc_one = 0; //hold crc values until they're reset with calculations +// static int transmit_crc_two = 0; +// +// static int incoming_byte[6]; +// +// static int bytes_received = 0; +// +// int req_packet_number = -1; +// +// if (pc().readable()) { +// incoming_byte[bytes_received] = pc().getc(); +// pc().printf("<%d> ", incoming_byte[bytes_received]); +// +// bytes_received++; +// +// if (bytes_received > 5) { +// +// req_packet_number = incoming_byte[2] * 256 + incoming_byte[3]; +// +// pc().printf("req_packet_number = %d\n\r", req_packet_number); +// +// //reset +// bytes_received = 0; +// } +// } +// +// //setTransmitPacketNumber(0); +// +// /* OPEN THE FILE */ +// string file_name_string = _file_system_string + "LOG000.csv"; + //_fp = fopen(file_name_string.c_str(), "r"); + + /* RECEIVE CORRECT CHECKSUM, SEND PACKET */ + //transmitPacketNumber(req_packet_number); + + //fclose(_fp); + +// switch(transmit_state) { +// case HEADER_117: +// led3() = !led3(); +// +// //continue processing +// if (incoming_byte == 117){ //"u" +// transmit_state = HEADER_101; +// } +// //did not receive byte 1 +// else { +// transmit_state = HEADER_117; //go back to checking the first packet +// } +// break; +// +// case HEADER_101: +// if(incoming_byte == 101) { //"e" +// transmit_state = TRANSMIT_PACKET_1; +// //pc().printf(" U E \n\r"); +// } +// else { +// transmit_state = HEADER_117; +// } +// break; +// +// case TRANSMIT_PACKET_1: +// if (incoming_byte >= 0) { +// input_packet[0] = 117; +// input_packet[1] = 101; +// input_packet[2] = incoming_byte; +// +// transmit_state = TRANSMIT_PACKET_2; +// //_reply_byte3 = incoming_byte; +// +// led1() = !led1(); +// //pc().printf(" T P \n\r"); +// //pc().printf("DEBUG: Transmit Packet %d\n\r", incoming_byte); +// } +// break; +// +// case TRANSMIT_PACKET_2: +// +// if (incoming_byte >= 0) { +// input_packet[3] = incoming_byte; +// //_reply_byte4 = incoming_byte; +// } +// transmit_state = PACKET_CRC_ONE; +// +// break; +// +// case (PACKET_CRC_ONE): +// transmit_crc_one = calcCrcOneArray(input_packet, 4); //calc CRC 1 from the input packet (size 4) +// +// if (incoming_byte == transmit_crc_one) { +// transmit_state = PACKET_CRC_TWO; +// } +// +// else +// transmit_state = HEADER_117; +// //or state remains the same? +// +// break; +// +// case (PACKET_CRC_TWO): +// transmit_state = HEADER_117; +// transmit_crc_two = calcCrcTwoArray(input_packet, 4); //calc CRC 2 from the input packet (size 4) +// +// //check if CRC TWO is correct (then send full packet) +// if (incoming_byte == transmit_crc_two) { +// requested_packet_number = input_packet[2] * 256 + input_packet[3]; //compute the numbers 0 through 65535 with the two bytes +// +// //receive correct checksum, immediately send this packet +// transmitPacketNumber(requested_packet_number); +// +// //fseek(_fp, 0, SEEK_END); //reset _fp (this was causing errors with the other function) +// led3() = !led3(); +// +// } //end of checksum (incoming_byte) if statement +// +// break; +// } //switch statement complete +// } //while statement complete } int MbedLogger::readTransmitPacket() { @@ -642,6 +745,24 @@ return false; } +bool MbedLogger::endTransmitPacket() { + + int incoming_byte; + + while (pc().readable()) { + incoming_byte = pc().getc(); //get first byte + + if (incoming_byte == 16) { + incoming_byte = pc().getc(); //get second byte + return true; + } + //quick and dirty + + } + + return false; +} + void MbedLogger::reOpenLineReader() { //open a new one string file_name_string = _file_system_string + "LOG000.csv"; @@ -685,35 +806,20 @@ } int MbedLogger::getNumberOfPacketsInCurrentLog() { - _packet_number = 0; - - int ch; + //takes less than a second to complete, verified 7/24/2018 + + //open the file + string file_name_string = _file_system_string + "LOG000.csv"; + _fp = fopen(file_name_string.c_str(), "r"); - _total_number_of_packets = 0; //clear this each time you read the file - - //if this is null, use the default, else use the new file - - if (_fp == NULL) { - string file_name_string = _file_system_string + "LOG000.csv"; + fseek(_fp, 0L, SEEK_END); + + size_t size = ftell(_fp); - _fp = fopen(file_name_string.c_str(), "r"); //open the log file to read - } - else { //else, use the file that is already open... - fseek(_fp, 0, SEEK_SET); // SEEK_SET is the beginning of file - } - - //counts the number of newline characters to generate the packet size - while (EOF != (ch=getc(_fp))) { - if ('\n' == ch) - _total_number_of_packets++; // records the number of new lines to determine how many packets to send - } - //move the FILE pointer back to the start fseek(_fp, 0, SEEK_SET); // SEEK_SET is the beginning of file - _file_transmission = true; //preparing to transmit files from MBED to Python - - //pc().printf("debug: _total_number_of_packets: %d\n\r",_total_number_of_packets); + _total_number_of_packets = size/254; return _total_number_of_packets; } @@ -1161,7 +1267,7 @@ unsigned int file_size = ftell(_fp); //For binary streams, this is the number of bytes from the beginning of the file. fseek(_fp, 0, SEEK_SET); //SEEK_SET is hte beginning of the file, not sure this is necessary - closeLogFile(); //can probably just close the file pointer and not worry about position + //closeLogFile(); //can probably just close the file pointer and not worry about position pc().printf("%s file size is %d\n\r", filename.c_str(), file_size); @@ -1336,10 +1442,6 @@ } } -int MbedLogger::getNumberOfPackets() { - return _total_number_of_packets; -} - //only do this for the MBED because of the limited file size //write one line to the file (open to write, this will erase all other data) and close it. void MbedLogger::eraseFile() { @@ -1533,6 +1635,8 @@ //also needed to reset a boolean flag on the transmit _fsm_transmit_complete = false; + + _end_transmit_packet = false; } int MbedLogger::currentPacketNumber() {