diff --git a/.travis.yml b/.travis.yml index b407dc6..719f5c3 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,13 +5,13 @@ sudo: required addons: apt: sources: - - ubuntu-toolchain-r-test + - ubuntu-toolchain-r-test packages: - - autoconf - - automake - - m4 - - gcc-4.9 - - g++-4.9 + - autoconf + - automake + - m4 + - gcc-4.9 + - g++-4.9 before_install: - git submodule update --init --recursive > /dev/null @@ -20,8 +20,29 @@ before_install: install: - "bash -ex ./install-wiringpi.sh" +before_script: + - "bash -ex ./prepare.sh" + - export TZ=UTC + script: - - "bash -ex ./build.sh" + - make utesting && ./utesting + - make clean && ./configure CPPFLAGS="-DDEBUG" && make utesting && ./utesting + - make clean && ./configure CPPFLAGS="-DNO_SMS" && make utesting && ./utesting + - make clean && ./configure CPPFLAGS="-DDEBUG -DNO_SMS" && make utesting && ./utesting + - make clean && make + - make clean && ./configure CPPFLAGS="-DNO_POWER_OFF" && make + - make clean && ./configure CPPFLAGS="-DDEBUG" && make + - make clean && ./configure CPPFLAGS="-DNO_POWER_OFF -DDEBUG" && make + - make clean && ./configure CPPFLAGS="-DNO_SMS" && make + - make clean && ./configure CPPFLAGS="-DDEBUG -DNO_SMS" && make + - make clean && ./configure CPPFLAGS="-DSIM" && make + - make clean && ./configure CPPFLAGS="-DDEBUG -DSIM" && make + - make clean && ./configure CPPFLAGS="-DREAL_SIM" && make + - make clean && ./configure CPPFLAGS="-DDEBUG -DREAL_SIM" && make + - make clean && ./configure CPPFLAGS="-DNO_SMS -DREAL_SIM" && make + - make clean && ./configure CPPFLAGS="-DDEBUG -DNO_SMS -DREAL_SIM" && make + - make clean && ./configure CPPFLAGS="-DNO_SMS -D_SIM" && make + - make clean && ./configure CPPFLAGS="-DDEBUG -DNO_SMS -D_SIM" && make branches: only: @@ -34,8 +55,7 @@ branches: notifications: email: recipients: - - iban.eguia@opendeusto.es - - jordan.aranda@me.com + - razican@protonmail.ch - eneko.cruz@opendeusto.es on_success: change on_failure: always diff --git a/Makefile.am b/Makefile.am index 3be2e86..1020a95 100644 --- a/Makefile.am +++ b/Makefile.am @@ -1,7 +1,7 @@ bin_PROGRAMS = openstratos -openstratos_SOURCES = openstratos.cc utils.cc threads.cc camera/Camera.cc gps/GPS.cc serial/Serial.cc logger/Logger.cc gsm/GSM.cc -openstratos_CPPFLAGS = -std=c++14 +openstratos_SOURCES = openstratos.cc logic/main_while.cc logic/shut_down.cc utils.cc threads.cc camera/Camera.cc gps/GPS.cc serial/Serial.cc logger/Logger.cc gsm/GSM.cc +openstratos_CPPFLAGS = -Wall -Wextra -Werror -pedantic-errors -std=c++14 EXTRA_PROGRAMS = utesting utesting_SOURCES = testing/testing.cc camera/Camera.cc gps/GPS.cc serial/Serial.cc logger/Logger.cc -utesting_CPPFLAGS = -std=c++14 -Itesting/bandit -Wno-unused-result -DOS_TESTING +utesting_CPPFLAGS = -Wall -Wextra -Werror -pedantic-errors -std=c++14 -Itesting/bandit -Wno-unused-parameter -DOS_TESTING diff --git a/README b/README index 0d24b7b..e463aeb 100644 --- a/README +++ b/README @@ -1,15 +1,15 @@ -# OpenStratos Server # +# OpenStratos # -[![Build Status](https://travis-ci.org/OpenStratos/server.svg?branch=develop)](https://travis-ci.org/OpenStratos/server) +[![Build Status](https://travis-ci.org/OpenStratos/server.svg?branch=master)](https://travis-ci.org/OpenStratos/server) -Server implemented in C++14. It will be in charge of the management of the balloon. It will -communicate via SMS to the provided phone. It will initialize all needed components, track and log -position and software messages, detect launch, burst and landing and send the landing position via -SMS. +Balloon control software implemented in C++14. It will be in charge of the management of the +balloon and will communicate via SMS to the provided phone. It will initialize all needed +components, track and log position and software messages, detect launch, burst and landing and send +the landing position via SMS. ## Requirements ## -The software needs a working instalation of WiringPi and a RaspiCam for the tests. It will perform +The software needs a working installation of WiringPi and a RaspiCam for the tests. It will perform some basic tests if no Raspberry Pi is being used. It works with Adafruit Fona module, even though it should work with other GSM modules. It also uses the Adafruit Ultimate GPS module. The following software is needed to compile OpenStratos (apart from the WiringPi library): @@ -23,23 +23,23 @@ software is needed to compile OpenStratos (apart from the WiringPi library): ## Compiling ## For compilation, a *build.sh* script is provided, that should be run as is. It will compile the -tests of OpenStratos and run them. After that, the main program can be compiled using ```make```. +tests of OpenStratos and run them. After that, the main program can be compiled using `make`. Optional configuration arguments can be passed. The first optional configuration argument is the *NO_SMS* flag. This prevents actual SMSs being sent, even if they are simulated. This way no charges -will be applied. For using this argument the directory should be cleaned with ```make clean``` and +will be applied. For using this argument the directory should be cleaned with `make clean` and then pass the *NO_SMS* flag to the configure script: ``` ./configure CPPFLAGS="-DNO_SMS" ``` -After that the usual ```make``` will compile the software. Note that the test do not send SMSs. The software itself comes with two built-in simulation modes: +After that the usual `make` will compile the software. Note that the test do not send SMSs. The software itself comes with two built-in simulation modes: ### Normal Simulation ### In this mode, a simple simulation is made, with a length of about 45 minutes. It will run through all the main stages of the program. For using this mode the directory should be cleaned with -```make clean``` and then pass the *SIM* flag to the configure script: +`make clean` and then pass the *SIM* flag to the configure script: ``` ./configure CPPFLAGS="-DSIM" @@ -51,14 +51,14 @@ It can be combined with the *NO_SMS* flag: ./configure CPPFLAGS="-DSIM -DNO_SMS" ``` -After that, the software can be compiled using ```make```. +After that, the software can be compiled using `make`. ### Realistic Simulation ### In this mode, a complete realistic simulation is made, that will last for about 5 hours. It will realistically simulate the times in a 35 km height balloon. It will be similar to the normal simulation, the only change will be in the timing. For using this mode the directory should be -cleaned with ```make clean``` and then pass the *REAL_SIM* flag to the configure script: +cleaned with `make clean` and then pass the *REAL_SIM* flag to the configure script: ``` ./configure CPPFLAGS="-DREAL_SIM" @@ -70,15 +70,15 @@ It can be combined with the *NO_SMS* flag: ./configure CPPFLAGS="-DREAL_SIM -DNO_SMS" ``` -After that, the software can be compiled using ```make```. The result of combining the two -simulation flags is undetermined. They should not be combined. +After that, the software can be compiled using `make`. The result of combining the two simulation +flags will be an error when building the main program. For testing it will have no effect. ### Debug Mode ### The software has a small debug mode, that prints from *stdio* some logs that occur before the log file is created. It also enables serial logging, that will log everything that happens in the -serial. This has a moderate overhead and should not be used in production. GSM and GPS loggers log everythong they send and receive. This is only needed to debug if something goes wrong with the -serial. For using this mode the directory should be cleaned with ```make clean``` and then pass the +serial. This has a moderate overhead and should not be used in production. GSM and GPS loggers log everything they send and receive. This is only needed to debug if something goes wrong with the +serial. For using this mode the directory should be cleaned with `make clean` and then pass the *DEBUG* flag to the configure script: ``` @@ -97,7 +97,7 @@ It can be combined with the *NO_SMS* flag or/and one of the simulation flags: For some testing there is no need to reboot or shut down the system in failures. For this, the software provides the no power off mode, that will prevent the software from shutting the -Raspberry Pi down. For using this mode the directory should be cleaned with ```make clean``` and +Raspberry Pi down. For using this mode the directory should be cleaned with `make clean` and then pass the *NO_POWER_OFF* flag to the configure script: ``` @@ -110,6 +110,7 @@ It can be combined with the *NO_SMS* flag, the *DEBUG* flag or/and one of the si ./configure CPPFLAGS="-DREAL_SIM -DNO_SMS -DDEBUG -DNO_POWER_OFF" ./configure CPPFLAGS="-DSIM -DDEBUG -DNO_POWER_OFF" ./configure CPPFLAGS="-DNO_SMS -DDEBUG -DNO_POWER_OFF" +``` ## License ## diff --git a/camera/Camera.cc b/camera/Camera.cc index 3d66d5a..cfbdf6f 100644 --- a/camera/Camera.cc +++ b/camera/Camera.cc @@ -41,9 +41,13 @@ Camera::~Camera() { this->logger->log("Stopping video recording..."); if ( ! this->stop()) - this->logger->log("Error soping video recording."); + { + this->logger->log("Error stoping video recording."); + } else + { this->logger->log("Video recording stopped."); + } } this->logger->log("Shut down finished"); delete this->logger; @@ -59,7 +63,10 @@ void Camera::record_thread(int time) bool Camera::record(int time) { - if (time != 0) this->logger->log("Recording for "+to_string(time/1000)+" seconds..."); + if (time != 0) + { + this->logger->log("Recording for "+to_string(time/1000)+" seconds..."); + } if ( ! this->recording) { this->logger->log("Not already recording, creating command..."); @@ -91,11 +98,18 @@ bool Camera::record(int time) t.detach(); } - if (result) this->logger->log("Video recording correctly started."); - else this->logger->log("Error starting video recording."); + if (result) + { + this->logger->log("Video recording correctly started."); + } + else + { + this->logger->log("Error starting video recording."); + } return result; } + return false; } bool Camera::record() @@ -107,21 +121,25 @@ bool Camera::record() bool Camera::take_picture(const string& exif) { bool was_recording = this->recording; - if (was_recording) this->logger->log("Recording video, stopping..."); - if (was_recording && ! this->stop()) return false; - this->logger->log("Video recording stopped."); + this->logger->log("Taking picture..."); + if (was_recording) + { + this->logger->log("The video is recording."); + } + if (was_recording && ! this->stop()) + { + return false; + } string filename = "data/img/img-"+ to_string(get_file_count("data/img/")) +".jpg"; #ifdef OS_TESTING filename = "data/img/test.jpg"; #endif - string exif_command = exif != "" ? " -x "+ exif : ""; - - string command = "raspistill -n -o "+ filename +" " + (PHOTO_RAW ? "-r" : "") + " -w "+ to_string(PHOTO_WIDTH) + string command = "raspistill -n -t 1 -o "+ filename +" " + (PHOTO_RAW ? "-r" : "") + " -w "+ to_string(PHOTO_WIDTH) +" -h "+ to_string(PHOTO_HEIGHT) +" -q "+ to_string(PHOTO_QUALITY) +" -co "+ to_string(PHOTO_CONTRAST) +" -br "+ to_string(PHOTO_BRIGHTNESS) - +" -ex "+ PHOTO_EXPOSURE + exif_command; + +" -ex "+ PHOTO_EXPOSURE + exif; this->logger->log("Picture command: '"+command+"'"); @@ -133,10 +151,19 @@ bool Camera::take_picture(const string& exif) int st = system(command.c_str()); bool result = st == 0; - if (result) this->logger->log("Picture taken correctly."); - else this->logger->log("Error taking picture."); + if (result) + { + this->logger->log("Picture taken correctly."); + } + else + { + this->logger->log("Error taking picture."); + } - if (was_recording) this->logger->log("Video recording was active before taking picture. Resuming..."); + if (was_recording) + { + this->logger->log("Video recording was active before taking picture. Resuming..."); + } if (was_recording && ! this->record()) { this->logger->log("Error resuming video recording."); @@ -159,6 +186,7 @@ bool Camera::stop() if (system("pkill raspivid") == 0) { this->logger->log("Video recording stopped correctly."); + this_thread::sleep_for(50ms); this->recording = false; return true; } @@ -166,13 +194,14 @@ bool Camera::stop() if ( ! this->is_really_recording()) { - this->logger->log("Warning: video was already stopped."); + this->logger->log("Warning: video had already stopped."); this->recording = false; return true; } return false; #else this->logger->log("Test mode. Video recording stop simulated."); + this_thread::sleep_for(50ms); this->recording = false; return true; #endif @@ -190,8 +219,11 @@ int os::get_file_count(const string& path) struct dirent *ep; dp = opendir(path.c_str()); - while (ep = readdir(dp)) i++; - (void) closedir(dp); + while ((ep = readdir(dp)) != NULL) + { + ++i; + } + closedir(dp); return i-2; } @@ -199,8 +231,12 @@ int os::get_file_count(const string& path) const string os::generate_exif_data() { string exif; - while (GPS::get_instance().get_PDOP() > 5) - this_thread::sleep_for(1s); + for (int i = 0; + i < 10 && ( ! GPS::get_instance().is_fixed() || GPS::get_instance().get_PDOP() > MAX_DOP); + ++i) + { + this_thread::sleep_for(500ms); + } double gps_lat = GPS::get_instance().get_latitude(); double gps_lon = GPS::get_instance().get_longitude(); @@ -209,14 +245,22 @@ const string os::generate_exif_data() float gps_pdop = GPS::get_instance().get_PDOP(); euc_vec gps_velocity = GPS::get_instance().get_velocity(); - exif += "GPSLatitudeRef="+to_string(gps_lat > 0 ? 'N' : 'S'); - exif += " GPSLatitude="+to_string(abs((int) gps_lat*1000000))+"/1000000,0/1,0/1"; - exif += " GPSLongitudeRef="+to_string(gps_lon > 0 ? 'E' : 'W'); - exif += " GPSLongitude="+to_string(abs((int) gps_lon*1000000))+"/1000000,0/1,0/1"; - exif += " GPSAltitudeRef=0 GPSAltitude="+to_string(gps_alt); - exif += " GPSSatellites="+to_string(gps_sat); - exif += " GPSDOP="+to_string(gps_pdop); - exif += " GPSSpeedRef=K GPSSpeed="+to_string(gps_velocity.speed*3.6); - exif += " GPSTrackRef=T GPSTrack="+to_string(gps_velocity.course); - exif += " GPSDifferential=0"; + exif += " -x GPS.GPSLatitudeRef="+string(gps_lat > 0 ? "N" : "S"); + exif += " -x GPS.GPSLatitude="+to_string( + abs((int) (gps_lat*1000000)) + )+"/1000000"; + exif += " -x GPS.GPSLongitudeRef="+string(gps_lat > 0 ? "E" : "W"); + exif += " -x GPS.GPSLongitude="+to_string( + abs((int) (gps_lon*1000000)) + )+"/1000000"; + exif += " -x GPS.GPSAltitudeRef=0 -x GPS.GPSAltitude="+to_string((int) (gps_alt*100))+"/100"; + exif += " -x GPS.GPSSatellites="+to_string(gps_sat); + exif += " -x GPS.GPSDOP="+to_string((int) (gps_pdop*1000))+"/1000"; + exif += " -x GPS.GPSSpeedRef=K -x GPS.GPSSpeed="+ + to_string((int) (gps_velocity.speed*3.6*1000))+"/1000"; + exif += " -x GPS.GPSTrackRef=T -x GPS.GPSTrack="+ + to_string((int) (gps_velocity.course*1000))+"/1000"; + exif += " -x GPS.GPSDifferential=0"; + + return exif; } diff --git a/configure.ac b/configure.ac index d35f714..5db8922 100644 --- a/configure.ac +++ b/configure.ac @@ -2,7 +2,7 @@ # Process this file with autoconf to produce a configure script. AC_PREREQ([2.68]) -AC_INIT(OpenStratos, 1.0, https://github.com/OpenStratos/server/issues) +AC_INIT(OpenStratos, 1.1.0, https://github.com/OpenStratos/server/issues) AC_CONFIG_SRCDIR([openstratos.cc]) AM_INIT_AUTOMAKE([subdir-objects]) AC_CONFIG_HEADERS([config.h]) diff --git a/constants.h b/constants.h index d94956a..d82e622 100644 --- a/constants.h +++ b/constants.h @@ -1,7 +1,10 @@ #ifndef CONSTANTS_H_ #define CONSTANTS_H_ - #define FLIGHT_LENGTH 4.4333 // Hours + #define FLIGHT_LENGTH 1.1 // Hours + #define FLIGHT_MAX_HEIGHT 12500 // Meters + #define ASCENT_VELOCITY 5.74 // m/s + #define DESCENT_VELOCITY FLIGHT_MAX_HEIGHT/(FLIGHT_LENGTH*3600-FLIGHT_MAX_HEIGHT/ASCENT_VELOCITY) // m/s #define BAT_GSM_MAX 4.2 #define BAT_GSM_MIN 3.7 @@ -16,9 +19,9 @@ #define VIDEO_BRIGHTNESS 50 #define VIDEO_EXPOSURE "antishake" - #define PHOTO_WIDTH 2592 - #define PHOTO_HEIGHT 1944 - #define PHOTO_QUALITY 90 + #define PHOTO_WIDTH 3280 + #define PHOTO_HEIGHT 2464 + #define PHOTO_QUALITY 98 #define PHOTO_RAW true #define PHOTO_CONTRAST 50 #define PHOTO_BRIGHTNESS 50 @@ -29,6 +32,13 @@ #define GPS_BAUDRATE 9600 #define GPS_ENDL "\r\n" + #define IDEAL_DOP 1 + #define EXCELENT_DOP 2 + #define GOOD_DOP 5 + #define MODERATE_DOP 10 + #define FAIR_DOP 20 + #define MAX_DOP MODERATE_DOP + #define GSM_LOC_SERV "gprs-service.com" #define GSM_UART "/dev/ttyUSB0" #define GSM_PWR_GPIO 7 diff --git a/gps/GPS.cc b/gps/GPS.cc index 3d14fc7..2056718 100644 --- a/gps/GPS.cc +++ b/gps/GPS.cc @@ -32,7 +32,10 @@ GPS::~GPS() { this->logger->log("Stopping GPS thread..."); this->should_stop = true; - while ( ! this->stopped) this_thread::sleep_for(1ms); + while ( ! this->stopped) + { + this_thread::sleep_for(1ms); + } this->logger->log("GPS thread stopped"); } @@ -43,11 +46,11 @@ GPS::~GPS() this->logger->log("Serial interface closed."); this->logger->log("Deallocating serial..."); delete this->serial; - this->logger->log("Serial deallocated"); + this->logger->log("Serial deallocated."); this->logger->log("Deallocating frame logger..."); delete this->frame_logger; - this->logger->log("Frame logger deallocated"); + this->logger->log("Frame logger deallocated."); } this->logger->log("Turning off GPS..."); this->turn_off(); @@ -61,74 +64,119 @@ bool GPS::initialize() gettimeofday(&timer, NULL); struct tm * now = gmtime(&timer.tv_sec); - this->logger = new Logger("data/logs/GPS/GPS."+ to_string(now->tm_year+1900) +"-"+ to_string(now->tm_mon) +"-"+ + this->logger = new Logger("data/logs/GPS/GPS."+ to_string(now->tm_year+1900) +"-"+ to_string(now->tm_mon+1) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ to_string(now->tm_min) +"-"+ to_string(now->tm_sec) +".log", "GPS"); this->frame_logger = new Logger("data/logs/GPS/GPSFrames."+ to_string(now->tm_year+1900) +"-"+ - to_string(now->tm_mon) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ + to_string(now->tm_mon+1) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ to_string(now->tm_min) +"-"+ to_string(now->tm_sec) +".log", "GPSFrame"); this->should_stop = false; this->stopped = true; + this->time = timer; + #ifndef OS_TESTING pinMode(GPS_ENABLE_GPIO, OUTPUT); + if (this->is_on()) + { + this->logger->log("GPS is on, turning off for 2 seconds for stability"); + this->turn_off(); + this_thread::sleep_for(2s); + } + this->logger->log("Turning GPS on..."); this->turn_on(); this->logger->log("GPS on."); #endif this->logger->log("Starting serial connection..."); - this->serial = new Serial(GPS_UART, GPS_BAUDRATE, "GPS"); - if ( ! this->serial->is_open()) { + #ifdef DEBUG + this->serial = new Serial(GPS_UART, GPS_BAUDRATE, "GPS"); + #else + this->serial = new Serial(GPS_UART, GPS_BAUDRATE); + #endif + if ( ! this->serial->is_open()) + { this->logger->log("GPS serial error."); return false; } this->logger->log("Serial connection started."); - this->logger->log("Starting GPS frame thread..."); - thread t(&GPS::gps_thread, this); - t.detach(); - this->logger->log("GPS frame thread running."); + this->logger->log("Setting GPS to pedestrian mode"); + this->enter_pedestrian_mode(); #ifndef OS_TESTING this->logger->log("Sending configuration frames..."); - this->serial->println("$PMTK220,100*2F"); - this->frame_logger->log("Sent: $PMTK220,100*2F"); - this->serial->println("$PMTK314,0,1,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29"); - this->frame_logger->log("Sent: $PMTK314,0,1,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29"); + + vector messages[] = + { + // Set refresh: + { 0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0x64, 0x00, 0x01, 0x00, 0x01, 0x00, 0x7A, 0x12 }, + // Disable GSV: + { 0xB5, 0x62, 0x05, 0x01, 0x02, 0x00, 0x06, 0x01, 0x0F, 0x38 }, + // Disable VTG: + { 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11 }, + // Disable GLL: + { 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15 }, + // Disable ZDA: + { 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19 }, + }; + + for (vector mes: messages) + { + for(uint8_t tries = 0; tries<100; tries++) + { + for (unsigned char c: mes) + { + this->serial->write(c); + } + this_thread::sleep_for(10ms); + } + } + this->logger->log("Configuration frames sent."); #endif + this->logger->log("Starting GPS frame thread..."); + thread t(&GPS::gps_thread, this); + t.detach(); + this->logger->log("GPS frame thread running."); + return true; } +bool GPS::is_on() const +{ + return digitalRead(GPS_ENABLE_GPIO) == HIGH; +} + bool GPS::turn_on() const { - if (digitalRead(GPS_ENABLE_GPIO) == LOW) + if ( ! this->is_on()) { digitalWrite(GPS_ENABLE_GPIO, HIGH); return true; } else { - this->logger->log("Error: Turning on GPS but GPS already on."); + this->logger->log("Warning: Turning on GPS but GPS already on."); return false; } } bool GPS::turn_off() const { - if (digitalRead(GPS_ENABLE_GPIO) == HIGH) + if (this->is_on()) { digitalWrite(GPS_ENABLE_GPIO, LOW); return true; } else { - this->logger->log("Error: Turning off GPS but GPS already off."); + this->logger->log("Warning: Turning off GPS but GPS already off."); return false; } } @@ -145,11 +193,13 @@ void GPS::gps_thread() if (available > 0) { - for (int i = 0; i < available; i++) + for (int i = 0; i < available; ++i) { char c = this->serial->read_char(); response += c; - if (response[response.length()-2] == '\r' && c == '\n') + if (response.length() > 2 && + response[response.length()-2] == '\r' && + c == '\n') { response = response.substr(0, response.length()-2); @@ -181,13 +231,22 @@ void GPS::gps_thread() bool GPS::is_valid(string frame) { regex frame_regex("\\$[A-Z][0-9A-Z\\.,-]*\\*[0-9A-F]{1,2}"); - if ( ! regex_match(frame, frame_regex)) return false; + if ( ! regex_match(frame, frame_regex)) + { + return false; + } uint_fast8_t checksum = 0; for (char c : frame) { - if (c == '$') continue; - if (c == '*') break; + if (c == '$') + { + continue; + } + if (c == '*') + { + break; + } checksum ^= c; } @@ -198,22 +257,43 @@ bool GPS::is_valid(string frame) void GPS::parse(const string& frame) { - if (frame.length() > 1 && is_valid(frame)) + if (frame.length() > 1 && this->is_valid(frame)) { + struct timeval os_clock; + gettimeofday(&os_clock, NULL); + double os_time = (double) os_clock.tv_sec + os_clock.tv_usec * 0.000001; + this->frame_logger->log(frame); - string frame_type = frame.substr(1, frame.find_first_of(',')-1); + string frame_type = frame.substr(3, frame.find_first_of(',')-3); - if (frame_type == "GPGGA") + try { - this->parse_GGA(frame); + if (frame_type == "GGA") + { + this->parse_GGA(frame); + } + else if (frame_type == "GSA") + { + this->parse_GSA(frame); + } + else if (frame_type == "RMC") + { + this->parse_RMC(frame); + } } - else if (frame_type == "GPGSA") + catch (const invalid_argument& ia) { - this->parse_GSA(frame); + this->logger->log("Failed to parse frame: " + frame); } - else if (frame_type == "GPRMC") + + double gps_time = (double) this->time.tv_sec + this->time.tv_usec * 0.000001; + if (this->active && (gps_time > os_time + 3 || gps_time < os_time - 3)) { - this->parse_RMC(frame); + #ifndef OS_TESTING + settimeofday(&this->time, NULL); + #endif + + this->logger->log("Date changed."); } } } @@ -224,33 +304,90 @@ void GPS::parse_GGA(const string& frame) string data; vector s_data; + string lat, lat_dec, lon, lon_dec; + // We put all fields in a vector - while(getline(ss, data, ',')) s_data.push_back(data); + while(getline(ss, data, ',')) + { + s_data.push_back(data); + } + + if (s_data.size() != 15) + { + this->logger->log("Failed to parse frame: " + frame); + return; + } // Is the data valid? - this->active = s_data[6] > "0"; + bool active = s_data[6] == "1" || s_data[6] == "2"; + if (this->active && ! active) + { + this->logger->log("Fix lost."); + } + else if ( ! this->active && active) + { + this->logger->log("Fix acquired."); + } + this->active = active; + + if (s_data[1].length() >= 6) // Update time + { + tm* time = gmtime(&this->time.tv_sec); + time->tm_hour = stoi(s_data[1].substr(0, 2)); + time->tm_min = stoi(s_data[1].substr(2, 2)); + time->tm_sec = stoi(s_data[1].substr(4, 2)); + + this->time.tv_sec = mktime(time); + this->time.tv_usec = s_data[1].length() > 6 ? stoi(s_data[1].substr(7, 2))*10000 : 0; + } if (this->active) { - // Update time - this->time.tm_hour = stoi(s_data[1].substr(0, 2)); - this->time.tm_min = stoi(s_data[1].substr(2, 2)); - this->time.tm_sec = stoi(s_data[1].substr(4, 2)); + if (s_data[2].length() > 2) + { + lat = s_data[2].substr(0, 2); + lat_dec = s_data[2].substr(2, s_data[2].length()-2); - // Update latitude - this->latitude = stoi(s_data[2].substr(0, 2)); - this->latitude += stof(s_data[2].substr(2, s_data[2].length()-2))/60; - if (s_data[3] == "S") this->latitude *= -1; + this->latitude = stoi(lat); + if( ! lat_dec.empty()) + { + this->latitude += stof(lat_dec)/60; + } + if (s_data[3] == "S") + { + this->latitude *= -1; + } + } - // Update longitude - this->longitude = stoi(s_data[4].substr(0, 3)); - this->longitude += stof(s_data[4].substr(3, s_data[4].length()-3))/60; - if (s_data[5] == "W") this->longitude *= -1; + if (s_data[4].length() > 3) + { + lon = s_data[4].substr(0, 3); + lon_dec = s_data[4].substr(3, s_data[4].length()-3); - // Update the rest of the GGA data - this->satellites = stoi(s_data[7]); - this->hdop = stof(s_data[8]); - this->altitude = stod(s_data[9]); + this->longitude = stoi(lon); + if( ! lon_dec.empty()) + { + this->longitude += stof(lon_dec)/60; + } + if (s_data[5] == "W") + { + this->longitude *= -1; + } + } + + // Validate and update the rest of the GGA data + if( ! s_data[7].empty()) + { + this->satellites = stoi(s_data[7]); + } + if( ! s_data[8].empty()) + { + this->hdop = stof(s_data[8]); + } + if( ! s_data[9].empty()) + { + this->altitude = stod(s_data[9]); + } } } @@ -261,17 +398,47 @@ void GPS::parse_GSA(const string& frame) vector s_data; // We put all fields in a vector - while(getline(ss, data, ',')) s_data.push_back(data); + while(getline(ss, data, ',')) + { + s_data.push_back(data); + } + + if (s_data.size() != 18) + { + this->logger->log("Failed to parse frame: " + frame); + return; + } // Is the data valid? - this->active = s_data[2] != "1"; + bool active = s_data[2] == "2" || s_data[2] == "3"; + + if (this->active && ! active) + { + this->logger->log("Fix lost."); + this->active = false; + } + else if ( ! this->active && active) + { + this->logger->log("Fix acquired."); + this->active = true; + } if (this->active) { // Update DOP - this->pdop = stof(s_data[15]); - this->hdop = stof(s_data[16]); - this->vdop = stof(s_data[17].substr(0, s_data[17].find_first_of('*'))); + if( ! s_data[15].empty()) + { + this->pdop = stof(s_data[15]); + } + if( ! s_data[16].empty()) + { + this->hdop = stof(s_data[16]); + } + string vdop = s_data[17].substr(0, s_data[17].find_first_of('*')); + if( ! vdop.empty()) + { + this->vdop = stof(vdop); + } } } @@ -281,35 +448,317 @@ void GPS::parse_RMC(const string& frame) string data; vector s_data; + string lat, lat_dec, lon, lon_dec; + // We put all fields in a vector - while(getline(ss, data, ',')) s_data.push_back(data); + while(getline(ss, data, ',')) + { + s_data.push_back(data); + } + + if (s_data.size() != 13) + { + this->logger->log("Failed to parse frame: " + frame); + return; + } // Is the data valid? - this->active = s_data[2] == "A"; + bool active = s_data[2] == "A"; + + if (this->active && ! active) + { + this->logger->log("Fix lost."); + this->active = false; + } + else if ( ! this->active && active) + { + this->logger->log("Fix acquired"); + this->active = true; + } + + if (s_data[1].length() >= 6) // Update time + { + tm* time = gmtime(&this->time.tv_sec); + time->tm_hour = stoi(s_data[1].substr(0, 2)); + time->tm_min = stoi(s_data[1].substr(2, 2)); + time->tm_sec = stoi(s_data[1].substr(4, 2)); + + if (s_data[9].length() == 6) // Update date + { + time->tm_mday = stoi(s_data[9].substr(0, 2)); + time->tm_mon = stoi(s_data[9].substr(2, 2))-1; + time->tm_year = stoi(s_data[9].substr(4, 2))+100; + } + + this->time.tv_sec = mktime(time); + this->time.tv_usec = s_data[1].length() > 6 ? stoi(s_data[1].substr(7, 2))*10000 : 0; + } + else if (s_data[9].length() == 6) // Update date + { + tm* time = gmtime(&this->time.tv_sec); + + time->tm_mday = stoi(s_data[9].substr(0, 2)); + time->tm_mon = stoi(s_data[9].substr(2, 2))-1; + time->tm_year = stoi(s_data[9].substr(4, 2))+100; + + this->time.tv_sec = mktime(time); + } if (this->active) { - // Update date and time - this->time.tm_hour = stoi(s_data[1].substr(0, 2)); - this->time.tm_min = stoi(s_data[1].substr(2, 2)); - this->time.tm_sec = stoi(s_data[1].substr(4, 2)); + if(s_data[3].length() > 2) + { + lat = s_data[3].substr(0, 2); + lat_dec = s_data[3].substr(2, s_data[3].length()-2); + + this->latitude = stoi(lat); + if( ! lat_dec.empty()) + { + this->latitude += stof(lat_dec)/60; + } + if (s_data[4] == "S") + { + this->latitude *= -1; + } + } + + if(s_data[5].length() > 3) + { + lon = s_data[5].substr(0, 3); + lon_dec = s_data[5].substr(3, s_data[5].length()-3); + + this->longitude = stoi(lon); + if( ! lon_dec.empty()) + { + this->longitude += stof(lon_dec)/60; + } + if (s_data[6] == "W") + { + this->longitude *= -1; + } + } + + // Check if non-empty, and if so, update velocity + if( ! s_data[7].empty()) + { + this->velocity.speed = kt_to_mps(stof(s_data[7])); + } + if( ! s_data[8].empty()) + { + this->velocity.course = stof(s_data[8]); + } + } +} + +void GPS::enter_airborne_1g_mode() +{ + int gps_dynamic_model_set_success = 0; + struct timeval time_now, time_start; + long ms_now, ms_start; + + vector setdm6 = // Byte at offset 2 determines new operation mode. + { + 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, + 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, + 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, + 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC + }; + + gettimeofday(&time_start, NULL); + ms_start = (long)((time_start.tv_sec)*1000 + (time_start.tv_usec)/1000); + ms_now = ms_start; + + // Prevent lock and timeout if not set after six seconds + while( ! gps_dynamic_model_set_success && (ms_now - ms_start)<6000) + { + gettimeofday(&time_now, NULL); + ms_now = (long)((time_now.tv_sec)*1000 + (time_now.tv_usec)/1000); - this->time.tm_mday = stoi(s_data[9].substr(0, 2)); - this->time.tm_mon = stoi(s_data[9].substr(2, 2))-1; - this->time.tm_year = stoi(s_data[9].substr(4, 2))+100; + this->send_ublox_packet(setdm6); + gps_dynamic_model_set_success = this->receive_check_ublox_ack(setdm6); + } - // Update latitude - this->latitude = stoi(s_data[3].substr(0, 2)); - this->latitude += stof(s_data[3].substr(2, s_data[3].length()-2))/60; - if (s_data[4] == "S") this->latitude *= -1; + if (gps_dynamic_model_set_success) + { + this->logger->log("GPS entered airborne (<1g) mode successfully"); + } + else + { + this->logger->log("GPS failed to enter airborne (<1g) mode"); + } +} - // Update longitude - this->longitude = stoi(s_data[5].substr(0, 3)); - this->longitude += stof(s_data[5].substr(3, s_data[5].length()-3))/60; - if (s_data[6] == "W") this->longitude *= -1; +void GPS::enter_stationary_mode() +{ + int gps_dynamic_model_set_success = 0; + struct timeval time_now, time_start; + long ms_now, ms_start; - // Update velocity - this->velocity.speed = kt_to_mps(stof(s_data[7])); - this->velocity.course = stof(s_data[8]); + vector setdm2 = // Byte at offset 2 determines new operation mode. + { + 0xB5, 0x62, 0x02, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, + 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, + 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, + 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC + }; + + gettimeofday(&time_start, NULL); + ms_start = (long)((time_start.tv_sec)*1000 + (time_start.tv_usec)/1000); + ms_now = ms_start; + + // Prevent lock and timeout if not set after six seconds + while( ! gps_dynamic_model_set_success && (ms_now - ms_start)<6000) + { + gettimeofday(&time_now, NULL); + ms_now = (long)((time_now.tv_sec)*1000 + (time_now.tv_usec)/1000); + + this->send_ublox_packet(setdm2); + gps_dynamic_model_set_success = this->receive_check_ublox_ack(setdm2); } + + if (gps_dynamic_model_set_success) + { + this->logger->log("GPS entered stationary mode successfully"); + } + else + { + this->logger->log("GPS failed to enter stationary mode"); + } +} + +void GPS::enter_pedestrian_mode() +{ + int gps_dynamic_model_set_success = 0; + struct timeval time_now, time_start; + long ms_now, ms_start; + + vector setdm3 = // Byte at offset 2 determines new operation mode. + { + 0xB5, 0x62, 0x03, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, + 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, + 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, + 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC + }; + + gettimeofday(&time_start, NULL); + ms_start = (long)((time_start.tv_sec)*1000 + (time_start.tv_usec)/1000); + ms_now = ms_start; + + // Prevent lock and timeout if not set after six seconds + while( ! gps_dynamic_model_set_success && (ms_now - ms_start)<6000) + { + gettimeofday(&time_now, NULL); + ms_now = (long)((time_now.tv_sec)*1000 + (time_now.tv_usec)/1000); + + this->send_ublox_packet(setdm3); + gps_dynamic_model_set_success = this->receive_check_ublox_ack(setdm3); + } + + if (gps_dynamic_model_set_success) + { + this->logger->log("GPS entered pedestrian mode successfully"); + } + else + { + this->logger->log("GPS failed to enter pedestrian mode"); + } +} + +void GPS::send_ublox_packet(vector message) +{ + if (this->serial->is_open()) + { + this->serial->flush(); + this->serial->write((unsigned char) 0xFF); + this_thread::sleep_for(500ms); + this->serial->write_vec(message); + } + else + { + this->logger->log("Tried to send a packet, but the serial is closed"); + } +} + +bool GPS::receive_check_ublox_ack(vector message) +{ + unsigned char ack_packet[10]; + unsigned int bytes_ordered; + unsigned char byte; + struct timeval time_now, time_start; + long ms_now, ms_start; + + ack_packet[0] = 0xB5; + ack_packet[1] = 0x62; + ack_packet[2] = 0x05; + ack_packet[3] = 0x01; + ack_packet[4] = 0x02; + ack_packet[5] = 0x00; + ack_packet[6] = message[2]; + ack_packet[7] = message[3]; + ack_packet[8] = 0x00; + ack_packet[9] = 0x00; + + for (int i = 0; i<8; ++i) + { + ack_packet[8]+= ack_packet[i]; + ack_packet[9]+= ack_packet[8]; + } + bytes_ordered = 0; + + gettimeofday(&time_start, NULL); + ms_start = (long)((time_start.tv_sec)*1000 + (time_start.tv_usec)/1000); + ms_now = ms_start; + + // Prevent lock and timeout if not set after three seconds + while (ms_now - ms_start <= 3000) + { + gettimeofday(&time_now, NULL); + ms_now = (long)((time_now.tv_sec)*1000 + (time_now.tv_usec)/1000); + + if (bytes_ordered > 9) + { + return true; + } + + if (this->serial->available()) + { + byte = (unsigned char) this->serial->read_char(); + if (byte == ack_packet[bytes_ordered]) + { + bytes_ordered++; + } + else + { + bytes_ordered = 0; + } + } + } + return false; +} + +void GPS::notify_initialization() +{ + this->logger->log("Initialization notified. Switching to pedestrian mode"); + this->enter_pedestrian_mode(); +} + +void GPS::notify_takeoff() +{ + this->logger->log("Takeoff notified. Switching to airborne mode"); + this->enter_airborne_1g_mode(); +} + +void GPS::notify_safe_mode() +{ + this->logger->log("Safe mode entry notified. Switching to airborne mode"); + this->enter_airborne_1g_mode(); +} + +void GPS::notify_landing() +{ + this->logger->log("Landing notified. Switching to stationary mode"); + this->enter_stationary_mode(); } diff --git a/gps/GPS.h b/gps/GPS.h index 3dac520..fa88494 100644 --- a/gps/GPS.h +++ b/gps/GPS.h @@ -6,6 +6,8 @@ #include #include +#include + #include "serial/Serial.h" #include "logger/Logger.h" @@ -13,11 +15,11 @@ using namespace std; namespace os { - struct euc_vec + typedef struct { float speed; float course; - }; + } euc_vec; class GPS { @@ -29,7 +31,7 @@ namespace os { atomic_bool should_stop; atomic_bool stopped; - tm time; + timeval time; bool active; uint_fast8_t satellites; double latitude; @@ -48,13 +50,20 @@ namespace os { void parse_GSA(const string& frame); void parse_RMC(const string& frame); + void enter_pedestrian_mode(); /*Before launch*/ + void enter_airborne_1g_mode(); /*While in flight*/ + void enter_stationary_mode(); /*When landed*/ + + void send_ublox_packet(vector message); + bool receive_check_ublox_ack(vector message); + public: GPS(GPS& copy) = delete; ~GPS(); static GPS& get_instance(); static bool is_valid(string frame); - tm get_time() const {return this->time;} + timeval get_time() const {return this->time;} bool is_fixed() const {return this->active;} uint_fast8_t get_satellites() const {return this->satellites;} double get_latitude() const {return this->latitude;} @@ -66,9 +75,15 @@ namespace os { euc_vec get_velocity() const {return this->velocity;} bool initialize(); + bool is_on() const; bool turn_on() const; bool turn_off() const; void parse(const string& frame); + + void notify_takeoff(); + void notify_landing(); + void notify_initialization(); + void notify_safe_mode(); }; } diff --git a/gsm/GSM.cc b/gsm/GSM.cc index 14d97fe..4057c70 100644 --- a/gsm/GSM.cc +++ b/gsm/GSM.cc @@ -6,6 +6,7 @@ #include #include #include +#include #include @@ -36,7 +37,7 @@ GSM::~GSM() delete this->command_logger; } - if (this->get_status()) + if (this->is_on()) { this->logger->log("Shutting down..."); this->turn_off(); @@ -53,32 +54,31 @@ bool GSM::initialize() gettimeofday(&timer, NULL); struct tm * now = gmtime(&timer.tv_sec); - this->logger = new Logger("data/logs/GSM/GSM."+ to_string(now->tm_year+1900) +"-"+ to_string(now->tm_mon) +"-"+ + this->logger = new Logger("data/logs/GSM/GSM."+ to_string(now->tm_year+1900) +"-"+ to_string(now->tm_mon+1) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ to_string(now->tm_min) +"-"+ to_string(now->tm_sec) +".log", "GSM"); this->command_logger = new Logger("data/logs/GSM/GSMCommands."+ to_string(now->tm_year+1900) +"-"+ - to_string(now->tm_mon) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ + to_string(now->tm_mon+1) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ to_string(now->tm_min) +"-"+ to_string(now->tm_sec) +".log", "GSMCommand"); pinMode(GSM_PWR_GPIO, OUTPUT); digitalWrite(GSM_PWR_GPIO, HIGH); pinMode(GSM_STATUS_GPIO, INPUT); - this->logger->log("Rebooting module for stability."); - this->turn_off(); - this->logger->log("Module off. Sleeping 3 seconds before turning it on..."); - this_thread::sleep_for(3s); + if (this->is_on()) + { + this->logger->log("Module is on, rebooting for stability."); + this->turn_off(); + this->logger->log("Module off. Sleeping 3 seconds before turning it on..."); + this_thread::sleep_for(3s); + } this->logger->log("Turning module on..."); this->turn_on(); - if (this->get_status()) - { - this->logger->log("Status checked. Module is on."); - } - else + if ( ! this->is_on()) { - this->logger->log("Error: Status checked. Module is off. Finishing initialization."); + this->logger->log("Error: The module is still off. Finishing initialization."); return false; } this->logger->log("Sleeping 3 seconds to let it turn completely on..."); @@ -86,7 +86,11 @@ bool GSM::initialize() this->occupied = true; this->logger->log("Starting serial connection..."); - this->serial = new Serial(GSM_UART, GSM_BAUDRATE, "GSM"); + #ifdef DEBUG + this->serial = new Serial(GSM_UART, GSM_BAUDRATE, "GSM"); + #else + this->serial = new Serial(GSM_UART, GSM_BAUDRATE); + #endif if ( ! this->serial->is_open()) { this->logger->log("GSM serial error."); @@ -100,11 +104,15 @@ bool GSM::initialize() this->logger->log("Checking OK initialization (3 times)..."); if (this->send_command_read("AT") != "OK") + { this->logger->log("Not initialized."); + } this_thread::sleep_for(100ms); if (this->send_command_read("AT") != "OK") + { this->logger->log("Not initialized."); + } this_thread::sleep_for(100ms); if (this->send_command_read("AT") != "OK") @@ -122,13 +130,16 @@ bool GSM::initialize() bool GSM::send_SMS(const string& message, const string& number) { - while (this->occupied) this_thread::sleep_for(10ms); + while (this->occupied) + { + this_thread::sleep_for(10ms); + } this->occupied = true; this->logger->log("Sending SMS: \""+message+"\" ("+ to_string(message.length()) +" characters) to number "+number+"."); if (message.length() > 160) { - this->logger->log("Error: SMS has more than 10 characters"); + this->logger->log("Error: SMS has more than 160 characters"); } else { @@ -150,8 +161,13 @@ bool GSM::send_SMS(const string& message, const string& number) this->serial->println(message); this->command_logger->log("Sent: '"+message+"'"); - for (int i = 0; i <= std::count(message.begin(), message.end(), '\n'); i++) - this->command_logger->log("Received: '"+this->serial->read_line()+"'"); // Eat message echo + for (int i = 0; i <= count(message.begin(), message.end(), '\n'); ++i) + { + // Eat message echo + this->command_logger->log( + "Received: '"+this->serial->read_line()+"'" + ); + } this->serial->println(); this->serial->read_line(); // Eat prompt @@ -190,7 +206,10 @@ bool GSM::send_SMS(const string& message, const string& number) bool GSM::get_location(double& latitude, double& longitude) { - while (this->occupied) this_thread::sleep_for(10ms); + while (this->occupied) + { + this_thread::sleep_for(10ms); + } this->occupied = true; if (this->send_command_read("AT+CMGF=1") != "OK") @@ -204,9 +223,13 @@ bool GSM::get_location(double& latitude, double& longitude) { this->logger->log("Error getting location on 'AT+CGATT=1' response."); if (this->send_command_read("AT+SAPBR=0,1") != "OK") + { this->logger->log("Error turning GPRS down."); + } else + { this->logger->log("GPRS off."); + } this->occupied = false; return false; @@ -216,9 +239,13 @@ bool GSM::get_location(double& latitude, double& longitude) { this->logger->log("Error getting location on 'AT+SAPBR=3,1,\"CONTYPE\",\"GPRS\"' response."); if (this->send_command_read("AT+SAPBR=0,1") != "OK") + { this->logger->log("Error turning GPRS down."); + } else + { this->logger->log("GPRS off."); + } this->occupied = false; return false; @@ -228,9 +255,13 @@ bool GSM::get_location(double& latitude, double& longitude) { this->logger->log("Error getting location on 'AT+SAPBR=3,1,\"APN\",\""+string(GSM_LOC_SERV)+"\"' response."); if (this->send_command_read("AT+SAPBR=0,1") != "OK") + { this->logger->log("Error turning GPRS down."); + } else + { this->logger->log("GPRS off."); + } this->occupied = false; return false; @@ -247,9 +278,13 @@ bool GSM::get_location(double& latitude, double& longitude) { this->logger->log("Error getting location on 'AT+SAPBR=1,1' response."); if (this->send_command_read("AT+SAPBR=0,1") != "OK") + { this->logger->log("Error turning GPRS down."); + } else + { this->logger->log("GPRS off."); + } this->occupied = false; return false; @@ -265,7 +300,10 @@ bool GSM::get_location(double& latitude, double& longitude) vector s_data; // We put all fields in a vector - while(getline(ss, data, ',')) s_data.push_back(data); + while(getline(ss, data, ',')) + { + s_data.push_back(data); + } latitude = stod(s_data[2]); longitude = stod(s_data[1]); @@ -273,13 +311,17 @@ bool GSM::get_location(double& latitude, double& longitude) this->serial->read_line(); // Eat new line response = this->serial->read_line(); this->command_logger->log("Received: '"+ response +"'"); - if (response == "ERROR" || response != "OK") + if (response != "OK") { this->logger->log("Error getting location on 'AT+CIPGSMLOC=1,1' response."); if (this->send_command_read("AT+SAPBR=0,1") != "OK") + { this->logger->log("Error turning GPRS down."); + } else + { this->logger->log("GPRS off."); + } this->occupied = false; return false; @@ -290,26 +332,33 @@ bool GSM::get_location(double& latitude, double& longitude) response = this->serial->read_line(); this->command_logger->log("Received: '"+ response +"'"); if (response != "OK") + { this->logger->log("Error turning GPRS down."); + } else + { this->logger->log("GPRS off."); + } this->occupied = false; return true; } -bool GSM::get_status() const +bool GSM::is_on() const { return digitalRead(GSM_STATUS_GPIO) == HIGH; } bool GSM::get_battery_status(double& main_bat_percentage, double& gsm_bat_percentage) { - while (this->occupied) this_thread::sleep_for(10ms); + while (this->occupied) + { + this_thread::sleep_for(10ms); + } this->occupied = true; this->logger->log("Checking Battery status."); - if (this->get_status()) + if (this->is_on()) { string gsm_response = this->send_command_read("AT+CBC"); this->serial->read_line(); // Eat new line @@ -317,24 +366,51 @@ bool GSM::get_battery_status(double& main_bat_percentage, double& gsm_bat_percen string adc_response = this->send_command_read("AT+CADC?"); this->serial->read_line(); // Eat new line this->serial->read_line(); // Eat OK - while (adc_response != "" && adc_response.substr(0, 6) != "+CADC:") + + chrono::high_resolution_clock::time_point start = chrono::high_resolution_clock::now(); + while (adc_response != "" && + adc_response.substr(0, 6) != "+CADC:" && + (chrono::high_resolution_clock::now() - start) < 1s) + { adc_response = this->serial->read_line(); + } - if (gsm_response.substr(0, 5) == "+CBC:" && adc_response.substr(0, 6) == "+CADC:") + if (gsm_response.length() >= 5 && + gsm_response.substr(0, 5) == "+CBC:" && + adc_response.length() >= 6 && + adc_response.substr(0, 6) == "+CADC:") { stringstream gsm_ss(gsm_response); string data; vector gsm_data; - while(getline(gsm_ss, data, ',')) gsm_data.push_back(data); - - int gsm_bat_voltage = stoi(gsm_data[2]); - int main_bat_voltage = stoi(adc_response.substr(9, 4)); - gsm_bat_percentage = (gsm_bat_voltage/1000.0-BAT_GSM_MIN)/(BAT_GSM_MAX-BAT_GSM_MIN); - main_bat_percentage = (main_bat_voltage/1000.0-BAT_MAIN_MIN)/(BAT_MAIN_MAX-BAT_MAIN_MIN); - - this->occupied = false; - return true; + while(getline(gsm_ss, data, ',')) + { + gsm_data.push_back(data); + } + + if (gsm_data.size() >= 3) + { + try + { + int gsm_bat_voltage = stoi(gsm_data[2]); + int main_bat_voltage = stoi(adc_response.substr(9, 4)); + gsm_bat_percentage = (gsm_bat_voltage/1000.0-BAT_GSM_MIN)/(BAT_GSM_MAX-BAT_GSM_MIN); + main_bat_percentage = (main_bat_voltage/1000.0-BAT_MAIN_MIN)/(BAT_MAIN_MAX-BAT_MAIN_MIN); + } + catch (const invalid_argument& ia) + { + this->occupied = false; + return false; + } + + this->occupied = false; + return true; + } + else + { + return false; + } } } else @@ -347,8 +423,12 @@ bool GSM::get_battery_status(double& main_bat_percentage, double& gsm_bat_percen bool GSM::has_connectivity() { - while (this->occupied) this_thread::sleep_for(10ms); + while (this->occupied) + { + this_thread::sleep_for(10ms); + } this->occupied = true; + string response = this->send_command_read("AT+CREG?"); this->serial->read_line(); // Eat new line this->serial->read_line(); // Eat OK @@ -359,7 +439,7 @@ bool GSM::has_connectivity() bool GSM::turn_on() const { - if ( ! this->get_status()) + if ( ! this->is_on()) { this->logger->log("Turning GSM on..."); @@ -381,7 +461,7 @@ bool GSM::turn_on() const bool GSM::turn_off() const { - if (this->get_status()) + if (this->is_on()) { this->logger->log("Turning GSM off..."); @@ -407,16 +487,19 @@ const string GSM::send_command_read(const string& command) const this->serial->flush(); this->serial->println(command); string response = this->serial->read_line(); - // Trimming - string ltrim = response.erase(0, response.find_first_not_of("\r\n\t")); - response = ltrim.erase(ltrim.find_last_not_of("\r\n\t")+1); - - if (response == command) // Sent command + if ( ! response.empty()) { - response = this->serial->read_line(); // Trimming string ltrim = response.erase(0, response.find_first_not_of("\r\n\t")); response = ltrim.erase(ltrim.find_last_not_of("\r\n\t")+1); + + if (response == command) // Sent command + { + response = this->serial->read_line(); + // Trimming + string ltrim = response.erase(0, response.find_first_not_of("\r\n\t")); + response = ltrim.erase(ltrim.find_last_not_of("\r\n\t")+1); + } } this->command_logger->log("Received: '"+response+"'"); diff --git a/gsm/GSM.h b/gsm/GSM.h index a489278..8af0d01 100644 --- a/gsm/GSM.h +++ b/gsm/GSM.h @@ -1,4 +1,4 @@ -#ifndef GSM_GMS_H_ +#ifndef GSM_GSM_H_ #define GSM_GSM_H_ #include @@ -34,11 +34,11 @@ namespace os { bool initialize(); bool send_SMS(const string& message, const string& number); bool get_location(double& latitude, double& longitude); - bool get_status() const; + bool is_on() const; bool get_battery_status(double& main_bat_percentage, double& gsm_bat_percentage); bool has_connectivity(); bool turn_on() const; bool turn_off() const; }; } -#endif +#endif // GSM_GSM_H_ diff --git a/install-wiringpi.sh b/install-wiringpi.sh index 89ceca6..7389763 100644 --- a/install-wiringpi.sh +++ b/install-wiringpi.sh @@ -1,7 +1,7 @@ #!/bin/bash printf "Installing WiringPi\n" -git clone https://github.com/OpenStratos/WiringPi.git > /dev/null -cd WiringPi +git clone git://git.drogon.net/wiringPi > /dev/null +cd wiringPi sudo ./build > /dev/null cd .. diff --git a/logger/Logger.cc b/logger/Logger.cc index 3d905ad..b182dc9 100644 --- a/logger/Logger.cc +++ b/logger/Logger.cc @@ -26,7 +26,7 @@ void Logger::log(const string& message) gettimeofday(&timer, NULL); struct tm * now = gmtime(&timer.tv_sec); - this->log_stream << "[" << log_prefix << "] - " << setfill('0') << setw(2) << now->tm_mon << "/" << + this->log_stream << "[" << log_prefix << "] - " << setfill('0') << setw(2) << now->tm_mon+1 << "/" << setfill('0') << setw(2) << now->tm_mday << "/" << (now->tm_year+1900) << " " << setfill('0') << setw(2) << now->tm_hour << ":" << setfill('0') << setw(2) << now->tm_min << ":" << setfill('0') << setw(2) << now->tm_sec << "." << setfill('0') << setw(6) << timer.tv_usec << diff --git a/logic/acquire_fix.cc b/logic/acquire_fix.cc new file mode 100644 index 0000000..8888358 --- /dev/null +++ b/logic/acquire_fix.cc @@ -0,0 +1,5 @@ +while ( ! GPS::get_instance().is_fixed()) + this_thread::sleep_for(1s); + +logger->log("GPS fix acquired, waiting 10 seconds for stabilization and date change."); +this_thread::sleep_for(10s); diff --git a/logic/go_down.cc b/logic/go_down.cc new file mode 100644 index 0000000..7883ce7 --- /dev/null +++ b/logic/go_down.cc @@ -0,0 +1,235 @@ +#if !defined SIM && !defined REAL_SIM + double start_alt = GPS::get_instance().get_altitude(); +#endif + +#if defined SIM || defined REAL_SIM + if (FLIGHT_MAX_HEIGHT > 25000) +#else + if (start_alt > 25000) +#endif +{ + wait_down_for(25000); + logger->log("25 km mark passed going down."); +} + +#if defined SIM || defined REAL_SIM + if (FLIGHT_MAX_HEIGHT > 15000) +#else + if (start_alt > 15000) +#endif +{ + wait_down_for(15000); + logger->log("15 km mark passed going down."); +} + +#if defined SIM || defined REAL_SIM + if (FLIGHT_MAX_HEIGHT > 5000) +#else + if (start_alt > 5000) +#endif +{ + wait_down_for(5000); + logger->log("5 km mark passed going down."); +} + +#if defined SIM || defined REAL_SIM + if (FLIGHT_MAX_HEIGHT > 2000) +#else + if (start_alt > 2000) +#endif +{ + wait_down_for(2000); + logger->log("2 km mark passed going down."); +} + +logger->log("Turning on GSM..."); +GSM::get_instance().turn_on(); + +double main_battery = 0, gsm_battery = 0; +bool bat_status = false; + +logger->log("Waiting for GSM connectivity..."); +int count = 0; +while ( ! GSM::get_instance().has_connectivity()) +{ + if (count == 20) + { + break; + } + + this_thread::sleep_for(1s); + ++count; +} + +if (count == 20) +{ + logger->log("No connectivity, waiting for 1.2 km mark or landing."); +} +else +{ + logger->log("GSM connected."); + + logger->log("Getting battery values..."); + if ((bat_status = + GSM::get_instance().get_battery_status(main_battery, gsm_battery))) + { + logger->log("Battery status received."); + } + else + { + logger->log("Error getting battery status."); + } + + logger->log("Trying to send first SMS..."); + for (int i = 0; + i < 5 && ( ! GPS::get_instance().is_fixed() || GPS::get_instance().get_PDOP() > MAX_DOP); + ++i) + { + this_thread::sleep_for(500ms); + } + + if ( ! GSM::get_instance().send_SMS( + "Alt: "+ to_string((int) GPS::get_instance().get_altitude()) + + " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) + + "\r\nLon: "+ to_string(GPS::get_instance().get_longitude()) + + "\r\nPDOP: "+ to_string(GPS::get_instance().get_PDOP()) + + "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()) + + "\r\nFix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + + (bat_status ? "\r\nMain bat: "+ to_string((int) (main_battery*100)) + + "%\r\nGSM bat: "+ to_string((int) (gsm_battery*100)) +"%" : "\r\nBat: ERR"), SMS_PHONE)) + { + logger->log("Error sending first SMS."); + } + else + { + logger->log("First SMS sent."); + } + + bat_status = false; +} + +if ( ! wait_down_for(1200)) +{ + logger->log("1.2 km mark passed going down."); + + count = 0; + while ( ! GSM::get_instance().has_connectivity()) + { + if (count == 20) + { + break; + } + + this_thread::sleep_for(1s); + ++count; + } + if (count == 20) + { + logger->log("No connectivity, waiting for 500 m mark or landing."); + } + else + { + logger->log("GSM connected."); + + logger->log("Getting battery values..."); + if ((bat_status = GSM::get_instance().get_battery_status(main_battery, gsm_battery))) + { + logger->log("Battery status received."); + } + else + { + logger->log("Error getting battery status."); + } + + logger->log("Trying to send second SMS..."); + for (int i = 0; + i < 5 && ( ! GPS::get_instance().is_fixed() || GPS::get_instance().get_PDOP() > MAX_DOP); + ++i) + { + this_thread::sleep_for(500ms); + } + + if ( ! GSM::get_instance().send_SMS( + "Alt: "+ to_string((int) GPS::get_instance().get_altitude()) + + " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) + + "\r\nLon: "+ to_string(GPS::get_instance().get_longitude()) + + "\r\nPDOP: "+ to_string(GPS::get_instance().get_PDOP()) + + "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()) + + "\r\nFix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + + (bat_status ? "\r\nMain bat: "+ to_string((int) (main_battery*100)) + + "%\r\nGSM bat: "+ to_string((int) (gsm_battery*100)) +"%" : "\r\nBat: ERR"), SMS_PHONE)) + { + logger->log("Error sending second SMS."); + } + else + { + logger->log("Second SMS sent."); + } + + bat_status = false; + } +} + +if ( ! wait_down_for(500)) +{ + logger->log("500 m mark passed going down."); + + count = 0; + while ( ! GSM::get_instance().has_connectivity()) + { + if (count > 15) + { + break; + } + + this_thread::sleep_for(1s); + ++count; + } + + if ( ! GSM::get_instance().has_connectivity()) + { + logger->log("No connectivity, waiting for landing."); + } + else + { + logger->log("GSM connected."); + + logger->log("Getting battery values..."); + if ((bat_status = GSM::get_instance().get_battery_status(main_battery, gsm_battery))) + { + logger->log("Battery status received."); + } + else + { + logger->log("Error getting battery status."); + } + + logger->log("Trying to send third SMS..."); + for (int i = 0; + i < 5 && ( ! GPS::get_instance().is_fixed() || GPS::get_instance().get_PDOP() > MAX_DOP); + ++i) + { + this_thread::sleep_for(500ms); + } + + if ( ! GSM::get_instance().send_SMS( + "Alt: "+ to_string((int) GPS::get_instance().get_altitude()) + + " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) + + "\r\nLon: "+ to_string(GPS::get_instance().get_longitude()) + + "\r\nPDOP: "+ to_string(GPS::get_instance().get_PDOP()) + + "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()) + + "\r\nFix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + + (bat_status ? "\r\nMain bat: "+ to_string((int) (main_battery*100)) + + "%\r\nGSM bat: "+ to_string((int) (gsm_battery*100)) +"%" : "\r\nBat: ERR"), SMS_PHONE)) + { + logger->log("Error sending third SMS."); + } + else + { + logger->log("Third SMS sent."); + } + } +} + +while ( ! has_landed()); +logger->log("Landed."); diff --git a/logic/go_up.cc b/logic/go_up.cc new file mode 100644 index 0000000..487f0b3 --- /dev/null +++ b/logic/go_up.cc @@ -0,0 +1,194 @@ +double main_battery = 0, gsm_battery = 0; +bool bat_status = false; + +logger->log("Getting battery values..."); +if ((bat_status = GSM::get_instance().get_battery_status(main_battery, gsm_battery))) +{ + logger->log("Battery status received."); +} +else +{ + logger->log("Error getting battery status."); +} + +logger->log("Trying to send launch confirmation SMS..."); +for (int i = 0; + i < 5 && ( ! GPS::get_instance().is_fixed() || GPS::get_instance().get_PDOP() > MAX_DOP); + ++i) +{ + this_thread::sleep_for(500ms); +} + +if ( ! GSM::get_instance().send_SMS( + "Launch.\r\nAlt: "+ to_string((int) GPS::get_instance().get_altitude()) + + " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) + + "\r\nLon: "+ to_string(GPS::get_instance().get_longitude()) + + "\r\nPDOP: "+ to_string(GPS::get_instance().get_PDOP()) + + "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()) + + "\r\nFix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + + (bat_status ? "\r\nMain bat: "+ to_string((int) (main_battery*100)) + + "%\r\nGSM bat: "+ to_string((int) (gsm_battery*100)) +"%" : "\r\nBat: ERR"), SMS_PHONE)) +{ + logger->log("Error sending launch confirmation SMS."); +} +else +{ + logger->log("Launch confirmation SMS sent."); +} + +for (int i = 0; + i < 5 && ( ! GPS::get_instance().is_fixed() || GPS::get_instance().get_PDOP() > MAX_DOP); + ++i) +{ + this_thread::sleep_for(500ms); +} +double maximum_altitude = GPS::get_instance().get_altitude(); + +wait_up_for(1200, maximum_altitude); +logger->log("1.2 km mark."); + +logger->log("Getting battery values..."); +if ((bat_status = GSM::get_instance().get_battery_status(main_battery, gsm_battery))) +{ + logger->log("Battery status received."); +} +else +{ + logger->log("Error getting battery status."); +} + +logger->log("Trying to send \"going up\" SMS..."); +for (int i = 0; + i < 5 && ( ! GPS::get_instance().is_fixed() || GPS::get_instance().get_PDOP() > MAX_DOP); + ++i) +{ + this_thread::sleep_for(500ms); +} + +if ( ! GSM::get_instance().send_SMS( + "Alt: "+ to_string((int) GPS::get_instance().get_altitude()) + + " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) + + "\r\nLon: "+ to_string(GPS::get_instance().get_longitude()) + + "\r\nPDOP: "+ to_string(GPS::get_instance().get_PDOP()) + + "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()) + + "\r\nFix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + + (bat_status ? "\r\nMain bat: "+ to_string((int) (main_battery*100)) + + "%\r\nGSM bat: "+ to_string((int) (gsm_battery*100)) +"%" : "\r\nBat: ERR"), SMS_PHONE) && + // Second attempt + ! GSM::get_instance().send_SMS( + "Alt: "+ to_string((int) GPS::get_instance().get_altitude()) + + " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) + + "\r\nLon: "+ to_string(GPS::get_instance().get_longitude()) + + "\r\nPDOP: "+ to_string(GPS::get_instance().get_PDOP()) + + "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()) + + "\r\nFix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + + (bat_status ? "\r\nMain bat: "+ to_string((int) (main_battery*100)) + + "%\r\nGSM bat: "+ to_string((int) (gsm_battery*100)) +"%" : "\r\nBat: ERR"), SMS_PHONE)) +{ + logger->log("Error sending \"going up\" SMS."); +} +else +{ + logger->log("\"Going up\" SMS sent."); +} + +logger->log("Turning off GSM..."); +GSM::get_instance().turn_off(); +logger->log("GSM off."); + +if (wait_up_for(5000, maximum_altitude)) +{ + logger->log("Balloon burst at about "+ to_string((int) maximum_altitude) +" m."); + + *state = set_state(GOING_DOWN); + logger->log("State changed to "+ state_to_string(*state) +"."); + continue; +} +logger->log("5 km mark passed going up."); + +if (wait_up_for(10000, maximum_altitude)) +{ + logger->log("Balloon burst at about "+ to_string((int) maximum_altitude) +" m."); + + *state = set_state(GOING_DOWN); + logger->log("State changed to "+ state_to_string(*state) +"."); + continue; +} +logger->log("10 km mark passed going up."); + +if (wait_up_for(15000, maximum_altitude)) +{ + logger->log("Balloon burst at about "+ to_string((int) maximum_altitude) +" m."); + + *state = set_state(GOING_DOWN); + logger->log("State changed to "+ state_to_string(*state) +"."); + continue; +} +logger->log("15 km mark passed going up."); + +if (wait_up_for(20000, maximum_altitude)) +{ + logger->log("Balloon burst at about "+ to_string((int) maximum_altitude) +" m."); + + *state = set_state(GOING_DOWN); + logger->log("State changed to "+ state_to_string(*state) +"."); + continue; +} +logger->log("20 km mark passed going up."); + +if (wait_up_for(25000, maximum_altitude)) +{ + logger->log("Balloon burst at about "+ to_string((int) maximum_altitude) +" m."); + + *state = set_state(GOING_DOWN); + logger->log("State changed to "+ state_to_string(*state) +"."); + continue; +} +logger->log("25 km mark passed going up."); + +if (wait_up_for(30000, maximum_altitude)) +{ + logger->log("Balloon burst at about "+ to_string((int) maximum_altitude) +" m."); + + *state = set_state(GOING_DOWN); + logger->log("State changed to "+ state_to_string(*state) +"."); + continue; +} +logger->log("30 km mark passed going up."); + +if (wait_up_for(35000, maximum_altitude)) +{ + logger->log("Balloon burst at about "+ to_string((int) maximum_altitude) +" m."); + + *state = set_state(GOING_DOWN); + logger->log("State changed to "+ state_to_string(*state) +"."); + continue; +} +logger->log("35 km mark passed going up."); + +if (wait_up_for(40000, maximum_altitude)) +{ + logger->log("Balloon burst at about "+ to_string((int) maximum_altitude) +" m."); + + *state = set_state(GOING_DOWN); + logger->log("State changed to "+ state_to_string(*state) +"."); + continue; +} +logger->log("40 km mark passed going up."); + +while ( ! has_bursted(maximum_altitude)) +{ + double current_altitude; + for (int i = 0; + i < 10 && ( ! GPS::get_instance().is_fixed() || + GPS::get_instance().get_VDOP() > MAX_DOP); ++i) + { + this_thread::sleep_for(500ms); + } + + if ((current_altitude = GPS::get_instance().get_altitude()) > maximum_altitude) + { + maximum_altitude = current_altitude; + } +} +logger->log("Balloon burst at about "+ to_string((int) maximum_altitude) +" m."); diff --git a/logic/initialize.cc b/logic/initialize.cc new file mode 100644 index 0000000..a816138 --- /dev/null +++ b/logic/initialize.cc @@ -0,0 +1,185 @@ +check_or_create("data/video", &logger); +check_or_create("data/img", &logger); + +float available_disk_space = get_available_disk_space(); + +logger.log("Available disk space: " + to_string(available_disk_space/1073741824) + " GiB"); +if (available_disk_space < (FLIGHT_LENGTH*1.25+0.5)*7549747200) +{ + logger.log("Error: Not enough disk space."); + + #ifndef NO_POWER_OFF + sync(); + reboot(RB_POWER_OFF); + #else + exit(1); + #endif +} + +logger.log("Disk space enough for about " + to_string(available_disk_space/7549747200) + + " hours of fullHD video."); + +logger.log("Initializing WiringPi..."); +wiringPiSetup(); +logger.log("WiringPi initialized."); + +logger.log("Initializing GPS..."); +if ( ! GPS::get_instance().initialize()) +{ + logger.log("GPS initialization error."); + + #ifndef NO_POWER_OFF + sync(); + reboot(RB_POWER_OFF); + #else + exit(1); + #endif +} +logger.log("GPS initialized."); + +// Notify the GPS about its initialization, so that it sets itself to the +// appropriate mode +logger.log("Notifying GPS about initialization..."); +GPS::get_instance().notify_initialization(); + +logger.log("Initializing GSM..."); +if ( ! GSM::get_instance().initialize()) +{ + logger.log("GSM initialization error."); + logger.log("Turning GPS off..."); + if (GPS::get_instance().turn_off()) + logger.log("GPS off."); + else + logger.log("Error turning GPS off."); + + #ifndef NO_POWER_OFF + sync(); + reboot(RB_POWER_OFF); + #else + exit(1); + #endif +} +logger.log("GSM initialized."); + +logger.log("Checking batteries..."); +double main_battery, gsm_battery; +if ( ! GSM::get_instance().get_battery_status(main_battery, gsm_battery) && + ! GSM::get_instance().get_battery_status(main_battery, gsm_battery)) +{ + logger.log("Error checking batteries."); + + logger.log("Turning GSM off..."); + if (GSM::get_instance().turn_off()) + logger.log("GSM off."); + else + logger.log("Error turning GSM off."); + + logger.log("Turning GPS off..."); + if (GPS::get_instance().turn_off()) + logger.log("GPS off."); + else + logger.log("Error turning GPS off."); + + #ifndef NO_POWER_OFF + sync(); + reboot(RB_POWER_OFF); + #else + exit(1); + #endif +} + +logger.log("Batteries checked => Main battery: "+ (main_battery > -1 ? to_string(main_battery*100)+"%" : "disconnected") + + " - GSM battery: "+ to_string(gsm_battery*100) +"%"); + +if ((main_battery < 0.95 && main_battery > -1) || gsm_battery < 0.95) +{ + logger.log("Error: Not enough battery."); + + logger.log("Turning GSM off..."); + if (GSM::get_instance().turn_off()) + logger.log("GSM off."); + else + logger.log("Error turning GSM off."); + + logger.log("Turning GPS off..."); + if (GPS::get_instance().turn_off()) + logger.log("GPS off."); + else + logger.log("Error turning GPS off."); + + #ifndef NO_POWER_OFF + sync(); + reboot(RB_POWER_OFF); + #else + exit(1); + #endif +} + +logger.log("Waiting for GSM connectivity..."); +while ( ! GSM::get_instance().has_connectivity()) + this_thread::sleep_for(1s); + +logger.log("GSM connected."); + +logger.log("Testing camera recording..."); +#ifndef RASPICAM + logger.log("Error: No raspivid found. Is this a Raspberry?"); + exit(1); +#endif +logger.log("Recording 10 seconds as test..."); +if ( ! Camera::get_instance().record(10000)) +{ + logger.log("Error starting recording"); + + logger.log("Turning GSM off..."); + if (GSM::get_instance().turn_off()) + logger.log("GSM off."); + else + logger.log("Error turning GSM off."); + + logger.log("Turning GPS off..."); + if (GPS::get_instance().turn_off()) + logger.log("GPS off."); + else + logger.log("Error turning GPS off."); + + #ifndef NO_POWER_OFF + sync(); + reboot(RB_POWER_OFF); + #else + exit(1); + #endif +} +this_thread::sleep_for(11s); +if (file_exists("data/video/test.h264")) +{ + logger.log("Camera test OK."); + + logger.log("Removing test file..."); + if (remove("data/video/test.h264")) + logger.log("Error removing test file."); + else + logger.log("Test file removed."); +} +else +{ + logger.log("Test recording error."); + logger.log("Turning GSM off..."); + if (GSM::get_instance().turn_off()) + logger.log("GSM off."); + else + logger.log("Error turning GSM off."); + + logger.log("Turning GPS off..."); + if (GPS::get_instance().turn_off()) + logger.log("GPS off."); + else + logger.log("Error turning GPS off."); + + #ifndef NO_POWER_OFF + sync(); + reboot(RB_POWER_OFF); + #else + exit(1); + #endif +} diff --git a/logic/land.cc b/logic/land.cc new file mode 100644 index 0000000..b9a5344 --- /dev/null +++ b/logic/land.cc @@ -0,0 +1,101 @@ +logger->log("Stopping video..."); +if ( ! Camera::get_instance().stop()) + logger->log("Error stopping video."); +else + logger->log("Video stopped."); + +logger->log("Notifying GPS about landing..."); +GPS::get_instance().notify_landing(); + +logger->log("Waiting 1 minute before sending landed SMS..."); +this_thread::sleep_for(1min); + +double main_battery = 0, gsm_battery = 0; +bool bat_status = false; + +logger->log("Getting battery values..."); +if ((bat_status = + GSM::get_instance().get_battery_status(main_battery, gsm_battery) || + GSM::get_instance().get_battery_status(main_battery, gsm_battery))) +{ + logger->log("Battery status received."); +} +else +{ + logger->log("Error getting battery status."); +} + +logger->log("Sending landed SMS..."); +for (int i = 0; + i < 10 && ( ! GPS::get_instance().is_fixed() || GPS::get_instance().get_PDOP() > MAX_DOP); + ++i) +{ + this_thread::sleep_for(500ms); +} + +if ( ! GSM::get_instance().send_SMS( + "Landed.\r\nAlt: "+ to_string((int) GPS::get_instance().get_altitude()) + + " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) + + "\r\nLon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ + "\r\nHDOP: "+to_string(GPS::get_instance().get_HDOP()) + + "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()) + + "\r\nFix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + + (bat_status ? "\r\nMain bat: "+ to_string((int) (main_battery*100)) + + "%\r\nGSM bat: "+ to_string((int) (gsm_battery*100)) +"%" : "\r\nBat: ERR"), SMS_PHONE)) +{ + logger->log("Error sending landed SMS. Trying again in 10 minutes..."); +} +else +{ + logger->log("Landed SMS sent. Sending backup SMS in 10 minutes..."); +} + +this_thread::sleep_for(10min); + +logger->log("Getting battery values..."); +if ((bat_status = + GSM::get_instance().get_battery_status(main_battery, gsm_battery) || + GSM::get_instance().get_battery_status(main_battery, gsm_battery))) +{ + logger->log("Battery status received."); +} +else +{ + logger->log("Error getting battery status."); +} + +logger->log("Sending second landed SMS..."); +for (int i = 0; + i < 60 && ( ! GPS::get_instance().is_fixed() || GPS::get_instance().get_PDOP() > GOOD_DOP); + ++i) +{ + this_thread::sleep_for(500ms); +} + +while (( ! GSM::get_instance().send_SMS( + "Landed.\r\nAlt: "+ to_string((int) GPS::get_instance().get_altitude()) + + " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) + + "\r\nLon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ + "\r\nHDOP: "+to_string(GPS::get_instance().get_HDOP()) + + "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()) + + "\r\nFix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + + (bat_status ? "\r\nMain bat: "+ to_string((int) (main_battery*100)) + + "%\r\nGSM bat: "+ to_string((int) (gsm_battery*100)) +"%" : "\r\nBat: ERR"), SMS_PHONE) || + ! GPS::get_instance().is_fixed()) && + (main_battery >= 0 || main_battery < -1) && gsm_battery >= 0) +{ + logger->log("Error sending second SMS or GPS without fix, trying again in 5 minutes."); + this_thread::sleep_for(5min); + GSM::get_instance().get_battery_status(main_battery, gsm_battery); +} + +if ((main_battery < 0 && main_battery > -1) || gsm_battery < 0) +{ + logger->log("Not enough battery."); + logger->log("Main battery: "+ to_string(main_battery*100) + + "% - GSM battery: "+ to_string(gsm_battery*100) +"%"); +} +else +{ + logger->log("Second SMS sent."); +} diff --git a/logic/logic.h b/logic/logic.h new file mode 100644 index 0000000..d7308f6 --- /dev/null +++ b/logic/logic.h @@ -0,0 +1,33 @@ +#ifndef LOGIC_LOGIC_H_ +#define LOGIC_LOGIC_H_ + +#include +#ifdef DEBUG + #include + #include +#endif + +#include +#ifndef NO_POWER_OFF + #include + #include +#endif + +#include + +#include "config.h" +#include "constants.h" +#include "utils.h" +#include "threads.h" +#include "logger/Logger.h" +#include "gps/GPS.h" +#include "camera/Camera.h" +#include "gsm/GSM.h" + +namespace os +{ + void main_while(Logger* logger, State* state); + void shut_down(Logger* logger); +} + +#endif // LOGIC_LOGIC_H_ diff --git a/logic/main_logic.cc b/logic/main_logic.cc new file mode 100644 index 0000000..8495d9e --- /dev/null +++ b/logic/main_logic.cc @@ -0,0 +1,61 @@ +struct timeval timer; +gettimeofday(&timer, NULL); +struct tm* now = gmtime(&timer.tv_sec); + +#ifdef DEBUG + cout << "[OpenStratos] Current time: " << setfill('0') << setw(2) << now->tm_hour << ":" << + setfill('0') << setw(2) << now->tm_min << ":" << setfill('0') << setw(2) << now->tm_sec + << " UTC of " << setfill('0') << setw(2) << now->tm_mon << "/" << + setfill('0') << setw(2) << now->tm_mday << "/" << (now->tm_year+1900) << endl; +#endif + +check_or_create("data"); +State state = set_state(INITIALIZING); + +check_or_create("data/logs"); +check_or_create("data/logs/main"); +check_or_create("data/logs/system"); +check_or_create("data/logs/camera"); +check_or_create("data/logs/GPS"); +check_or_create("data/logs/GSM"); + +#ifdef DEBUG + cout << "[OpenStratos] Starting logger..." << endl; +#endif + +Logger logger("data/logs/main/OpenStratos."+ to_string(now->tm_year+1900) +"-"+ + to_string(now->tm_mon+1) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ + to_string(now->tm_min) +"-"+ to_string(now->tm_sec) +".log", "OpenStratos"); + +#ifdef DEBUG + cout << "[OpenStratos] Logger started." << endl; +#endif + +logger.log(PACKAGE_STRING); + +logger.log("Starting system thread..."); +thread system_thread(&system_thread_fn, ref(state)); +logger.log("System thread started."); + +#include "logic/initialize.cc" + +logger.log("Starting battery thread..."); +thread battery_thread(&battery_thread_fn, ref(state)); +logger.log("Battery thread started."); + +logger.log("Starting picture thread..."); +thread picture_thread(&picture_thread_fn, ref(state)); +logger.log("Picture thread started."); + +state = set_state(ACQUIRING_FIX); +logger.log("State changed to "+ state_to_string(state) +"."); + +main_while(&logger, &state); + +logger.log("Joining threads..."); +picture_thread.join(); +battery_thread.join(); +system_thread.join(); +logger.log("Threads joined."); + +shut_down(&logger); diff --git a/logic/main_while.cc b/logic/main_while.cc new file mode 100644 index 0000000..644493a --- /dev/null +++ b/logic/main_while.cc @@ -0,0 +1,57 @@ +#include "logic/logic.h" + +void os::main_while(Logger* logger, State* state) +{ + double launch_altitude = 0; + + while (*state != SHUT_DOWN) + { + if (*state == ACQUIRING_FIX) + { + #include "logic/acquire_fix.cc" + *state = set_state(FIX_ACQUIRED); + logger->log("State changed to "+ state_to_string(*state) +"."); + } + else if (*state == FIX_ACQUIRED) + { + logger->log("Sleeping 30 seconds for fix stabilization..."); + this_thread::sleep_for(30s); + + #include "logic/start_recording.cc" + #include "logic/send_init_sms.cc" + *state = set_state(WAITING_LAUNCH); + logger->log("State changed to "+ state_to_string(*state) +"."); + } + else if (*state == WAITING_LAUNCH) + { + #include "logic/wait_launch.cc" + *state = set_state(GOING_UP); + logger->log("State changed to "+ state_to_string(*state) +"."); + } + else if (*state == GOING_UP) + { + #include "logic/go_up.cc" + } + else if (*state == GOING_DOWN) + { + #include "logic/go_down.cc" + *state = set_state(LANDED); + logger->log("State changed to "+ state_to_string(*state) +"."); + } + else if (*state == LANDED) + { + #include "logic/land.cc" + *state = set_state(SHUT_DOWN); + logger->log("State changed to "+ state_to_string(*state) +"."); + } + else + { + #ifndef NO_POWER_OFF + sync(); + reboot(RB_AUTOBOOT); + #else + exit(1); + #endif + } + } +} diff --git a/logic/safe_mode.cc b/logic/safe_mode.cc new file mode 100644 index 0000000..0cdb691 --- /dev/null +++ b/logic/safe_mode.cc @@ -0,0 +1,308 @@ +State last_state = get_last_state(); +State state = set_state(SAFE_MODE); + +int count = 0; +double latitude = 0, longitude = 0; + +check_or_create("data/logs"); +check_or_create("data/logs/main"); +check_or_create("data/logs/system"); +check_or_create("data/logs/camera"); +check_or_create("data/logs/GPS"); +check_or_create("data/logs/GSM"); + +struct timeval timer; +gettimeofday(&timer, NULL); +struct tm* now = gmtime(&timer.tv_sec); + +Logger logger("data/logs/main/OpenStratos."+ to_string(now->tm_year+1900) +"-"+ + to_string(now->tm_mon+1) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) + +"-"+ to_string(now->tm_min) +"-"+ to_string(now->tm_sec) +".log", "OpenStratos"); + +logger.log(PACKAGE_STRING); +logger.log("Safe mode."); +logger.log("Last state was "+state_to_string(last_state)+"."); + +switch (last_state) +{ + case INITIALIZING: + case ACQUIRING_FIX: + { + logger.log("OpenStratos did not send the initialization SMS."); + remove(STATE_FILE); + logger.log("Rebooting after deleting the state file to start again."); + #ifndef NO_POWER_OFF + sync(); + reboot(RB_AUTOBOOT); + #else + exit(0); + #endif + break; + } + case FIX_ACQUIRED: + // It could be that the SMS was sent but the state didn't change + case WAITING_LAUNCH: + case GOING_UP: + case GOING_DOWN: + case LANDED: + { + if (last_state == FIX_ACQUIRED || last_state == WAITING_LAUNCH) + { + logger.log("OpenStratos might have been launched, trying to recover."); + } + else if (last_state == LANDED) + { + logger.log("OpenStratos had already landed, trying to recover and make sure it sends recovery messages."); + } + else + { + logger.log("OpenStratos has already been launched, trying to recover."); + } + + logger.log("Initializing WiringPi..."); + wiringPiSetup(); + logger.log("WiringPi initialized."); + + logger.log("Initializing GPS..."); + while ( ! GPS::get_instance().initialize() && ++count < 5) + { + logger.log("GPS initialization error."); + } + + if (count < 5) + { + logger.log("GPS initialized."); + count = 0; + logger.log("Waiting for GPS fix..."); + while ( ! GPS::get_instance().is_fixed() && ++count < 100) + { + this_thread::sleep_for(10s); + } + + if (count == 100) + { + logger.log("Not getting fix. Going to recovery mode."); + #ifndef NO_POWER_OFF + sync(); + reboot(RB_AUTOBOOT); + #else + exit(1); + #endif + } + + logger.log("GPS fix acquired. Sleeping 30 seconds for stability."); + this_thread::sleep_for(30s); + logger.log("Getting real state from GPS information."); + state = (state == LANDED) ? LANDED : get_real_state(); + logger.log("Seems that the current state is "+state_to_string(state)+"."); + + logger.log("Initializing GSM..."); + if ( ! GSM::get_instance().initialize()) + { + logger.log("GSM initialization error. Going to recovery mode."); + #ifndef NO_POWER_OFF + sync(); + reboot(RB_AUTOBOOT); + #else + exit(1); + #endif + } + logger.log("GSM initialized."); + + if (state == GOING_UP || state == GOING_DOWN) + { + GSM::get_instance().turn_off(); + } + + if (state != LANDED) + { + logger.log("Trying to start recording..."); + if (Camera::get_instance().record()) + { + logger.log("Recording."); + } + else + { + logger.log("Error starting recording."); + } + } + + logger.log("Entering main while from safe mode."); + main_while(&logger, &state); + shut_down(&logger); + } + else + { + logger.log("Error initializing GPS. Going to recovery mode."); + #ifndef NO_POWER_OFF + sync(); + reboot(RB_AUTOBOOT); + #else + exit(1); + #endif + } + break; + } + case SHUT_DOWN: + { + logger.log("Seems OpenStratos was shutting down when the error occurred."); + shut_down(&logger); + break; + } + case SAFE_MODE: + { + logger.log("Recovery mode"); + + logger.log("Initializing WiringPi..."); + wiringPiSetup(); + logger.log("WiringPi initialized."); + + logger.log("Initializing GSM..."); + while ( ! GSM::get_instance().initialize()) + { + logger.log("GSM initialization error."); + } + logger.log("GSM initialized"); + logger.log("Waiting for GSM connectivity..."); + while ( ! GSM::get_instance().has_connectivity()) + { + this_thread::sleep_for(5s); + } + logger.log("GSM connected."); + + logger.log("Sending mayday messages..."); + for (count = 0; count < 2;) + { + this_thread::sleep_for(5min); + + GSM::get_instance().get_location(latitude, longitude) && + GSM::get_instance().send_SMS("MAYDAY\r\nLat: "+ to_string(latitude) + + "\r\nLon: "+ to_string(longitude), SMS_PHONE) && ++count; + } + logger.log("Mayday messages sent."); + + logger.log("Initializing GPS..."); + count = 0; + while ( ! GPS::get_instance().initialize() && ++count < 5) + { + logger.log("GPS initialization error."); + } + + if (count < 5) + { + logger.log("GPS initialized."); + count = 0; + logger.log("Waiting for GPS fix..."); + while ( ! GPS::get_instance().is_fixed() && ++count < 100) + { + this_thread::sleep_for(10s); + } + + if (count == 100) + { + logger.log("Not getting fix."); + shut_down(&logger); + } + else + { + logger.log("GPS fix acquired."); + + logger.log("Notifying GPS about entering safe mode..."); + GPS::get_instance().notify_safe_mode(); + + this_thread::sleep_for(5s); + for (int i = 0; + GPS::get_instance().get_PDOP() > FAIR_DOP && i < 10; + ++i) + { + this_thread::sleep_for(500ms); + } + + logger.log("Sending mayday message with GPS information..."); + while ( ! GSM::get_instance().send_SMS("MAYDAY\r\nLat: " + + to_string(GPS::get_instance().get_latitude()) + + "\r\nLon: "+ to_string(GPS::get_instance().get_longitude()) + + "\r\nAlt: "+ to_string(GPS::get_instance().get_altitude()) + + "\r\nFix: OK", SMS_PHONE)) + { + logger.log("Error sending mayday message."); + this_thread::sleep_for(1min); + } + + logger.log("GPS fix acquired. Sleeping one minute for stability."); + this_thread::sleep_for(1min); + logger.log("Getting real state from GPS information."); + state = (state == LANDED) ? LANDED : get_real_state(); + logger.log("Seems that the current state is "+state_to_string(state)+"."); + + double main_battery = 0, gsm_battery = 0; + bool bat_status = false; + if ((bat_status = GSM::get_instance().get_battery_status(main_battery, gsm_battery))) + { + logger.log("Battery status received."); + } + else + { + logger.log("Error getting battery status."); + } + + while (GPS::get_instance().get_PDOP() > MAX_DOP) + { + this_thread::sleep_for(500ms); + if (++count > 10) + { + break; + } + } + + if (count < 10) + { + logger.log("Sending complete information SMS."); + if ( ! GSM::get_instance().send_SMS( + "Recovered.\r\nAlt: "+ to_string((int) GPS::get_instance().get_altitude()) + + " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) + + "\r\nLon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ + "\r\nPDOP: "+to_string(GPS::get_instance().get_PDOP()) + + "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()) + + "\r\nFix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + + (bat_status ? "\r\nMain bat: "+ to_string((int) (main_battery*100)) + + "%\r\nGSM bat: "+ to_string((int) (gsm_battery*100)) +"%" : "\r\nBat: ERR"), SMS_PHONE) && + // Second attempt + ! GSM::get_instance().send_SMS( + "Recovered.\r\nAlt: "+ to_string((int) GPS::get_instance().get_altitude()) + + " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) + + "\r\nLon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ + "\r\nPDOP: "+to_string(GPS::get_instance().get_PDOP()) + + "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()) + + "\r\nFix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + + (bat_status ? "\r\nMain bat: "+ to_string((int) (main_battery*100)) + + "%\r\nGSM bat: "+ to_string((int) (gsm_battery*100)) +"%" : "\r\nBat: ERR"), SMS_PHONE)) + { + logger.log("Error sending GPS information messages."); + } + } + + if (state != LANDED) + { + logger.log("Trying to start recording..."); + if (Camera::get_instance().record()) + { + logger.log("Recording."); + } + else + { + logger.log("Error starting recording."); + } + } + + logger.log("Entering main while again after recovery."); + main_while(&logger, &state); + shut_down(&logger); + } + } + else + { + shut_down(&logger); + } + } +} diff --git a/logic/send_init_sms.cc b/logic/send_init_sms.cc new file mode 100644 index 0000000..455af31 --- /dev/null +++ b/logic/send_init_sms.cc @@ -0,0 +1,71 @@ +double main_battery = 0, gsm_battery = 0; +bool bat_status = false; + +logger->log("Getting battery values..."); +if ((bat_status = GSM::get_instance().get_battery_status(main_battery, gsm_battery))) +{ + logger->log("Battery status received."); +} +else +{ + logger->log("Error getting battery status."); +} + +logger->log("Waiting for GPS PDOP < "+to_string(MAX_DOP)+"..."); +while ( ! GPS::get_instance().is_fixed() || GPS::get_instance().get_PDOP() > MAX_DOP) +{ + this_thread::sleep_for(1s); +} + +logger->log("Sending initialization SMS..."); +if ( ! GSM::get_instance().send_SMS( + "Init: OK.\r\nAlt: "+ to_string((int) GPS::get_instance().get_altitude()) + + " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) + + "\r\nLon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ + "\r\nPDOP: "+to_string(GPS::get_instance().get_PDOP()) + + "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()) + + "\r\nFix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + + (bat_status ? "\r\nMain bat: "+ to_string((int) (main_battery*100)) + + "%\r\nGSM bat: "+ to_string((int) (gsm_battery*100)) +"%" : "\r\nBat: ERR") + + "\r\nWaiting launch.", SMS_PHONE)) +{ + logger->log("Error sending initialization SMS."); + + logger->log("Stoping video recording."); + if (Camera::get_instance().stop()) + { + logger->log("Recording stopped."); + } + else + { + logger->log("Error stopping recording."); + } + + logger->log("Turning GSM off..."); + if (GSM::get_instance().turn_off()) + { + logger->log("GSM off."); + } + else + { + logger->log("Error turning GSM off."); + } + + logger->log("Turning GPS off..."); + if (GPS::get_instance().turn_off()) + { + logger->log("GPS off."); + } + else + { + logger->log("Error turning GPS off."); + } + + #ifndef NO_POWER_OFF + sync(); + reboot(RB_POWER_OFF); + #else + exit(1); + #endif +} +logger->log("Initialization SMS sent."); diff --git a/logic/shut_down.cc b/logic/shut_down.cc new file mode 100644 index 0000000..227bb02 --- /dev/null +++ b/logic/shut_down.cc @@ -0,0 +1,28 @@ +#include "logic/logic.h" + +void os::shut_down(Logger* logger) +{ + logger->log("Shutting down..."); + + logger->log("Turning GSM off..."); + if (GSM::get_instance().turn_off()) + { + logger->log("GSM off."); + } + else + { + logger->log("Error turning GSM off."); + } + + logger->log("Turning GPS off..."); + if (GPS::get_instance().turn_off()) + { + logger->log("GPS off."); + } + else + { + logger->log("Error turning GPS off."); + } + + logger->log("Powering off..."); +} diff --git a/logic/start_recording.cc b/logic/start_recording.cc new file mode 100644 index 0000000..70cc3ca --- /dev/null +++ b/logic/start_recording.cc @@ -0,0 +1,24 @@ +logger->log("Starting video recording..."); +if ( ! Camera::get_instance().record()) +{ + logger->log("Error starting recording"); + logger->log("Turning GSM off..."); + if (GSM::get_instance().turn_off()) + logger->log("GSM off."); + else + logger->log("Error turning GSM off."); + + logger->log("Turning GPS off..."); + if (GPS::get_instance().turn_off()) + logger->log("GPS off."); + else + logger->log("Error turning GPS off."); + + #ifndef NO_POWER_OFF + sync(); + reboot(RB_POWER_OFF); + #else + exit(1); + #endif +} +logger->log("Recording started."); diff --git a/logic/wait_launch.cc b/logic/wait_launch.cc new file mode 100644 index 0000000..debbb03 --- /dev/null +++ b/logic/wait_launch.cc @@ -0,0 +1,16 @@ +logger->log("Waiting for launch..."); +launch_altitude = GPS::get_instance().get_altitude(); +#ifdef SIM + this_thread::sleep_for(2min); +#endif +#ifdef REAL_SIM + this_thread::sleep_for(10min); +#endif + +while ( ! has_launched(launch_altitude)) + this_thread::sleep_for(1s); + +logger->log("Balloon launched."); + +logger->log("Notifying GPS about launch..."); +GPS::get_instance().notify_takeoff(); diff --git a/openstratos.cc b/openstratos.cc index 7be98ce..5772760 100644 --- a/openstratos.cc +++ b/openstratos.cc @@ -11,7 +11,7 @@ int main(void) cout << "[OpenStratos] Realistic simulation." << endl; #endif - cout << "[OpenStratos] Starting..." << endl; + cout << "[OpenStratos] Starting " << PACKAGE_STRING << "..." << endl; #endif if ( ! file_exists(STATE_FILE)) @@ -19,14 +19,14 @@ int main(void) #ifdef DEBUG cout << "[OpenStratos] No state file. Starting main logic..." << endl; #endif - main_logic(); + #include "logic/main_logic.cc" } else { #ifdef DEBUG cout << "[OpenStratos] State file found. Starting safe mode..." << endl; #endif - safe_mode(); + #include "logic/safe_mode.cc" } #ifndef NO_POWER_OFF @@ -36,1184 +36,3 @@ int main(void) return 0; } - -void os::main_logic() -{ - struct timeval timer; - gettimeofday(&timer, NULL); - struct tm* now = gmtime(&timer.tv_sec); - - #ifdef DEBUG - cout << "[OpenStratos] Current time: " << setfill('0') << setw(2) << now->tm_hour << ":" << - setfill('0') << setw(2) << now->tm_min << ":" << setfill('0') << setw(2) << now->tm_sec << - " UTC of " << setfill('0') << setw(2) << now->tm_mon << "/" << - setfill('0') << setw(2) << now->tm_mday << "/" << (now->tm_year+1900) << endl; - #endif - - check_or_create("data"); - State state = set_state(INITIALIZING); - - check_or_create("data/logs"); - check_or_create("data/logs/main"); - check_or_create("data/logs/system"); - check_or_create("data/logs/camera"); - check_or_create("data/logs/GPS"); - check_or_create("data/logs/GSM"); - - #ifdef DEBUG - cout << "[OpenStratos] Starting logger..." << endl; - #endif - - Logger logger("data/logs/main/OpenStratos."+ to_string(now->tm_year+1900) +"-"+ - to_string(now->tm_mon) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ - to_string(now->tm_min) +"-"+ to_string(now->tm_sec) +".log", "OpenStratos"); - - #ifdef DEBUG - cout << "[OpenStratos] Logger started." << endl; - #endif - - logger.log("Starting system thread..."); - thread system_thread(&system_thread_fn, ref(state)); - logger.log("System thread started."); - - initialize(&logger, now); - - logger.log("Starting battery thread..."); - thread battery_thread(&battery_thread_fn, ref(state)); - logger.log("Battery thread started."); - - logger.log("Starting picture thread..."); - thread picture_thread(&picture_thread_fn, ref(state)); - logger.log("Picture thread started."); - - state = set_state(ACQUIRING_FIX); - logger.log("State changed to "+ state_to_string(state) +"."); - - main_while(&logger, &state); - - logger.log("Joining threads..."); - picture_thread.join(); - battery_thread.join(); - system_thread.join(); - logger.log("Threads joined."); - - shut_down(&logger); -} - -void os::safe_mode() -{ - State last_state = get_last_state(); - State state = set_state(SAFE_MODE); - Logger* logger; - int count = 0; - double latitude = 0, longitude = 0; - - check_or_create("data/logs"); - check_or_create("data/logs/main"); - check_or_create("data/logs/system"); - check_or_create("data/logs/camera"); - check_or_create("data/logs/GPS"); - check_or_create("data/logs/GSM"); - - if (last_state > ACQUIRING_FIX) - { - struct timeval timer; - gettimeofday(&timer, NULL); - struct tm* now = gmtime(&timer.tv_sec); - - logger = new Logger("data/logs/main/OpenStratos."+ to_string(now->tm_year+1900) +"-"+ - to_string(now->tm_mon) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ - to_string(now->tm_min) +"-"+ to_string(now->tm_sec) +".log", "OpenStratos"); - } - - switch (last_state) - { - case INITIALIZING: - case ACQUIRING_FIX: - remove(STATE_FILE); - #ifndef NO_POWER_OFF - sync(); - reboot(RB_AUTOBOOT); - #else - exit(0); - #endif - break; - case FIX_ACQUIRED: // It could be that the SMS was sent but the state didn't change - case WAITING_LAUNCH: - case GOING_UP: - case GOING_DOWN: - case LANDED: - logger->log("Initializing WiringPi..."); - wiringPiSetup(); - logger->log("WiringPi initialized."); - - logger->log("Initializing GPS..."); - while ( ! GPS::get_instance().initialize() && ++count < 5) - logger->log("GPS initialization error."); - - if (count < 5) - { - logger->log("GPS initialized."); - count = 0; - logger->log("Waiting for GPS fix..."); - while ( ! GPS::get_instance().is_fixed() && ++count < 100) - this_thread::sleep_for(10s); - - if (count == 100) - { - logger->log("Not getting fix. Going to recovery mode."); - delete logger; - #ifndef NO_POWER_OFF - sync(); - reboot(RB_AUTOBOOT); - #else - exit(1); - #endif - } - - logger->log("GPS fix acquired."); - this_thread::sleep_for(1min); - state = (state == LANDED) ? LANDED : get_real_state(); - - logger->log("Initializing GSM..."); - if ( ! GSM::get_instance().initialize()) - { - logger->log("GSM initialization error. Going to recovery mode."); - delete logger; - #ifndef NO_POWER_OFF - sync(); - reboot(RB_AUTOBOOT); - #else - exit(1); - #endif - } - logger->log("GSM initialized."); - - if (state != LANDED) - { - logger->log("Trying to start recording..."); - if (Camera::get_instance().record()) - logger->log("Recording."); - else - logger->log("Error starting recording"); - } - - main_while(logger, &state); - shut_down(logger); - } - else - { - logger->log("Error initializing GPS. Going to recovery mode."); - delete logger; - #ifndef NO_POWER_OFF - sync(); - reboot(RB_AUTOBOOT); - #else - exit(1); - #endif - } - break; - case SHUT_DOWN: - shut_down(logger); - break; - case SAFE_MODE: - logger->log("Recovery mode"); - - logger->log("Initializing WiringPi..."); - wiringPiSetup(); - logger->log("WiringPi initialized."); - - logger->log("Initializing GSM..."); - while ( ! GSM::get_instance().initialize()) - logger->log("GSM initialization error."); - logger->log("GSM initialized"); - logger->log("Waiting for GSM connectivity..."); - while ( ! GSM::get_instance().has_connectivity()) this_thread::sleep_for(5s); - logger->log("GSM connected."); - - logger->log("Sending mayday messages..."); - for (count = 0; count < 10;) - { - this_thread::sleep_for(20s); - - GSM::get_instance().get_location(latitude, longitude); - GSM::get_instance().send_SMS("MAYDAY\r\nLat: "+ to_string(latitude) +"\r\n"+ - "Lon: "+ to_string(longitude), SMS_PHONE) && ++count; - } - logger->log("Mayday messages sent."); - - logger->log("Initializing GPS..."); - count = 0; - while ( ! GPS::get_instance().initialize() && ++count < 5) - logger->log("GPS initialization error."); - - if (count < 5) - { - logger->log("GPS initialized."); - count = 0; - logger->log("Waiting for GPS fix..."); - while ( ! GPS::get_instance().is_fixed() && ++count < 100) - this_thread::sleep_for(10s); - - if (count == 100) - { - logger->log("Not getting fix."); - shut_down(logger); - } - - logger->log("GPS fix acquired."); - this_thread::sleep_for(1min); - state = (state == LANDED) ? LANDED : get_real_state(); - - main_while(logger, &state); - shut_down(logger); - } - else - { - shut_down(logger); - } - } - - if (logger) delete logger; -} - -void os::main_while(Logger* logger, State* state) -{ - double launch_altitude; - - while (*state != SHUT_DOWN) - { - if (*state == ACQUIRING_FIX) - { - aquire_fix(logger); - *state = set_state(FIX_ACQUIRED); - logger->log("State changed to "+ state_to_string(*state) +"."); - } - else if (*state == FIX_ACQUIRED) - { - this_thread::sleep_for(2min); - logger->log("Sleeping 2 minutes for fix stabilization."); - - start_recording(logger); - send_init_sms(logger); - *state = set_state(WAITING_LAUNCH); - logger->log("State changed to "+ state_to_string(*state) +"."); - } - else if (*state == WAITING_LAUNCH) - { - wait_launch(logger, launch_altitude); - *state = set_state(GOING_UP); - logger->log("State changed to "+ state_to_string(*state) +"."); - } - else if (*state == GOING_UP) - { - go_up(logger, launch_altitude); - *state = set_state(GOING_DOWN); - logger->log("State changed to "+ state_to_string(*state) +"."); - } - else if (*state == GOING_DOWN) - { - go_down(logger); - *state = set_state(LANDED); - logger->log("State changed to "+ state_to_string(*state) +"."); - } - else if (*state == LANDED) - { - land(logger); - *state = set_state(SHUT_DOWN); - logger->log("State changed to "+ state_to_string(*state) +"."); - } - else - { - #ifndef NO_POWER_OFF - sync(); - reboot(RB_AUTOBOOT); - #else - exit(1); - #endif - } - } -} - -void os::initialize(Logger* logger, tm* now) -{ - check_or_create("data/video", logger); - check_or_create("data/img", logger); - - float available_disk_space = get_available_disk_space(); - - logger->log("Available disk space: " + to_string(available_disk_space/1073741824) + " GiB"); - if (available_disk_space < (FLIGHT_LENGTH*1.25+0.5)*7549747200) - { - logger->log("Error: Not enough disk space."); - - #ifndef NO_POWER_OFF - sync(); - reboot(RB_POWER_OFF); - #else - exit(1); - #endif - } - - logger->log("Disk space enough for about " + to_string(available_disk_space/7549747200) + - " hours of fullHD video."); - - logger->log("Initializing WiringPi..."); - wiringPiSetup(); - logger->log("WiringPi initialized."); - - logger->log("Initializing GPS..."); - if ( ! GPS::get_instance().initialize()) - { - logger->log("GPS initialization error."); - - #ifndef NO_POWER_OFF - sync(); - reboot(RB_POWER_OFF); - #else - exit(1); - #endif - } - logger->log("GPS initialized."); - - logger->log("Initializing GSM..."); - if ( ! GSM::get_instance().initialize()) - { - logger->log("GSM initialization error."); - logger->log("Turning GPS off..."); - if (GPS::get_instance().turn_off()) - logger->log("GPS off."); - else - logger->log("Error turning GPS off."); - - #ifndef NO_POWER_OFF - sync(); - reboot(RB_POWER_OFF); - #else - exit(1); - #endif - } - logger->log("GSM initialized."); - - logger->log("Checking batteries..."); - double main_battery, gsm_battery; - if ( ! GSM::get_instance().get_battery_status(main_battery, gsm_battery) && - ! GSM::get_instance().get_battery_status(main_battery, gsm_battery)) - { - logger->log("Error checking batteries."); - - logger->log("Turning GSM off..."); - if (GSM::get_instance().turn_off()) - logger->log("GSM off."); - else - logger->log("Error turning GSM off."); - - logger->log("Turning GPS off..."); - if (GPS::get_instance().turn_off()) - logger->log("GPS off."); - else - logger->log("Error turning GPS off."); - - #ifndef NO_POWER_OFF - sync(); - reboot(RB_POWER_OFF); - #else - exit(1); - #endif - } - - logger->log("Batteries checked => Main battery: "+ (main_battery > -1 ? to_string(main_battery*100)+"%" : "disconnected") + - " - GSM battery: "+ to_string(gsm_battery*100) +"%"); - - if ((main_battery < 0.9 && main_battery > -1) || gsm_battery < 0.85) - { - logger->log("Error: Not enough battery."); - - logger->log("Turning GSM off..."); - if (GSM::get_instance().turn_off()) - logger->log("GSM off."); - else - logger->log("Error turning GSM off."); - - logger->log("Turning GPS off..."); - if (GPS::get_instance().turn_off()) - logger->log("GPS off."); - else - logger->log("Error turning GPS off."); - - #ifndef NO_POWER_OFF - sync(); - reboot(RB_POWER_OFF); - #else - exit(1); - #endif - } - - logger->log("Waiting for GSM connectivity..."); - while ( ! GSM::get_instance().has_connectivity()) - this_thread::sleep_for(1s); - - logger->log("GSM connected."); - - logger->log("Testing camera recording..."); - #ifndef RASPICAM - logger->log("Error: No raspivid found. Is this a Raspberry?"); - exit(1); - #endif - logger->log("Recording 10 seconds as test..."); - if ( ! Camera::get_instance().record(10000)) - { - logger->log("Error starting recording"); - - #ifndef NO_POWER_OFF - sync(); - reboot(RB_POWER_OFF); - #else - exit(1); - #endif - } - this_thread::sleep_for(11s); - if (file_exists("data/video/test.h264")) - { - logger->log("Camera test OK."); - - logger->log("Removing test file..."); - if (remove("data/video/test.h264")) - logger->log("Error removing test file."); - else - logger->log("Test file removed."); - } - else - { - logger->log("Test recording error."); - logger->log("Turning GSM off..."); - if (GSM::get_instance().turn_off()) - logger->log("GSM off."); - else - logger->log("Error turning GSM off."); - - logger->log("Turning GPS off..."); - if (GPS::get_instance().turn_off()) - logger->log("GPS off."); - else - logger->log("Error turning GPS off."); - - #ifndef NO_POWER_OFF - sync(); - reboot(RB_POWER_OFF); - #else - exit(1); - #endif - } -} - -void os::aquire_fix(Logger* logger) -{ - while ( ! GPS::get_instance().is_fixed()) - this_thread::sleep_for(1s); - - logger->log("GPS fix acquired, waiting for date change."); - this_thread::sleep_for(2s); - - struct timezone tz = {0, 0}; - tm gps_time = GPS::get_instance().get_time(); - struct timeval tv = {timegm(&gps_time), 0}; - settimeofday(&tv, &tz); - - logger->log("System date change."); -} - -void os::start_recording(Logger* logger) -{ - logger->log("Starting video recording..."); - if ( ! Camera::get_instance().record()) - { - logger->log("Error starting recording"); - logger->log("Turning GSM off..."); - if (GSM::get_instance().turn_off()) - logger->log("GSM off."); - else - logger->log("Error turning GSM off."); - - logger->log("Turning GPS off..."); - if (GPS::get_instance().turn_off()) - logger->log("GPS off."); - else - logger->log("Error turning GPS off."); - - #ifndef NO_POWER_OFF - sync(); - reboot(RB_POWER_OFF); - #else - exit(1); - #endif - } - logger->log("Recording started."); -} - -void os::send_init_sms(Logger* logger) -{ - double main_battery = 0, gsm_battery = 0; - bool bat_status = false; - - logger->log("Getting battery values..."); - if (bat_status = GSM::get_instance().get_battery_status(main_battery, gsm_battery)) - logger->log("Battery status received."); - else - logger->log("Error getting battery status."); - - logger->log("Sending initialization SMS..."); - if ( ! GSM::get_instance().send_SMS( - "Init: OK\r\nAlt: "+ to_string((int) GPS::get_instance().get_altitude()) + - " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) +"\r\n"+ - "Lon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ - (bat_status ? "Main bat: "+ to_string((int) (main_battery*100)) +"%\r\n"+ - "GSM bat: "+ to_string((int) (gsm_battery*100)) +"%\r\n" : "Bat: ERR\r\n") + - "Fix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + - "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()) + - "\r\nWaiting Launch", SMS_PHONE)) - { - logger->log("Error sending initialization SMS."); - - logger->log("Stoping video recording."); - if (Camera::get_instance().stop()) - { - logger->log("Recording stopped."); - } - else - { - logger->log("Error stopping recording."); - } - - logger->log("Turning GSM off..."); - if (GSM::get_instance().turn_off()) - logger->log("GSM off."); - else - logger->log("Error turning GSM off."); - - logger->log("Turning GPS off..."); - if (GPS::get_instance().turn_off()) - logger->log("GPS off."); - else - logger->log("Error turning GPS off."); - - #ifndef NO_POWER_OFF - sync(); - reboot(RB_POWER_OFF); - #else - exit(1); - #endif - } - logger->log("Initialization SMS sent."); -} - -void os::wait_launch(Logger* logger, double& launch_altitude) -{ - logger->log("Waiting for launch..."); - launch_altitude = GPS::get_instance().get_altitude(); - #ifdef SIM - this_thread::sleep_for(2min); - #endif - #ifdef REAL_SIM - this_thread::sleep_for(10min); - #endif - - while ( ! has_launched(launch_altitude)) - this_thread::sleep_for(1s); - - logger->log("Balloon launched."); -} - -void os::go_up(Logger* logger, double launch_altitude) -{ - double main_battery = 0, gsm_battery = 0; - bool bat_status = false; - - logger->log("Getting battery values..."); - if (bat_status = GSM::get_instance().get_battery_status(main_battery, gsm_battery)) - logger->log("Battery status received."); - else - logger->log("Error getting battery status."); - - logger->log("Trying to send launch confirmation SMS..."); - if ( ! GSM::get_instance().send_SMS( - "Launch\r\nAlt: "+ to_string((int) launch_altitude) + - " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) +"\r\n"+ - "Lon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ - (bat_status ? "Main bat: "+ to_string((int) (main_battery*100)) +"%\r\n"+ - "GSM bat: "+ to_string((int) (gsm_battery*100)) +"%\r\n" : "Bat: ERR\r\n") + - "Fix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + - "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()), SMS_PHONE)) - { - logger->log("Error sending launch confirmation SMS."); - } - else - { - logger->log("Launch confirmation SMS sent."); - } - - double maximum_altitude = 0; - double current_altitude = GPS::get_instance().get_altitude(); - - #if !defined SIM && !defined REAL_SIM - while (current_altitude = GPS::get_instance().get_altitude() < 1200) - { - if (current_altitude > maximum_altitude) maximum_altitude = current_altitude; - this_thread::sleep_for(2s); - } - #else - this_thread::sleep_for(124s); - #endif - logger->log("1.2 km mark."); - - logger->log("Getting battery values..."); - if (bat_status = GSM::get_instance().get_battery_status(main_battery, gsm_battery)) - logger->log("Battery status received."); - else - logger->log("Error getting battery status."); - - logger->log("Trying to send \"going up\" SMS..."); - if ( ! GSM::get_instance().send_SMS( - "Alt: "+ to_string((int) GPS::get_instance().get_altitude()) + - " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) +"\r\n"+ - "Lon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ - (bat_status ? "Main bat: "+ to_string((int) (main_battery*100)) +"%\r\n"+ - "GSM bat: "+ to_string((int) (gsm_battery*100)) +"%\r\n" : "Bat: ERR\r\n") + - "Fix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + - "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()), SMS_PHONE) && - // Second attempt - ! GSM::get_instance().send_SMS( - "Alt: "+ to_string((int) GPS::get_instance().get_altitude()) + - " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) +"\r\n"+ - "Lon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ - (bat_status ? "Main bat: "+ to_string((int) (main_battery*100)) +"%\r\n"+ - "GSM bat: "+ to_string((int) (gsm_battery*100)) +"%\r\n" : "Bat: ERR\r\n") + - "Fix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + - "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()), SMS_PHONE)) - { - logger->log("Error sending \"going up\" SMS."); - } - else - { - logger->log("\"Going up\" SMS sent."); - } - - logger->log("Turning off GSM..."); - GSM::get_instance().turn_off(); - logger->log("GSM off."); - - bool bursted = false; - - #if defined SIM && !defined REAL_SIM - this_thread::sleep_for(2min); - logger->log("5 km mark passed going up."); - #elif defined REAL_SIM && !defined SIM - this_thread::sleep_for(1357s); - logger->log("5 km mark passed going up."); - #else - while ( ! (bursted = has_bursted(maximum_altitude)) && - (current_altitude = GPS::get_instance().get_altitude()) < 5000) - { - if (current_altitude > maximum_altitude) maximum_altitude = current_altitude; - } - if ( ! bursted) logger->log("5 km mark passed going up."); - else return; - #endif - - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - - #if defined SIM && !defined REAL_SIM - this_thread::sleep_for(2min); - logger->log("10 km mark passed going up."); - #elif defined REAL_SIM && !defined SIM - this_thread::sleep_for(1786s); - logger->log("10 km mark passed going up."); - #else - while ( ! (bursted = has_bursted(maximum_altitude)) && - (current_altitude = GPS::get_instance().get_altitude()) < 10000) - { - if (current_altitude > maximum_altitude) maximum_altitude = current_altitude; - } - if ( ! bursted) logger->log("10 km mark passed going up."); - else return; - #endif - - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - - #if defined SIM && !defined REAL_SIM - this_thread::sleep_for(2min); - logger->log("15 km mark passed going up."); - #elif defined REAL_SIM && !defined SIM - this_thread::sleep_for(1786s); - logger->log("15 km mark passed going up."); - #else - while ( ! (bursted = has_bursted(maximum_altitude)) && - (current_altitude = GPS::get_instance().get_altitude()) < 15000) - { - if (current_altitude > maximum_altitude) maximum_altitude = current_altitude; - } - if ( ! bursted) logger->log("15 km mark passed going up."); - else return; - #endif - - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - - #if defined SIM && !defined REAL_SIM - this_thread::sleep_for(2min); - logger->log("20 km mark passed going up."); - #elif defined REAL_SIM && !defined SIM - this_thread::sleep_for(1786s); - logger->log("20 km mark passed going up."); - #else - while ( ! (bursted = has_bursted(maximum_altitude)) && - (current_altitude = GPS::get_instance().get_altitude()) < 20000) - { - if (current_altitude > maximum_altitude) maximum_altitude = current_altitude; - } - if ( ! bursted) logger->log("20 km mark passed going up."); - else return; - #endif - - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - - #if defined SIM && !defined REAL_SIM - this_thread::sleep_for(2min); - logger->log("25 km mark passed going up."); - #elif defined REAL_SIM && !defined SIM - this_thread::sleep_for(1786s); - logger->log("25 km mark passed going up."); - #else - while ( ! (bursted = has_bursted(maximum_altitude)) && - (current_altitude = GPS::get_instance().get_altitude()) < 25000) - { - if (current_altitude > maximum_altitude) maximum_altitude = current_altitude; - } - if ( ! bursted) logger->log("25 km mark passed going up."); - else return; - #endif - - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - - #if defined SIM && !defined REAL_SIM - this_thread::sleep_for(2min); - logger->log("30 km mark passed going up."); - #elif defined REAL_SIM && !defined SIM - this_thread::sleep_for(1786s); - logger->log("30 km mark passed going up."); - #else - while ( ! (bursted = has_bursted(maximum_altitude)) && - (current_altitude = GPS::get_instance().get_altitude()) < 30000) - { - if (current_altitude > maximum_altitude) maximum_altitude = current_altitude; - } - if ( ! bursted) logger->log("30 km mark passed going up."); - else return; - #endif - - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - - #if defined SIM && !defined REAL_SIM - this_thread::sleep_for(2min); - #elif defined REAL_SIM && !defined SIM - this_thread::sleep_for(1740s); - #else - while ( ! (bursted = has_bursted(maximum_altitude)) && - (current_altitude = GPS::get_instance().get_altitude()) < 35000) - { - if (current_altitude > maximum_altitude) maximum_altitude = current_altitude; - } - if ( ! bursted) logger->log("35 km mark passed going up."); - else return; - #endif - - while ( ! has_bursted(maximum_altitude)) - { - if ((current_altitude = GPS::get_instance().get_altitude()) > maximum_altitude) - maximum_altitude = current_altitude; - - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - } - - logger->log("Balloon burst at about "+ to_string((int) maximum_altitude) +" m."); -} - -void os::go_down(Logger* logger) -{ - double main_battery = 0, gsm_battery = 0; - bool bat_status = false; - - #if defined SIM && !defined REAL_SIM - this_thread::sleep_for(1min); - #elif defined REAL_SIM && !defined SIM - this_thread::sleep_for(317s); - #else - while (GPS::get_instance().get_altitude() > 25000) - { - this_thread::sleep_for(5s); - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - } - #endif - - logger->log("25 km mark passed going down."); - - #if defined SIM && !defined REAL_SIM - this_thread::sleep_for(1min); - #elif defined REAL_SIM && !defined SIM - this_thread::sleep_for(684s); - #else - while (GPS::get_instance().get_altitude() > 15000) - { - this_thread::sleep_for(5s); - - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - } - #endif - - logger->log("15 km mark passed going down."); - - #if defined SIM && !defined REAL_SIM - this_thread::sleep_for(2min); - #elif defined REAL_SIM && !defined SIM - this_thread::sleep_for(1450s); - #else - while (GPS::get_instance().get_altitude() > 5000) - { - this_thread::sleep_for(5s); - - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - } - #endif - - logger->log("5 km mark passed going down."); - - #if defined SIM && !defined REAL_SIM - this_thread::sleep_for(1min); - #elif defined REAL_SIM && !defined SIM - this_thread::sleep_for(650s); - #else - while (GPS::get_instance().get_altitude() > 2000) - { - this_thread::sleep_for(5s); - - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - } - #endif - logger->log("2 km mark passed going down."); - - logger->log("Turning on GSM..."); - GSM::get_instance().turn_on(); - - logger->log("Waiting for GSM connectivity..."); - int count = 0; - while ( ! GSM::get_instance().has_connectivity()) - { - if (count == 20) break; - this_thread::sleep_for(1s); - ++count; - } - if (count == 20) - { - logger->log("No connectivity, waiting for 1.2 km mark or landing."); - } - else - { - logger->log("GSM connected."); - - logger->log("Getting battery values..."); - if (bat_status = GSM::get_instance().get_battery_status(main_battery, gsm_battery)) - logger->log("Battery status received."); - else - logger->log("Error getting battery status."); - - logger->log("Trying to send first SMS..."); - if ( ! GSM::get_instance().send_SMS( - "Alt: "+ to_string((int) GPS::get_instance().get_altitude()) + - " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) +"\r\n"+ - "Lon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ - (bat_status ? "Main bat: "+ to_string((int) (main_battery*100)) +"%\r\n"+ - "GSM bat: "+ to_string((int) (gsm_battery*100)) +"%\r\n" : "Bat: ERR\r\n") + - "Fix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + - "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()), SMS_PHONE)) - { - logger->log("Error sending first SMS."); - } - else - { - logger->log("First SMS sent."); - } - - bat_status = false; - } - - bool landed = false; - - #if defined SIM && !defined REAL_SIM - this_thread::sleep_for(1min); - #elif defined REAL_SIM && !defined SIM - this_thread::sleep_for(183s); - #else - while (GPS::get_instance().get_altitude() > 1200 && ! (landed = has_landed())) - { - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - } - #endif - - if ( ! landed) - { - logger->log("1.2 km mark passed going down."); - - count = 0; - while ( ! GSM::get_instance().has_connectivity()) - { - if (count == 20) break; - this_thread::sleep_for(1s); - ++count; - } - if (count == 20) - { - logger->log("No connectivity, waiting for 500 m mark or landing."); - } - else - { - logger->log("GSM connected."); - - logger->log("Getting battery values..."); - if (bat_status = GSM::get_instance().get_battery_status(main_battery, gsm_battery)) - logger->log("Battery status received."); - else - logger->log("Error getting battery status."); - - logger->log("Trying to send second SMS..."); - if ( ! GSM::get_instance().send_SMS( - "Alt: "+ to_string((int) GPS::get_instance().get_altitude()) + - " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) +"\r\n"+ - "Lon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ - (bat_status ? "Main bat: "+ to_string((int) (main_battery*100)) +"%\r\n"+ - "GSM bat: "+ to_string((int) (gsm_battery*100)) +"%\r\n" : "Bat: ERR\r\n") + - "Fix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + - "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()), SMS_PHONE)) - { - logger->log("Error sending second SMS."); - } - else - { - logger->log("Second SMS sent."); - } - - bat_status = false; - } - } - - #if defined SIM && !defined REAL_SIM - this_thread::sleep_for(1min); - #elif defined REAL_SIM && !defined SIM - this_thread::sleep_for(117s); - #else - while (GPS::get_instance().get_altitude() > 500 && ! (landed = has_landed())) - { - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - } - #endif - - if ( ! landed) - { - logger->log("500 m mark passed going down."); - - count = 0; - while ( ! GSM::get_instance().has_connectivity()) - { - if (count > 15) break; - this_thread::sleep_for(1s); - ++count; - } - if ( ! GSM::get_instance().has_connectivity()) - { - logger->log("No connectivity, waiting for landing."); - } - else - { - logger->log("GSM connected."); - - logger->log("Getting battery values..."); - if (bat_status = GSM::get_instance().get_battery_status(main_battery, gsm_battery)) - logger->log("Battery status received."); - else - logger->log("Error getting battery status."); - - - logger->log("Trying to send third SMS..."); - if ( ! GSM::get_instance().send_SMS( - "Alt: "+ to_string((int) GPS::get_instance().get_altitude()) + - " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) +"\r\n"+ - "Lon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ - (bat_status ? "Main bat: "+ to_string((int) (main_battery*100)) +"%\r\n"+ - "GSM bat: "+ to_string((int) (gsm_battery*100)) +"%\r\n" : "Bat: ERR\r\n") + - "Fix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + - "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()), SMS_PHONE)) - { - logger->log("Error sending third SMS."); - } - else - { - logger->log("Third SMS sent."); - } - } - } - - while ( ! has_landed()) - { - if (get_available_disk_space() < 2000000000) - { - logger->log("Not enough disk space. Stopping video..."); - Camera::get_instance().stop(); - } - } - logger->log("Landed."); -} - -void os::land(Logger* logger) -{ - logger->log("Stopping video..."); - if ( ! Camera::get_instance().stop()) - logger->log("Error stopping video."); - else - logger->log("Video stopped."); - - logger->log("Waiting 1 minute before sending landed SMS..."); - this_thread::sleep_for(1min); - - double main_battery = 0, gsm_battery = 0; - bool bat_status = false; - - logger->log("Getting battery values..."); - if (bat_status = (GSM::get_instance().get_battery_status(main_battery, gsm_battery) || - GSM::get_instance().get_battery_status(main_battery, gsm_battery))) - logger->log("Battery status received."); - else - logger->log("Error getting battery status."); - - logger->log("Sending landed SMS..."); - if ( ! GSM::get_instance().send_SMS( - "Landed\r\nAlt: "+ to_string((int) GPS::get_instance().get_altitude()) + - " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) +"\r\n"+ - "Lon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ - (bat_status ? "Main bat: "+ to_string((int) (main_battery*100)) +"%\r\n"+ - "GSM bat: "+ to_string((int) (gsm_battery*100)) +"%\r\n" : "Bat: ERR\r\n") + - "Fix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + - "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()), SMS_PHONE)) - { - logger->log("Error sending landed SMS. Trying again in 10 minutes..."); - } - else - { - logger->log("Landed SMS sent. Sending backup SMS in 10 minutes..."); - } - - this_thread::sleep_for(10min); - - logger->log("Getting battery values..."); - if (bat_status = (GSM::get_instance().get_battery_status(main_battery, gsm_battery) || - GSM::get_instance().get_battery_status(main_battery, gsm_battery))) - logger->log("Battery status received."); - else - logger->log("Error getting battery status."); - - logger->log("Sending second landed SMS..."); - while (( ! GSM::get_instance().send_SMS( - "Landed\r\nAlt: "+ to_string((int) GPS::get_instance().get_altitude()) + - " m\r\nLat: "+ to_string(GPS::get_instance().get_latitude()) +"\r\n"+ - "Lon: "+ to_string(GPS::get_instance().get_longitude()) +"\r\n"+ - (bat_status ? "Main bat: "+ to_string((int) (main_battery*100)) +"%\r\n"+ - "GSM bat: "+ to_string((int) (gsm_battery*100)) +"%\r\n" : "Bat: ERR\r\n") + - "Fix: "+ (GPS::get_instance().is_fixed() ? "OK" : "ERR") + - "\r\nSat: "+ to_string(GPS::get_instance().get_satellites()), SMS_PHONE) || - ! GPS::get_instance().is_fixed()) && - (main_battery >= 0 || main_battery < -1) && gsm_battery >= 0) - { - logger->log("Error sending second SMS or GPS without fix, trying again in 5 minutes."); - this_thread::sleep_for(5min); - GSM::get_instance().get_battery_status(main_battery, gsm_battery); - } - - if ((main_battery < 0 && main_battery > -1) || gsm_battery < 0) - { - logger->log("Not enough battery."); - logger->log("Main battery: "+ to_string(main_battery*100) + - "% - GSM battery: "+ to_string(gsm_battery*100) +"%"); - } - else - { - logger->log("Second SMS sent."); - } -} - -void os::shut_down(Logger* logger) -{ - logger->log("Shutting down..."); - - logger->log("Turning GSM off..."); - if (GSM::get_instance().turn_off()) - logger->log("GSM off."); - else - logger->log("Error turning GSM off."); - - logger->log("Turning GPS off..."); - if (GPS::get_instance().turn_off()) - logger->log("GPS off."); - else - logger->log("Error turning GPS off."); - - logger->log("Powering off..."); -} diff --git a/openstratos.h b/openstratos.h index deac4ad..2013931 100644 --- a/openstratos.h +++ b/openstratos.h @@ -1,45 +1,22 @@ #ifndef OPENSTRATOS_H_ #define OPENSTRATOS_H_ -#include -#include +#if defined SIM && defined REAL_SIM + #error You cannot try to compile a real simulation and a normal simulation at the same time. +#endif + #ifdef DEBUG #include #endif -#include #ifndef NO_POWER_OFF #include #include #endif -#include - #include "config.h" -#include "constants.h" #include "utils.h" -#include "threads.h" -#include "logger/Logger.h" -#include "gps/GPS.h" -#include "camera/Camera.h" -#include "gsm/GSM.h" - -namespace os -{ - void main_logic(); - void safe_mode(); - void main_while(Logger* logger, State* state); - - void initialize(Logger* logger, tm* now); - void aquire_fix(Logger* logger); - void start_recording(Logger* logger); - void send_init_sms(Logger* logger); - void wait_launch(Logger* logger, double& launch_altitude); - void go_up(Logger* logger, double launch_altitude); - void go_down(Logger* logger); - void land(Logger* logger); - void shut_down(Logger* logger); -} +#include "logic/logic.h" using namespace std; using namespace os; diff --git a/build.sh b/prepare.sh similarity index 71% rename from build.sh rename to prepare.sh index b7471b0..17c6f84 100755 --- a/build.sh +++ b/prepare.sh @@ -5,7 +5,6 @@ autoheader automake --add-missing autoconf ./configure -make utesting mkdir data mkdir data/logs @@ -13,6 +12,3 @@ mkdir data/logs/GPS mkdir data/logs/camera mkdir data/logs/main mkdir data/video - -printf "\n\n----- Starting unit tests -----\n\n" -./utesting diff --git a/serial/Serial.cc b/serial/Serial.cc index 146012e..0f32d96 100644 --- a/serial/Serial.cc +++ b/serial/Serial.cc @@ -5,6 +5,7 @@ #include #include #include +#include #include @@ -19,17 +20,21 @@ using namespace std; using namespace os; -Serial::Serial(const string& url, int baud_rate, const string& log_path) +#ifdef DEBUG + Serial::Serial(const string& url, int baud_rate, const string& log_path) +#else + Serial::Serial(const string& url, int baud_rate) +#endif { this->open = false; - struct timeval timer; - gettimeofday(&timer, NULL); - struct tm * now = gmtime(&timer.tv_sec); - #ifdef DEBUG + struct timeval timer; + gettimeofday(&timer, NULL); + struct tm * now = gmtime(&timer.tv_sec); + this->logger = new Logger("data/logs/"+log_path+"/Serial."+ to_string(now->tm_year+1900) +"-"+ - to_string(now->tm_mon) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ + to_string(now->tm_mon+1) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ to_string(now->tm_min) +"-"+ to_string(now->tm_sec) +".log", "Serial"); #endif @@ -37,10 +42,19 @@ Serial::Serial(const string& url, int baud_rate, const string& log_path) this->fd = serialOpen(url.c_str(), baud_rate); #ifdef DEBUG - if (this->fd == -1) this->logger->log("Error: connection fd is -1."); - else this->open = true; + if (this->fd == -1) + { + this->logger->log("Error: connection fd is -1."); + } + else + { + this->open = true; + } #else - if (this->fd != -1) this->open = true; + if (this->fd != -1) + { + this->open = true; + } #endif #endif } @@ -48,7 +62,9 @@ Serial::Serial(const string& url, int baud_rate, const string& log_path) Serial::~Serial() { if (this->open) + { this->close(); + } #ifdef DEBUG delete this->logger; @@ -76,16 +92,21 @@ void Serial::println() const void Serial::write(unsigned char c) const { serialPutchar(this->fd, c); +} - #ifdef DEBUG - this->logger->log("Sent char: '"+string(1, c)+"'"); - #endif +void Serial::write_vec(vector chars) const +{ + for (unsigned char c: chars) + { + this->write(c); + } } void Serial::close() { #ifndef OS_TESTING - if (this->open) { + if (this->open) + { serialClose(this->fd); this->open = false; } @@ -116,10 +137,11 @@ const string Serial::read_line(double timeout) const { string response = ""; string logstr = ""; - bool rfound = false, endl_found = false; - int available = 0; #ifndef OS_TESTING + bool rfound = false, endl_found = false; + int available = 0; + struct timeval t1, t2; double elapsed_time = 0; gettimeofday(&t1, NULL); @@ -139,13 +161,22 @@ const string Serial::read_line(double timeout) const break; } - while (available = serialDataAvail(this->fd) > 0) + while ((available = serialDataAvail(this->fd)) > 0) { char c = serialGetchar(this->fd); - if (c == '\r') logstr += "\\r"; - else if (c == '\n') logstr += "\\n"; - else logstr += c; + if (c == '\r') + { + logstr += "\\r"; + } + else if (c == '\n') + { + logstr += "\\n"; + } + else + { + logstr += c; + } if (c == '\r') { diff --git a/serial/Serial.h b/serial/Serial.h index 0323a31..cea7256 100644 --- a/serial/Serial.h +++ b/serial/Serial.h @@ -4,6 +4,7 @@ #include #include +#include #include "constants.h" #ifdef DEBUG @@ -24,13 +25,18 @@ namespace os { void gps_thread(); public: - Serial(const string& url, int baud_rate, const string& log_path); + #ifdef DEBUG + Serial(const string& url, int baud_rate, const string& log_path); + #else + Serial(const string& url, int baud_rate); + #endif Serial(Serial& copy) = delete; ~Serial(); void println(const string& str) const; void println() const; void write(unsigned char c) const; + void write_vec(vector chars) const; void close(); bool is_open() const; char read_char() const; diff --git a/testing/bandit b/testing/bandit index 76f1e76..7b28220 160000 --- a/testing/bandit +++ b/testing/bandit @@ -1 +1 @@ -Subproject commit 76f1e76d9842d4bee1742ee2b01fdcceed3421d4 +Subproject commit 7b282209fb1cd9bfa36472feee453b35454e57c0 diff --git a/testing/camera_test.cc b/testing/camera_test.cc index ed4e12a..6823ff8 100644 --- a/testing/camera_test.cc +++ b/testing/camera_test.cc @@ -1,6 +1,7 @@ -describe("Camera", [](){ - - it("recording test", [&](){ +describe("Camera", []() +{ + it("recording test", [&]() + { AssertThat(Camera::get_instance().record(2000), Equals(true)); AssertThat(Camera::get_instance().is_recording(), Equals(true)); @@ -12,7 +13,8 @@ describe("Camera", [](){ #endif }); - it("recording and stopping test", [&](){ + it("recording and stopping test", [&]() + { AssertThat(Camera::get_instance().record(), Equals(true)); AssertThat(Camera::get_instance().is_recording(), Equals(true)); @@ -25,7 +27,8 @@ describe("Camera", [](){ #endif }); - it("picture taking test", [&](){ + it("picture taking test", [&]() + { AssertThat(Camera::get_instance().take_picture(), Equals(true)); #ifdef RASPICAM @@ -34,7 +37,8 @@ describe("Camera", [](){ }); #ifdef RASPICAM - it("video file creation test", [&](){ + it("video file creation test", [&]() + { AssertThat(Camera::get_instance().record(10000), Equals(true)); this_thread::sleep_for(chrono::seconds(11)); @@ -49,7 +53,8 @@ describe("Camera", [](){ #endif #ifdef RASPICAM - it("picture file creation test", [&](){ + it("picture file creation test", [&]() + { int start_file_count = get_file_count("data/img/"); AssertThat(Camera::get_instance().take_picture(), Equals(true)); int end_file_count = get_file_count("data/img/"); diff --git a/testing/gps_test.cc b/testing/gps_test.cc index e647605..2034dba 100644 --- a/testing/gps_test.cc +++ b/testing/gps_test.cc @@ -1,16 +1,19 @@ -describe("GPS", [](){ - - before_each([&](){ +describe("GPS", []() +{ + before_each([&]() + { GPS::get_instance().initialize(); }); - it("Knots to m/s conversion test", [&](){ + it("Knots to m/s conversion test", [&]() + { AssertThat(kt_to_mps(150), Is().EqualToWithDelta(77.1666667, 0.005)); AssertThat(kt_to_mps(75), Is().EqualToWithDelta(38.58333, 0.005)); AssertThat(kt_to_mps(0), Equals(0)); }); - it("frame validity test", [&](){ + it("frame validity test", [&]() + { string valid = "$REPORT,0,23,185213184,1421782514,1,4140.7276,-0404.8853,73,52,43*1E"; string valid2 = "$PMTK226,3,30*4"; string not_valid = "$REPORT,0,23,185213184,1421782514,1,4140.7276,-0404.8853,73,52,43*14"; @@ -20,43 +23,51 @@ describe("GPS", [](){ AssertThat(GPS::is_valid(not_valid), Equals(false)); }); - it("GGA frame parser test", [&](){ - GPS::get_instance().parse("$GPGGA,151025,2011.3454,N,12020.2464,W,1,05,1.53,20134.13,M,20103.45,M,,*56"); + it("GGA frame parser test", [&]() + { + GPS::get_instance().parse("$GNGGA,065022.90,4518.48425,N,00113.59390,W,1,05,2.33,10202.2,M,10235.9,M,,*60"); AssertThat(GPS::get_instance().is_fixed(), Equals(true)); - tm gps_time = GPS::get_instance().get_time(); - AssertThat(gps_time.tm_hour, Equals(15)); - AssertThat(gps_time.tm_min, Equals(10)); - AssertThat(gps_time.tm_sec, Equals(25)); + timeval gps_time = GPS::get_instance().get_time(); + tm* ptm = gmtime(&gps_time.tv_sec); - AssertThat(GPS::get_instance().get_satellites(), Equals(5)); - AssertThat(GPS::get_instance().get_latitude(), Is().EqualToWithDelta(20.18909, 0.00001)); - AssertThat(GPS::get_instance().get_longitude(), Is().EqualToWithDelta(-120.33744, 0.00001)); + AssertThat(ptm->tm_hour, Equals(6)); + AssertThat(ptm->tm_min, Equals(50)); + AssertThat(ptm->tm_sec, Equals(22)); + AssertThat(gps_time.tv_usec, Equals(900000)); - AssertThat(GPS::get_instance().get_HDOP(), Is().EqualToWithDelta(1.53, 0.0005)); - AssertThat(GPS::get_instance().get_altitude(), Is().EqualToWithDelta(20134.13, 0.0005)); + AssertThat(GPS::get_instance().get_satellites(), Equals(5)); + AssertThat(GPS::get_instance().get_latitude(), + Is().EqualToWithDelta(45.308070833, 0.00001)); + AssertThat(GPS::get_instance().get_longitude(), + Is().EqualToWithDelta(-1.226565, 0.00001)); + + AssertThat(GPS::get_instance().get_HDOP(), + Is().EqualToWithDelta(2.33, 0.0005)); + AssertThat(GPS::get_instance().get_altitude(), + Is().EqualToWithDelta(10202.2, 0.0005)); }); - it("GGA frame parser pass test", [&](){ - - int hour = GPS::get_instance().get_time().tm_hour; - int min = GPS::get_instance().get_time().tm_min; - int sec = GPS::get_instance().get_time().tm_sec; - + it("GGA frame parser pass test", [&]() + { int satellites = GPS::get_instance().get_satellites(); double latitude = GPS::get_instance().get_latitude(); double longitude = GPS::get_instance().get_longitude(); double altitude = GPS::get_instance().get_altitude(); float hdop = GPS::get_instance().get_HDOP(); - GPS::get_instance().parse("$GPGGA,170810,2316.3654,S,12225.2464,E,0,07,1.89,18647.15,M,18640.35,M,,*53"); + GPS::get_instance().parse("$GNGGA,054320.70,4416.46425,N,00512.59490,W,0,05,1.25,9502.63,M,9515.2,M,,*55"); AssertThat(GPS::get_instance().is_fixed(), Equals(false)); - AssertThat(GPS::get_instance().get_time().tm_hour, Equals(hour)); - AssertThat(GPS::get_instance().get_time().tm_min, Equals(min)); - AssertThat(GPS::get_instance().get_time().tm_sec, Equals(sec)); + timeval gps_time = GPS::get_instance().get_time(); + tm* ptm = gmtime(&gps_time.tv_sec); + + AssertThat(ptm->tm_hour, Equals(5)); + AssertThat(ptm->tm_min, Equals(43)); + AssertThat(ptm->tm_sec, Equals(20)); + AssertThat(gps_time.tv_usec, Equals(700000)); AssertThat(GPS::get_instance().get_satellites(), Equals(satellites)); AssertThat(GPS::get_instance().get_latitude(), Equals(latitude)); @@ -65,77 +76,93 @@ describe("GPS", [](){ AssertThat(GPS::get_instance().get_HDOP(), Equals(hdop)); }); - it("GSA frame parser test", [&](){ - GPS::get_instance().parse("$GPGSA,A,3,,,,,,16,18,,22,24,,,3.6,2.1,2.2*3C"); + it("GSA frame parser test", [&]() + { + GPS::get_instance().parse("$GNGSA,A,3,22,01,19,23,17,03,11,31,,,,,1.91,0.99,1.63*1E"); AssertThat(GPS::get_instance().is_fixed(), Equals(true)); - AssertThat(GPS::get_instance().get_HDOP(), Is().EqualToWithDelta(2.1, 0.0005)); - AssertThat(GPS::get_instance().get_VDOP(), Is().EqualToWithDelta(2.2, 0.0005)); + AssertThat(GPS::get_instance().get_PDOP(), + Is().EqualToWithDelta(1.91, 0.0005)); + AssertThat(GPS::get_instance().get_HDOP(), + Is().EqualToWithDelta(0.99, 0.0005)); + AssertThat(GPS::get_instance().get_VDOP(), + Is().EqualToWithDelta(1.63, 0.0005)); }); - it("GSA frame parser pass test", [&](){ + it("GSA frame parser pass test", [&]() + { + float pdop = GPS::get_instance().get_PDOP(); float hdop = GPS::get_instance().get_HDOP(); float vdop = GPS::get_instance().get_VDOP(); - GPS::get_instance().parse("$GPGSA,A,1,19,28,14,18,27,22,31,39,,,,,1.7,1.0,1.3*36"); + GPS::get_instance().parse("$GNGSA,A,1,22,01,19,23,17,03,11,31,,,,,2.30,0.05,1.52*13"); AssertThat(GPS::get_instance().is_fixed(), Equals(false)); + AssertThat(GPS::get_instance().get_PDOP(), Equals(pdop)); AssertThat(GPS::get_instance().get_HDOP(), Equals(hdop)); AssertThat(GPS::get_instance().get_VDOP(), Equals(vdop)); }); - it("RMC frame parser test", [&](){ - GPS::get_instance().parse("$GPRMC,225446,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E*68"); + it("RMC frame parser test", [&]() + { + GPS::get_instance().parse("$GNRMC,083250.50,A,4210.48220,N,00306.55496,W,0.072,0.25,070616,,,A*6A"); AssertThat(GPS::get_instance().is_fixed(), Equals(true)); - tm gps_time = GPS::get_instance().get_time(); + timeval gps_time = GPS::get_instance().get_time(); + tm* ptm = gmtime(&gps_time.tv_sec); - AssertThat(gps_time.tm_mday, Equals(19)); - AssertThat(gps_time.tm_mon, Equals(10)); - AssertThat(gps_time.tm_year, Equals(194)); - AssertThat(gps_time.tm_hour, Equals(22)); - AssertThat(gps_time.tm_min, Equals(54)); - AssertThat(gps_time.tm_sec, Equals(46)); + AssertThat(ptm->tm_mday, Equals(7)); + AssertThat(ptm->tm_mon, Equals(5)); + AssertThat(ptm->tm_year, Equals(116)); + AssertThat(gps_time.tv_usec, Equals(500000)); - AssertThat(GPS::get_instance().get_latitude(), Is().EqualToWithDelta(49.27417, 0.00001)); - AssertThat(GPS::get_instance().get_longitude(), Is().EqualToWithDelta(-123.18533, 0.00001)); + AssertThat(ptm->tm_hour, Equals(8)); + AssertThat(ptm->tm_min, Equals(32)); + AssertThat(ptm->tm_sec, Equals(50)); - AssertThat(GPS::get_instance().get_velocity().speed, Is().EqualToWithDelta(0.25722, 0.00001)); - AssertThat(GPS::get_instance().get_velocity().course, Is().EqualToWithDelta(54.7, 0.001)); - }); - - it("RMC frame parser pass test", [&](){ + AssertThat(GPS::get_instance().get_latitude(), + Is().EqualToWithDelta(42.174703333, 0.00001)); + AssertThat(GPS::get_instance().get_longitude(), + Is().EqualToWithDelta(-3.109249333, 0.00001)); - tm gps_time = GPS::get_instance().get_time(); - int hour = gps_time.tm_hour; - int min = gps_time.tm_min; - int sec = gps_time.tm_sec; - int mday = gps_time.tm_mday; - int mon = gps_time.tm_mon; - int year = gps_time.tm_year; + AssertThat(GPS::get_instance().get_velocity().speed, + Is().EqualToWithDelta(0.03704, 0.00001)); + AssertThat(GPS::get_instance().get_velocity().course, + Is().EqualToWithDelta(0.25, 0.001)); + }); + it("RMC frame parser pass test", [&]() + { double latitude = GPS::get_instance().get_latitude(); double longitude = GPS::get_instance().get_longitude(); float speed = GPS::get_instance().get_velocity().speed; float course = GPS::get_instance().get_velocity().course; - GPS::get_instance().parse("$GPRMC,081836,V,3751.65,S,14507.36,E,000.0,360.0,130998,011.3,E*75"); + GPS::get_instance().parse("$GNRMC,093024.50,V,4012.20220,N,00508.50496,W,0.015,0.80,060515,,,A*71"); AssertThat(GPS::get_instance().is_fixed(), Equals(false)); - tm new_gps_time = GPS::get_instance().get_time(); + timeval gps_time = GPS::get_instance().get_time(); + tm* ptm = gmtime(&gps_time.tv_sec); - AssertThat(new_gps_time.tm_hour, Equals(hour)); - AssertThat(new_gps_time.tm_min, Equals(min)); - AssertThat(new_gps_time.tm_sec, Equals(sec)); - AssertThat(new_gps_time.tm_mday, Equals(mday)); - AssertThat(new_gps_time.tm_mon, Equals(mon)); - AssertThat(new_gps_time.tm_year, Equals(year)); + AssertThat(ptm->tm_hour, Equals(9)); + AssertThat(ptm->tm_min, Equals(30)); + AssertThat(ptm->tm_sec, Equals(24)); + AssertThat(gps_time.tv_usec, Equals(500000)); + + AssertThat(ptm->tm_mday, Equals(6)); + AssertThat(ptm->tm_mon, Equals(4)); + AssertThat(ptm->tm_year, Equals(115)); AssertThat(GPS::get_instance().get_latitude(), Equals(latitude)); AssertThat(GPS::get_instance().get_longitude(), Equals(longitude)); AssertThat(GPS::get_instance().get_velocity().speed, Equals(speed)); AssertThat(GPS::get_instance().get_velocity().course, Equals(course)); }); + + it("incomplete frame parser test", [&]() + { + GPS::get_instance().parse("$GNGSA,A,2,,N,00256.61767,W,0.124,,140616,,,A*74"); + }); }); diff --git a/testing/testing.cc b/testing/testing.cc index 712b2d1..662d6cd 100644 --- a/testing/testing.cc +++ b/testing/testing.cc @@ -1,6 +1,9 @@ +#include #include +#include #include +#include #include @@ -21,7 +24,8 @@ int main(int argc, char* argv[]) return run(argc, argv); } -go_bandit([](){ +go_bandit([]() +{ if ( ! file_exists("data")) mkdir("data", 0755); diff --git a/threads.cc b/threads.cc index 940c635..c12233b 100644 --- a/threads.cc +++ b/threads.cc @@ -23,15 +23,15 @@ void os::system_thread_fn(State& state) gettimeofday(&timer, NULL); struct tm * now = gmtime(&timer.tv_sec); - Logger cpu_logger("data/logs/system/CPU."+ to_string(now->tm_year+1900) +"-"+ to_string(now->tm_mon) +"-"+ + Logger cpu_logger("data/logs/system/CPU."+ to_string(now->tm_year+1900) +"-"+ to_string(now->tm_mon+1) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ to_string(now->tm_min) +"-"+ to_string(now->tm_sec) +".log", "CPU"); - Logger ram_logger("data/logs/system/RAM."+ to_string(now->tm_year+1900) +"-"+ to_string(now->tm_mon) +"-"+ + Logger ram_logger("data/logs/system/RAM."+ to_string(now->tm_year+1900) +"-"+ to_string(now->tm_mon+1) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ to_string(now->tm_min) +"-"+ to_string(now->tm_sec) +".log", "RAM"); - Logger temp_logger("data/logs/system/Temp."+ to_string(now->tm_year+1900) +"-"+ to_string(now->tm_mon) +"-"+ + Logger temp_logger("data/logs/system/Temp."+ to_string(now->tm_year+1900) +"-"+ to_string(now->tm_mon+1) +"-"+ to_string(now->tm_mday) +"."+ to_string(now->tm_hour) +"-"+ to_string(now->tm_min) +"-"+ to_string(now->tm_sec) +".log", "Temp"); @@ -42,34 +42,53 @@ void os::system_thread_fn(State& state) while (state != SHUT_DOWN) { + if (get_available_disk_space() < 2000000000) + { + Camera::get_instance().stop(); + } + ifstream cpu_temp_file("/sys/class/thermal/thermal_zone0/temp"); string cpu_temp_str((istreambuf_iterator(cpu_temp_file)), istreambuf_iterator()); cpu_temp_file.close(); gpu_temp_process = popen("/opt/vc/bin/vcgencmd measure_temp", "r"); - fgets(gpu_response, 11, gpu_temp_process); + if (fgets(gpu_response, 11, gpu_temp_process) != NULL) { + try + { + temp_logger.log("CPU: "+to_string(stoi(cpu_temp_str)/1000.0)+ + " GPU: "+string(gpu_response).substr(5, 4)); + } + catch (const invalid_argument& ia) {} + } pclose(gpu_temp_process); - temp_logger.log("CPU: "+to_string(stoi(cpu_temp_str)/1000.0)+" GPU: "+ - string(gpu_response).substr(5, 4)); - cpu_command_process = popen("grep 'cpu ' /proc/stat", "r"); - fgets(cpu_command, 100, cpu_command_process); + if (fgets(cpu_command, 100, cpu_command_process) != NULL) + { + const string cpu_command_str = string(cpu_command); + stringstream ss(cpu_command_str); + string data; + vector s_data; + + // We put all fields in a vector + while(getline(ss, data, ' ')) + { + s_data.push_back(data); + } + + if (s_data.size() >= 6) + { + try { + // Note that s_data[1] is "" + cpu_logger.log(to_string((stof(s_data[2])+stof(s_data[4]))/ + (stof(s_data[2])+stof(s_data[4])+stof(s_data[5])))); + } + catch (const invalid_argument& ia) {} + } + } pclose(cpu_command_process); - const string cpu_command_str = string(cpu_command); - stringstream ss(cpu_command_str); - string data; - vector s_data; - - // We put all fields in a vector - while(getline(ss, data, ' ')) s_data.push_back(data); - - // Note that s_data[1] is "" - cpu_logger.log(to_string((stof(s_data[2])+stof(s_data[4]))/ - (stof(s_data[2])+stof(s_data[4])+stof(s_data[5])))); - sysinfo(&info); ram_logger.log(to_string(((double) info.freeram)/info.totalram)); @@ -141,12 +160,37 @@ void os::battery_thread_fn(State& state) while (state != SHUT_DOWN) { - if (GSM::get_instance().get_status()) + if (GSM::get_instance().is_on()) { GSM::get_instance().get_battery_status(main_battery, gsm_battery); logger.log("Main: "+ to_string(main_battery)); logger.log("GSM: "+ to_string(gsm_battery)); } + else + { + for (int i = 0; i < 5; ++i) + { + this_thread::sleep_for(2min); + if (state == SHUT_DOWN) + { + return; + } + } + if ( ! GSM::get_instance().is_on()) + { + GSM::get_instance().turn_on(); + this_thread::sleep_for(30s); + } + + GSM::get_instance().get_battery_status(main_battery, gsm_battery); + logger.log("Main: "+ to_string(main_battery)); + logger.log("GSM: "+ to_string(gsm_battery)); + + if (state == GOING_DOWN || GPS::get_instance().get_altitude() > 1200) + { + GSM::get_instance().turn_off(); + } + } this_thread::sleep_for(3min); } diff --git a/utils.cc b/utils.cc index 8491e07..2063a29 100644 --- a/utils.cc +++ b/utils.cc @@ -18,18 +18,26 @@ void os::check_or_create(const string& path, Logger* logger) if ( ! file_exists(path)) { if (logger != NULL) + { logger->log("No '"+path+"' directory, creating..."); + } #ifdef DEBUG else + { cout << "[OpenStratos] No '"+path+"' directory, creating..." << endl; + } #endif if (mkdir(path.c_str(), 0755) != 0) { if (logger != NULL) + { logger->log("Error creating '"+path+"' directory."); + } #ifdef DEBUG else + { cout << "[OpenStratos] Error creating '"+path+"' directory." << endl; + } #endif sync(); @@ -38,10 +46,14 @@ void os::check_or_create(const string& path, Logger* logger) else { if (logger != NULL) + { logger->log("'"+path+"' directory created."); + } #ifdef DEBUG else + { cout << "[OpenStratos] '"+path+"' directory created." << endl; + } #endif } } @@ -63,14 +75,38 @@ State os::get_last_state() istreambuf_iterator()); state_file.close(); - if (state_str == "INITIALIZING") return INITIALIZING; - if (state_str == "ACQUIRING_FIX") return ACQUIRING_FIX; - if (state_str == "FIX_ACQUIRED") return FIX_ACQUIRED; - if (state_str == "WAITING_LAUNCH") return WAITING_LAUNCH; - if (state_str == "GOING_UP") return GOING_UP; - if (state_str == "GOING_DOWN") return GOING_DOWN; - if (state_str == "LANDED") return LANDED; - if (state_str == "SHUT_DOWN") return SHUT_DOWN; + if (state_str == "INITIALIZING") + { + return INITIALIZING; + } + if (state_str == "ACQUIRING_FIX") + { + return ACQUIRING_FIX; + } + if (state_str == "FIX_ACQUIRED") + { + return FIX_ACQUIRED; + } + if (state_str == "WAITING_LAUNCH") + { + return WAITING_LAUNCH; + } + if (state_str == "GOING_UP") + { + return GOING_UP; + } + if (state_str == "GOING_DOWN") + { + return GOING_DOWN; + } + if (state_str == "LANDED") + { + return LANDED; + } + if (state_str == "SHUT_DOWN") + { + return SHUT_DOWN; + } return SAFE_MODE; } @@ -106,4 +142,5 @@ const string os::state_to_string(State state) case SAFE_MODE: return "SAFE_MODE"; } + return ""; } diff --git a/utils.h b/utils.h index 2080550..54c9cfa 100644 --- a/utils.h +++ b/utils.h @@ -5,6 +5,9 @@ #include #include +#ifdef SIM + #include +#endif #include #include @@ -48,55 +51,94 @@ namespace os { inline State get_real_state() { + for (int i = 0; i < 10 && ( ! GPS::get_instance().is_fixed() || + GPS::get_instance().get_VDOP() > MAX_DOP); ++i) + { + this_thread::sleep_for(1s); + } + double start_alt = GPS::get_instance().get_altitude(); this_thread::sleep_for(5s); double end_alt = GPS::get_instance().get_altitude(); - if (end_alt - start_alt < -10) return set_state(GOING_DOWN); - else if (end_alt - start_alt > 5) return set_state(GOING_UP); - else if (end_alt > 8000) return set_state(GOING_DOWN); - else return set_state(LANDED); + if (end_alt - start_alt < -10) + { + return set_state(GOING_DOWN); + } + else if (end_alt - start_alt > 5) + { + return set_state(GOING_UP); + } + else if (end_alt > 1500) + { + return set_state(GOING_DOWN); + } + else + { + return set_state(LANDED); + } } inline bool has_launched(double launch_altitude) { - for (int i = 0; ! GPS::get_instance().is_fixed() && i < 10; ++i); - if ( ! GPS::get_instance().is_fixed()) return false; + for (int i = 0; i < 10 && ( ! GPS::get_instance().is_fixed() || + GPS::get_instance().get_VDOP() > MAX_DOP); ++i) + { + this_thread::sleep_for(500ms); + } + + if ( ! GPS::get_instance().is_fixed()) + { + return false; + } double first_altitude = GPS::get_instance().get_altitude(); - if (first_altitude > launch_altitude + 100) return true; + if (first_altitude > launch_altitude + 25) return true; this_thread::sleep_for(5s); - double second_altitude = GPS::get_instance().get_altitude(); #if defined SIM || defined REAL_SIM return true; #else - return second_altitude > first_altitude + 10; + double second_altitude = GPS::get_instance().get_altitude(); + return second_altitude > first_altitude + 8; #endif } inline bool has_bursted(double maximum_altitude) { - for (int i = 0; ! GPS::get_instance().is_fixed() && i < 10; ++i); - if ( ! GPS::get_instance().is_fixed()) return false; + for (int i = 0; i < 10 && ( ! GPS::get_instance().is_fixed() || + GPS::get_instance().get_VDOP() > MAX_DOP); ++i) + { + this_thread::sleep_for(500ms); + } + + if ( ! GPS::get_instance().is_fixed()) + { + return false; + } double first_altitude = GPS::get_instance().get_altitude(); if (first_altitude < maximum_altitude - 1000) return true; this_thread::sleep_for(6s); - double second_altitude = GPS::get_instance().get_altitude(); #if defined SIM || defined REAL_SIM return true; #else + double second_altitude = GPS::get_instance().get_altitude(); return second_altitude < first_altitude - 10; #endif } inline bool has_landed() { - for (int i = 0; ! GPS::get_instance().is_fixed() && i < 10; ++i); + for (int i = 0; i < 10 && ( ! GPS::get_instance().is_fixed() || + GPS::get_instance().get_VDOP() > MAX_DOP); ++i) + { + this_thread::sleep_for(500ms); + } + if ( ! GPS::get_instance().is_fixed()) return false; double first_altitude = GPS::get_instance().get_altitude(); @@ -105,6 +147,101 @@ namespace os { return abs(first_altitude-second_altitude) < 5; } + + inline bool wait_up_for(double altitude, double& maximum_altitude) + { + #if defined SIM && !defined REAL_SIM + this_thread::sleep_for(2min); + + if (altitude > FLIGHT_MAX_HEIGHT) + { + maximum_altitude = FLIGHT_MAX_HEIGHT; + return true; + } + else + { + maximum_altitude = altitude; + return false; + } + #elif defined REAL_SIM && !defined SIM + if (altitude > FLIGHT_MAX_HEIGHT) + { + double sleep = (FLIGHT_MAX_HEIGHT - maximum_altitude)/ASCENT_VELOCITY; + this_thread::sleep_for(std::chrono::duration(sleep)); + maximum_altitude = FLIGHT_MAX_HEIGHT; + return true; + } + else + { + double sleep = (altitude - maximum_altitude)/ASCENT_VELOCITY; + this_thread::sleep_for(std::chrono::duration(sleep)); + maximum_altitude = altitude; + return false; + } + #else + bool bursted; + while ( ! (bursted = has_bursted(maximum_altitude))) + { + for (int i = 0; i < 10 && ( ! GPS::get_instance().is_fixed() || + GPS::get_instance().get_VDOP() > MAX_DOP); ++i) + { + this_thread::sleep_for(500ms); + } + + double current_altitude = GPS::get_instance().get_altitude(); + if (current_altitude > maximum_altitude) + { + maximum_altitude = current_altitude; + } + + if (current_altitude >= altitude) + { + break; + } + } + return bursted; + #endif + } + + inline bool wait_down_for(double altitude) { + #if defined SIM || defined REAL_SIM + if (altitude > FLIGHT_MAX_HEIGHT) + { + return false; + } + #endif + #if defined SIM && !defined REAL_SIM + this_thread::sleep_for(1min); + return false; + #elif defined REAL_SIM && !defined SIM + static double last_altitude; + + if (last_altitude > FLIGHT_MAX_HEIGHT) { + double sleep = (FLIGHT_MAX_HEIGHT-altitude)/DESCENT_VELOCITY; + this_thread::sleep_for(std::chrono::duration(sleep)); + } else { + double sleep = (last_altitude-altitude)/DESCENT_VELOCITY; + this_thread::sleep_for(std::chrono::duration(sleep)); + } + + last_altitude = altitude; + return false; + #else + for (int i = 0; i < 10 && ( ! GPS::get_instance().is_fixed() || + GPS::get_instance().get_VDOP() > MAX_DOP); ++i) + { + this_thread::sleep_for(500ms); + } + + while (GPS::get_instance().get_altitude() > altitude && + ! has_landed()) + { + this_thread::sleep_for(3s); + } + + return has_landed(); + #endif + } } #endif // UTILS_H_