(WS19-01)Wetterballon Entwicklung: Unterschied zwischen den Versionen

Aus Verteilte Systeme - Wiki
Wechseln zu: Navigation, Suche
(Quellcode Master)
(Quellcode Sensoren)
Zeile 1.271: Zeile 1.271:
 
Eine kleine Funktion zum Testen:
 
Eine kleine Funktion zum Testen:
 
<div class="mw-collapsible-content">
 
<div class="mw-collapsible-content">
 +
</div>
 +
</div>
  
 
== Versenden einer SMS über den Pi ==
 
== Versenden einer SMS über den Pi ==

Version vom 18. März 2020, 11:43 Uhr

Formalia

Um einen einheitlichen Stil bei der Entwicklung einzuhalten, haben wir vorarb einen C-Styleguide aufgesetzt. Die Dokumentation erfolgt über Doxygen. Für die Speicherung der Messdaten haben wir uns für das CSV-Format entschieden, da dies einfach und schnell auszuwerten ist. Nachfolgend sind die jeweiligen Artikel zu finden.

C-Styleguide

CSV-Format

Doxygen

Implementierung

Als Implementierungssprache standen Python und C zur Auswahl. Unsere Wahl fiel auf C, da damit alle Gruppenmitglieder schon erste Erfahrungen sammeln konnten. Zur Ansteuerung der über I²C angeschlossenen Sensoren standen uns die Schnittstellen i2c-dev, WiringPi und die Registermanipulation mittels der bcm2835-Bibliothek zur Auswahl. Um die passende Methode zu finden, haben wir uns mit allen Varianten beschäftigt und die Vor- und Nachteile wie folgt zusammengefasst:

i2c-dev:
+ einfache Verwendung
- adapter_nr muss ermittelt werden
- Umweg über Filesystem -> schlechtere Performance

WiringPi:
+ sehr einfache Verwendung
o bis auf Setup, zu unflexible Funktionen (Alternative: Verwendung von write und read)
- Umweg über Filesystem -> schlechtere Performance

Low Level (Registermanipulation):
+ potentiell beste Performance
+ maximale Kontrolle
+ Unabhängigkeit von I2C-Bibliotheken -> bessere Portabilität bei identischem Prozessor auf unterschiedlichen Betriebssystemen
o root-only
- komplexer -> fehleranfälliger, zeitraubender, aber es gibt eine gute Vorlage zur Implementierung

Nach der Abwägung der Vor- und Nachteile haben wir uns für eine Implementierung mittels der bcm2835-Bibliothek entschieden, da uns die bessere Performance den größten Vorteil bei der Erfüllung der Anforderungen bot, auch wenn die Benutzerfreundlichkeit darunter litt.

Quellcode Master

Nach dem in der Designphase

master.h
/**
 * @file  master.h
 * @brief Definitions for the Master program.
 *
 * @author Jonas Gaida, Florian Schwarz
 */

#ifndef INCLUDE_MASTER_H_
#define INCLUDE_MASTER_H_

#include "sensor.h"

#include "sensor_gyml8511.h"
#include "sensor_mcp9808.h"
#include "sensor_mpu9250.h"
#include "sensor_ms5611_out.h"
#include "sensor_ms8607_out.h"
#include "sensor_ms8607_in.h"
#include "sensor_sht31d_out.h"
#include "sensor_sht35_out.h"
#include "sensor_sht35_in.h"
#include "sensor_tmp117.h"
#include "sensor_camm8.h"

#include "sensor_height_sim.h" // Simulates flight height data

#include "sensor_buzzer.h"

#include "camera_raspicam.h"
#include "camera_usb.h"


/**
 * @brief Array of all registered sensors.
 *
 * This needs to be manually filled.
 */
const i2c_sensor_reg SENSORS[] = {
	MS8607_OUT,
	MS5611_OUT,
	MS8607_IN,
	SHT35_OUT,
	SHT31D_OUT,
	SHT35_IN,
	TMP117,
	MCP9808,
	MPU9250,
	GYML8511,
	CAMM8,
	HEIGHT_SIM,
	BUZZER
};


/**
 * @brief Constant for easy access to the total number of registered sensors.
 */
const size_t SENSOR_AMT = sizeof SENSORS / sizeof (i2c_sensor_reg);


/**
 * @brief Array of all cameras.
 *
 * This needs to be manually filled.
 */
const camera CAMERAS[] = {
	RASPICAM,
	USBCAM
};


/**
 * @brief Constant for easy access to the total number of cameras.
 */
const size_t CAMERA_AMT = sizeof CAMERAS / sizeof (camera);


/**
 * @brief Path of the persistence file for the phase.
 */
const char *const PERSISTENCE_PATH = "./phase";



/**
 * @brief The phase in which the master currently is.
 */
extern phase active_phase;



int main(void);


// Signal handler functions

/**
 * @brief Reads the phase saved in the file at PERSISTENCE_PATH.
 *
 * This method only reads if the file exists. Otherwise it does not create it
 * with a default phase. Instead it just returns unsuccessfully.
 *
 * @param[out] p Pointer to a phase which will be filled.
 *
 * @return @c EXIT_SUCCESS if successful else @c EXIT_FAILURE.
 *
 * @sa PERSISTENCE_PATH
 */
int read_phase(phase *const p);


/**
 * @brief Writes the given phase to the file at PERSISTENCE_PATH.
 *
 * @param[in] p The phase to be written.
 *
 * @return @c EXIT_SUCCESS if successful else @c EXIT_FAILURE.
 *
 * @sa PERSISTENCE_PATH
 */
int write_phase(phase p);


/**
 * @brief Checks the health running master.
 *
 * Implements the function type required by a signal handler.
 *
 * @param[in] sig The signal which triggered the handler call.
 */
void sh_maintenance(int sig);


/**
 * @brief Forces the active phase to be re-read.
 *
 * Implements the function type required by a signal handler.
 *
 * @param[in] sig The signal which triggered the handler call.
 *
 * @sa read_phase, active_phase
 */
void sh_update_phase(int sig);


/**
 * @brief Contains logic for the FALLBACK_STATE_TO_RECOVERY_CHANGER task
 *
 * Ensures that master switches to RECOVERY phase after MAX_FLIGHT_TIME.
 * To withstand forced reboot, this task writes its time for phase change to a file specified by MAX_FLIGHT_TIME_PATH
 *
 * @param[in,out] thiz pointer to own task struct
 * @param[in,out] cur_phase pointer to current active phase
 * @param[in] cur_time current time
 *
 */
int fallback_recovery_task(task *thiz, phase *cur_phase, struct timespec cur_time);
#endif  // INCLUDE_MASTER_H_
master.c
/**
 * @file  master.c
 * @brief Implementation of master.h
 *
 * @author Jonas Gaida, Florian Schwarz
 */

#include "master.h"

#include <stdlib.h>  // free, exit codes
#include <unistd.h>  // calloc
#include <stdio.h>  // fprintf, fflush, stdout, stderr
#include <fcntl.h>  // open
#include <sys/stat.h>  // open flags
#include <signal.h>  // struct sigaction, sigaction
#include <string.h>  // strerror, strlen
#include <time.h>  // clock_gettime
#include <limits.h>  // INT_MAX
#include <errno.h>  // errno

#include "bcm2835.h"
#include "timespec_helper.h"
#include "task.h"
#include "task_led.h"
#include "task_gsm.h"
#include "task_watchdog.h"



/**
 * @brief Signal which triggers the sh_maintenance function.
 *
 * @sa sh_maintenance
 */
static const int MAINTENANCE_SIGNAL = SIGUSR1;

/**
 * @brief Signal which triggers the interaction function.
 *
 * @sa interaction
 */
static const int UPDATE_SIGNAL = SIGUSR2;


/**
 * @brief Maximum time in s until state change to RECOVERY
 *
 * Fallback mechanism, to guarantee final RECOVERY mode.
 */
static const unsigned long MAX_FLIGHT_TIME_S = 3 * 60 * 60; // 3 hours
/**
 * @brief Stores time for phase change to RECOVERY
 */
const char *const MAX_FLIGHT_TIME_PATH = "./max_flight_time.pers";

/**
 * @brief Length of the ID suffix of each sensor's file name.
 *
 * This is used in a @c printf call as a '*' length field. These expect @c int
 * values. Therefor the value of this constant has to stay within @c int
 * boundaries.
 */
static const size_t FORMAT_FILE_ID_LENGTH = 3;

/**
 * @brief Number of milliseconds to wait after a failed init/measurement
 */
static const unsigned long ERR_SEQ_DELAY_MS = 50;

/**
 * @brief Number of milliseconds which each reconnection attempt to an sensor takes
 */
static const unsigned long ERR_PERIOD_MS = 3000;

/**
 * @brief Threshold for how many sequenced errors until sensors transition to failure state
 */
static const unsigned char ERR_THRESHOLD = 3;

/**
 * @brief Struct for connected sensors
 */
static task *tasks = NULL;

/**
 * @brief Struct for connected sensors
 */
static size_t tasks_amt = 0;

/**
 * @brief Struct for connected sensors
 */
static i2c_sensor *s_con = NULL;

/**
 * @brief Amount of actually connected sensors
 */
static size_t s_con_amt = 0;


// Defined as "extern" in master.h.
phase active_phase;


int main(void) {
	int rv = EXIT_SUCCESS; // return value for final exit
	//	int rv_f;  // return value of functions.
	//	int rv_l;  // return value for local sections

	uid_t uid = 0;  // For root check.
	baudrate baudrate = MAX_BAUDRATE;  // Should be highest by master supported I2C-frequency
	task *task = NULL;
	i2c_sensor *sensor = NULL, *sensor_height = NULL, *sensor_gps = NULL;
	size_t sensor_name_length = 0;
	size_t file_name_length = 0;
	char *file_name = NULL;
	ssize_t written = 0;  // For write() calls.
	struct timespec cur_time, diff_time;

	struct sigaction maintenance_action_default;
	struct sigaction update_action_default;
	struct sigaction maintenance_action;
	struct sigaction update_action;
	sigset_t all;
	// TODO-DEL - Signal-Blocking while sensors running not useful in case of program freezing
//	sigset_t normal; 

	const long WATCHDOG_KEEPALIVE_MS = 3000;
	const char *const WATCHPUPPY_SIGNALING_CMD = "killall -SIGUSR1 watchpuppy.sh";
	const long WATCHPUPPY_SIGNALING_CYCLE_S = 25; // needs to be smaller than TIMETOWAKEUP in watchpuppy.sh

	const double CYCLE_LEN_MULT = 1.0; // To globaly manipulate cycle_len

	// Constants for flight phase evaluation by height differences calculated by air pressure

	// Rough false value filtering
	const double HEIGHT_MIN = -500.0; // coast of dead sea
	const double HEIGHT_MAX = 50000.0; // balloon ascension limit + some puffer due to height calculation formular error in higher heights

	const unsigned char HEIGHT_AMT = 10; // Amount to collect, before calculate average vertical speed, false values included
	const double V_VELO_HALT_TO_FLIGHT_THRESHOLD = 0.5; // Threshold in m/s
	const uint16_t V_MOVEMENT_FILTER_MSK = 0xFFFF; // Bitmask to get last 16 Bits
	const uint16_t V_MOVEMENT_LANDED_MSK = 0x0000; // Bitmask to check for enough detected sequential non-movements
	const uint16_t  V_MOVEMENT_FLIGHING_MSK = V_MOVEMENT_FILTER_MSK; // Bitmask to check for enough detected sequential movements

	double height_last = 0.0; // Last height value
	double height_cur = 0.0; // Current height value
	double height_cnt = 0.0; // Counter for all height values, also false values
	double dheight_sum = 0.0; // Sum of delta height values

	double v_velo = 0.0; // vertical velocity in m/s
	uint16_t v_movement_seq = 0; // Bit counter for sequential vertical movement

	const long MASTER_LED_MS = 500;
	const long SENSORS_LED_MS = 100;
	int master_LED_on = 0;
	int sensors_LED_on = 0;


	// Camera configurations
	const camera_mode USB_CAMERA_MODE = PHOTO;
	const char *USB_CAMERA_FILENAME = "imgs/camera_usb_bottom";
	const camera_parameters USB_CAMERA_PARM = {
		0,
		10,    // freq, 1 picture per x seconds
//		1920, // width, too much CPU usage
//		1080, // height
//		1280, // width
//		720, // height
		640, // width
		480, // height
		0
	};
	char usb_camera_is_active = 0;

	const camera_mode RASPICAM_MODE = VIDEO;
	const char *RASPICAM_FILENAME = "vids/raspicam_side";
	const camera_parameters RASPICAM_PARM = {
		3 * 60 * 60,	  // duration, 3 hours, should be larger than flight time
		0,
		1920, // width
		1080, // height
		30    // framerate
	};
	char raspicam_is_active = 0;

	const long GSM_SEND_MS = 180000;  // Every 3 minutes.

	int tmp = 0;  // For unrelated use.

	// FLOW #1
	// Check for root privileges (bcm2835 doesn't do that automatically).
	if ((uid = getuid()) && geteuid() == uid)
	{
		errno = EPERM;
		fprintf(stderr, "%s:%i: root check failed: %s\n", __FILE__, __LINE__, strerror(errno));
		rv = EXIT_FAILURE;
		goto ERR;
	}

	// FLOW #2.1
	// Setup signal handling
	// Maintenance handler
	maintenance_action.sa_flags = 0;
	sigemptyset(&maintenance_action.sa_mask);
	sigaddset(&maintenance_action.sa_mask, MAINTENANCE_SIGNAL);
	maintenance_action.sa_handler = sh_maintenance;
	tmp = sigaction(MAINTENANCE_SIGNAL, &maintenance_action, &maintenance_action_default);
	if (0 > tmp) {
		fprintf(stderr, "%s:%d: setting maintenance handler failed\n", __FILE__, __LINE__);
		rv = EXIT_FAILURE;
		goto ERR;
	}
	// FLOW #2.2
	// Phase update handler
	update_action.sa_flags = 0;
	sigemptyset(&update_action.sa_mask);
	sigaddset(&update_action.sa_mask, UPDATE_SIGNAL);
	update_action.sa_handler = sh_update_phase;
	tmp = sigaction(UPDATE_SIGNAL, &update_action, &update_action_default);
	if (0 > tmp) {
		fprintf(stderr, "%s:%d: setting phase update handler failed\n", __FILE__, __LINE__);
		rv = EXIT_FAILURE;
		goto ERR_SIG_MAINT;
	}
	// Mask to block all signals
	tmp = sigfillset(&all);
	if (-1 == tmp) {
		fprintf(stderr, "%s:%i: filling blocking signal mask failed\n", __FILE__, __LINE__);
		rv = EXIT_FAILURE;
		goto ERR_SIG_UPDT;
	}

	// FLOW #3
	// Try reading a previously saved phase or instead use and write START.
	tmp = read_phase(&active_phase);
	if (EXIT_FAILURE == tmp) {
		active_phase = START;
		tmp = write_phase(active_phase);
		if (EXIT_FAILURE == tmp) {
			fprintf(stderr, "%s:%i: writing initial phase failed\n", __FILE__, __LINE__);
			rv = EXIT_FAILURE;
			goto ERR_SIG_UPDT;
		}
	}
	// FLOW #3.1
	// Update v_movement_seq
	if (FLIGHT == active_phase) {
		v_movement_seq = 0xFF;
	}

	// FLOW #4.1
	// bcm2835_init() comes with error message printing to stderr.
	if (!bcm2835_init()) {
		fprintf(stderr, "%s:%i: bcm2835_init failed\n", __FILE__, __LINE__);
		rv = EXIT_FAILURE;
		goto ERR_SIG_UPDT;
	}

	// FLOW #4.2
	// Initialize I2C functionality.
	if (!bcm2835_i2c_begin()) {
		fprintf(stderr, "%s:%i: I2C initialization failed\n", __FILE__, __LINE__);
		rv = EXIT_FAILURE;
		goto ERR_BCM_LIB_CLOSE;
	}

	// FLOW #5
	// Create and initialize array for connected sensors
	bcm2835_i2c_set_baudrate(MIN_BAUDRATE);
	if (EXIT_FAILURE == sensor_create_init_connected_sensors(SENSORS, SENSOR_AMT, &s_con, &s_con_amt)) {
		fprintf(stderr, "%s:%i: sensor_create_connected_sensors failed\n", __FILE__, __LINE__);
		rv = EXIT_FAILURE;
		goto ERR_BCM_I2C_END;
	}

	// FLOW #6
	// Determine greatest common I2C-frequency
	for (size_t i = 0; s_con_amt > i; i++) {
		if (NULL != s_con[i].reg) {
			if (baudrate > s_con[i].reg->i2c_freq_max) {
				baudrate = MIN_BAUDRATE;
			}
		}
	}
	bcm2835_i2c_set_baudrate(baudrate);

	// FLOW #7.1
	sensor_get_best_by_date_type(s_con, s_con_amt, HEIGHT, &sensor_height);
	if (NULL == sensor_height) {
		fprintf(stderr, "%s:%d: no height sensor found. terminate program.\n", __FILE__, __LINE__);
		rv = EXIT_FAILURE;
		goto ERR_S_CON_DESTROY;
	}

	// FLOW #7.2
	sensor_get_best_by_date_type(s_con, s_con_amt, POS_LAT, &sensor_gps);
	if (NULL == sensor_gps) {
		fprintf(stderr, "%s:%d: no GPS sensor found. Terminate program.\n", __FILE__, __LINE__);
		rv = EXIT_FAILURE;
		goto ERR_S_CON_DESTROY;
	}

	// FLOW #8.1
	// Find length of longest sensor name to allocate sufficient memory for file names.
	if (INT_MAX < FORMAT_FILE_ID_LENGTH) {
		fprintf(stderr, "%s:%i: FORMAT_FILE_ID_LENGTH too large: expected 0..%i, got %zu\n", __FILE__, __LINE__, INT_MAX, FORMAT_FILE_ID_LENGTH);
		rv = EXIT_FAILURE;
		goto ERR_S_CON_DESTROY;
	}

	for (size_t i = 0; s_con_amt > i; ++i) {
		sensor_name_length = strlen(s_con[i].reg->name);
		if (sensor_name_length > file_name_length) {
			file_name_length = sensor_name_length;
		}
	}
	file_name_length += FORMAT_FILE_ID_LENGTH + 6;  // Length of "_###.csv\0".
	file_name = calloc(file_name_length, sizeof (char));
	if (NULL == file_name) {
		fprintf(stderr, "%s:%i: calloc of %zu bytes failed\n", __FILE__, __LINE__, file_name_length * sizeof (char));
		rv = EXIT_FAILURE;
		goto ERR_S_CON_DESTROY;
	}

	// FLOW #8.2
	// CSV-Preperations
	for (size_t i = 0; s_con_amt > i; ++i) {
		// Prepare filenames.
		tmp = snprintf(file_name, file_name_length, "%s_%0*zu.csv", s_con[i].reg->name, (int) FORMAT_FILE_ID_LENGTH, i);
		if (0 > tmp) {
			fprintf(stderr, "%s:%i: buffering file name for %s failed\n", __FILE__, __LINE__, s_con[i].reg->name);
			rv = EXIT_FAILURE;
			goto ERR_FILE_NAME_FREE;
		}

		// Open files.
		tmp = access(file_name, F_OK);  // Check if file exists (for CSV header).
		s_con[i].csv_file = open(file_name, O_CREAT | O_WRONLY | O_APPEND, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
		if (0 > s_con[i].csv_file) {
			fprintf(stderr, "%s:%i: opening file %s failed: %s\n", __FILE__, __LINE__, file_name, strerror(errno));
			rv = EXIT_FAILURE;
			goto ERR_FILE_NAME_FREE;
		}
		// If file did not exist, write header.
		if (tmp) {
			errno = 0;
			written = write(s_con[i].csv_file, s_con[i].reg->csv_header, strlen(s_con[i].reg->csv_header));
			if (0 > written) {
				fprintf(stderr, "%s:%d: writing CSV header failed: %s\n", __FILE__, __LINE__, strerror(errno));
				rv = EXIT_FAILURE;
				goto ERR_FILE_NAME_FREE;
			}
		}
	}

	// FLOW #9.1
	// Create tasks
	tasks_amt = s_con_amt + TASK_TYPE_AMT - 1;
	tasks = malloc(tasks_amt * sizeof (struct task));
	if (NULL == tasks) {
		fprintf(stderr, "%s:%i: malloc of %zu bytes failed\n", __FILE__, __LINE__, file_name_length * sizeof (char));
		rv = EXIT_FAILURE;
		goto ERR_FILE_NAME_FREE;
	}
	// FLOW #9.2
	// Init tasks
	// Sensors
	for (size_t i = 0; s_con_amt > i; i++) {
		tasks[i].type = SENSOR;
		tasks[i].state = RUNNING;
		tasks[i].next_time = (struct timespec) {0, 0};
		tasks[i].p = &(s_con[i]);
		sensor = (i2c_sensor *) (tasks[i].p);
		tasks[i].active_phases = sensor->reg->active_phases;
		s_con[i].t = &(tasks[i]);

	}
	// LED tasks
	tasks[s_con_amt].type = LED_MASTER;
	tasks[s_con_amt].state = RUNNING;
	tasks[s_con_amt].active_phases = START | ASCEND;
	errno = 0;
	if (0 > clock_gettime(CLOCK_MONOTONIC, &(tasks[s_con_amt].next_time))) {
		fprintf(stderr, "%s:%i: acquiring time failed: %s\n", __FILE__, __LINE__, strerror(errno));
		rv = EXIT_FAILURE;
		goto ERR_TASKS_FREE;
	}
	tasks[s_con_amt + 1].type = LED_SENSORS;
	tasks[s_con_amt + 1].state = RUNNING;
	tasks[s_con_amt + 1].active_phases = START | ASCEND;
	errno = 0;
	if (0 > clock_gettime(CLOCK_MONOTONIC, &(tasks[s_con_amt + 1].next_time))) {
		fprintf(stderr, "%s:%i: acquiring time failed: %s\n", __FILE__, __LINE__, strerror(errno));
		rv = EXIT_FAILURE;
		goto ERR_TASKS_FREE;
	}
	// Fallback task for state change, if sensors or state transitions fail
	tasks[s_con_amt + 2].type = FALLBACK_STATE_TO_RECOVERY_CHANGER;
	tasks[s_con_amt + 2].state = RUNNING;
	tasks[s_con_amt + 2].active_phases = START | FLIGHT | RECOVERY;
	tasks[s_con_amt + 2].next_time = (struct timespec) {0, 0};
	tasks[s_con_amt + 2].p = NULL;
	// Camera USB task
	tasks[s_con_amt + 3].type = CAM_USB_TASK;
	tasks[s_con_amt + 3].state = RUNNING;
	tasks[s_con_amt + 3].active_phases = START | FLIGHT | RECOVERY;
	tasks[s_con_amt + 3].next_time = (struct timespec) {0, 0};
	// Camera raspi task
	tasks[s_con_amt + 4].type = CAM_RASPI_TASK;
	tasks[s_con_amt + 4].state = RUNNING;
	tasks[s_con_amt + 4].active_phases = START | FLIGHT | RECOVERY;
	tasks[s_con_amt + 4].next_time = (struct timespec) {0, 0};
	// GSM sender task
	tasks[s_con_amt + 5].type = GSM_TASK;
	tasks[s_con_amt + 5].state = RUNNING;
	tasks[s_con_amt + 5].active_phases = START | FLIGHT | RECOVERY;
	errno = 0;
	if (0 > clock_gettime(CLOCK_MONOTONIC, &(tasks[s_con_amt + 5].next_time))) {
		fprintf(stderr, "%s:%i: acquiring time failed: %s\n", __FILE__, __LINE__, strerror(errno));
		rv = EXIT_FAILURE;
		goto ERR_TASKS_FREE;
	}
	// Watchdog task
	tasks[s_con_amt + 6].type = WATCHDOG;
	tasks[s_con_amt + 6].state = RUNNING;
	tasks[s_con_amt + 6].active_phases = START | FLIGHT | RECOVERY;
	errno = 0;
	if (0 > clock_gettime(CLOCK_MONOTONIC, &(tasks[s_con_amt + 6].next_time))) {
		fprintf(stderr, "%s:%i: acquiring time failed: %s\n", __FILE__, __LINE__, strerror(errno));
		rv = EXIT_FAILURE;
		goto ERR_TASKS_FREE;
	}
	// Watchpuppy task
	tasks[s_con_amt + 7].type = WATCHPUPPY;
	tasks[s_con_amt + 7].state = RUNNING;
	tasks[s_con_amt + 7].active_phases = START | FLIGHT | RECOVERY;
	errno = 0;
	if (0 > clock_gettime(CLOCK_MONOTONIC, &(tasks[s_con_amt + 7].next_time))) {
		fprintf(stderr, "%s:%i: acquiring time failed: %s\n", __FILE__, __LINE__, strerror(errno));
		rv = EXIT_FAILURE;
		goto ERR_TASKS_FREE;
	}

	// FLOW #10
	// Update states by phases
	if (EXIT_FAILURE == task_update_states_by_phase(tasks, tasks_amt, active_phase)) {
		fprintf(stderr, "%s:%i: task_update_states_by_phase failed\n", __FILE__, __LINE__);
		rv = EXIT_FAILURE;
		goto ERR_TASKS_FREE;
	}

	// Main loop
	while (QUIT != active_phase && EXIT_SUCCESS == task_get_next(tasks, tasks_amt, &task)) {

		// Is task ready? If not, sleep remaining time
		if (0 > clock_gettime(CLOCK_MONOTONIC, &cur_time)) {
			fprintf(stderr, "%s:%i: acquiring time failed: %s\n", __FILE__, __LINE__, strerror(errno));
			rv = EXIT_FAILURE;
			goto ERR_TASKS_FREE;
		}
		if (timespec_gt(task->next_time, cur_time)) {
			diff_time = timespec_sub(task->next_time, cur_time);
			do {
				errno = 0;
			} while (0 > clock_nanosleep(CLOCK_MONOTONIC, 0,&diff_time, &diff_time) && EINTR == errno);
		}

		switch (task->type) {
			case SENSOR:

			sensor = (i2c_sensor*) (task->p);
				// Sensor measurement cycle
				// Repeat, as long as sensor isn't sleeping and there's no delay
				do {

					// TODO-DEL - Signal-Blocking while sensors running not useful in case of program freezing
//					// Block all signals.
//					tmp = sigprocmask(SIG_BLOCK, &all, &normal);
//					if (-1 == tmp) {
//						fprintf(stderr, "%s:%i: setting blocking signal mask failed\n", __FILE__, __LINE__);
//					}

					switch (sensor->state) {

						case INIT:
							if (EXIT_FAILURE == sensor->reg->init(active_phase, &(sensor->it_rem), &(sensor->processing_time))) { // Init_Err
								fprintf(stderr, "%s:%i: init for %s failed\n", __FILE__, __LINE__, sensor->reg->name);
								sensor->err_seq++;
								sensor->err_total++;
								if (ERR_THRESHOLD <= sensor->err_seq) {
									sensor->state = FAILURE;
									sensor->failure_total++;
									if (sensor == sensor_height) {
										sensor_get_best_by_date_type(s_con, s_con_amt, HEIGHT, &sensor_height);
									}
									sensor->processing_time = ERR_PERIOD_MS;
								} else {
									sensor->processing_time = ERR_SEQ_DELAY_MS;
								}
							} else { // Init_Succ
								sensor->err_seq = 0;
								if (0 < sensor->processing_time) {
									// Update next_time
									if (0 > clock_gettime(CLOCK_MONOTONIC, &cur_time)) {
										fprintf(stderr, "%s:%i: acquiring time failed: %s\n", __FILE__, __LINE__, strerror(errno));
										rv = EXIT_FAILURE;
										goto ERR_TASKS_FREE;
									}
									task->next_time = timespec_add_ms(cur_time, sensor->processing_time);
								}
								if (0 < sensor->it_rem) { // Init_Unfinished
									sensor->it_rem--;
								} else { // Init_Finished
									sensor->state = MEASURE;
									sensor->it_rem = -1;
									// Set sensor->next_cycle_time
									if (0 > clock_gettime(CLOCK_MONOTONIC, &(sensor->next_cycle_time))) {
										fprintf(stderr, "%s:%i: acquiring time failed: %s\n", __FILE__, __LINE__, strerror(errno));
										rv = EXIT_FAILURE;
										goto ERR_TASKS_FREE;
									}
									sensor->next_cycle_time = timespec_add_ms(sensor->next_cycle_time, sensor->reg->cycle_len * CYCLE_LEN_MULT);
								}
							}
							break;
						case MEASURE:
							if (EXIT_FAILURE == sensor->reg->measure(active_phase, &(sensor->measurements), &(sensor->it_rem), &(sensor->processing_time))) { // Measure_Err
								fprintf(stderr, "%s:%i: measure for %s failed\n", __FILE__, __LINE__, sensor->reg->name);
								sensor->err_seq++;
								sensor->err_total++;
								if (ERR_THRESHOLD <= sensor->err_seq) {
									sensor->state = FAILURE;
									sensor->failure_total++;
									if (sensor == sensor_height) {
										sensor_get_best_by_date_type(s_con, s_con_amt, HEIGHT, &sensor_height);
									}
									sensor->processing_time = 0;
								} else {
									sensor->processing_time = ERR_SEQ_DELAY_MS;
								}
							} else { // Measure_Succ
								sensor->err_seq = 0;
								if (0 < sensor->it_rem) { // Measure_Unfinished
									sensor->it_rem--;
									if (0 < sensor->processing_time) {
										// Update next_time
										if (0 > clock_gettime(CLOCK_MONOTONIC, &cur_time)) {
											fprintf(stderr, "%s:%i: acquiring time failed: %s\n", __FILE__, __LINE__, strerror(errno));
											rv = EXIT_FAILURE;
											goto ERR_TASKS_FREE;
										}
										task->next_time = timespec_add_ms(cur_time, sensor->processing_time);
									}
								} else { // Measure_Finished
									sensor->it_rem = -1;
									// TODO-UC
//									task->next_time = timespec_add_ms(sensor->next_cycle_time, sensor->processing_time);
//									sensor->next_cycle_time = timespec_add_ms(sensor->next_cycle_time, sensor->reg->cycle_len * CYCLE_LEN_MULT);

									// TODO-TEST-2-B
									if (0 > clock_gettime(CLOCK_MONOTONIC, &cur_time)) {
										fprintf(stderr, "%s:%i: acquiring time failed: %s\n", __FILE__, __LINE__, strerror(errno));
										rv = EXIT_FAILURE;
										goto ERR_TASKS_FREE;
									}
									task->next_time = timespec_add_ms(cur_time, sensor->processing_time);
									sensor->next_cycle_time = timespec_add_ms(sensor->next_cycle_time, sensor->reg->cycle_len * CYCLE_LEN_MULT);
									if (timespec_gt(sensor->next_cycle_time, task->next_time)) {
										task->next_time = sensor->next_cycle_time;
									} else {
										fprintf(stderr, "%s:%i: %ld.%03ld: sensor %s cycle_len exceeded\n", __FILE__, __LINE__, cur_time.tv_sec, cur_time.tv_nsec / 1000000, sensor->reg->name);
										sensor->next_cycle_time = task->next_time;
									}
									// TODO-TEST-2-E

									sensor->processing_time = 1; // Actual value doesn't matter, just to stop sensor loop
									if (EXIT_FAILURE == measurement_append_to_csv_file(sensor->csv_file, sensor->measurements)) {
										fprintf(stderr, "%s:%i: append_to_csv_file for %s's data failed\n", __FILE__, __LINE__, sensor->reg->name);
									}
									// Test for phase switching conditions.
									if (sensor_height == sensor) { // New height value?
										// TODO-MV - move to sensor_height probing an save in pointer
										for (size_t i = 0; sensor->reg->date_amt > i; i++) { // get height
											if (HEIGHT == sensor->reg->types[i]) {
												height_cur = sensor->measurements.data[i];
												break;
											}
										}
										if (HEIGHT_MIN < height_cur || HEIGHT_MAX > height_cur) { // No false value?
											dheight_sum += height_cur - height_last;
											height_last = height_cur;
										}
										height_cnt++;
										if (HEIGHT_AMT == height_cnt) { // Enough values collected? Calculate vert. velocity
											v_velo = dheight_sum / height_cnt / (double) sensor->reg->cycle_len * 1000;
											height_cnt = 0.0;
											dheight_sum = 0.0;

											if (v_velo > V_VELO_HALT_TO_FLIGHT_THRESHOLD || v_velo < (-1 * V_VELO_HALT_TO_FLIGHT_THRESHOLD)) { // Threshold to detect movement exceeded?
												v_movement_seq = v_movement_seq << 1 | 0x01;
												if (START == active_phase && V_MOVEMENT_FLIGHING_MSK == (v_movement_seq & V_MOVEMENT_FILTER_MSK)) { // In START pahse and enough movements detected?
													printf("Phase switched to FLIGHT!\n");
													active_phase = FLIGHT;
													tmp = write_phase(active_phase);
													if (EXIT_FAILURE == tmp) {
														fprintf(stderr, "%s:%i: writing updated phase failed: active_phase=%u\n", __FILE__, __LINE__, active_phase);
													}
													// Update states by phases
													if (EXIT_FAILURE == task_update_states_by_phase(tasks, tasks_amt, active_phase)) {
														fprintf(stderr, "%s:%i: task_update_states_by_phase failed\n", __FILE__, __LINE__);
														rv = EXIT_FAILURE;
														goto ERR_TASKS_FREE;
													}
												}
											} else { // Thresehold not exceeded
												v_movement_seq = (v_movement_seq << 1) | 0x0000;
												if (FLIGHT == active_phase && V_MOVEMENT_LANDED_MSK == (v_movement_seq & V_MOVEMENT_FILTER_MSK)) { // Enough stationaries detected?
													printf("Phase switched to RECOVERY!\n");
													active_phase = RECOVERY;
													tmp = write_phase(active_phase);
													if (EXIT_FAILURE == tmp) {
														fprintf(stderr, "%s:%i: writing updated phase failed: active_phase=%u\n", __FILE__, __LINE__, active_phase);
													}
													// Update states by phases
													if (EXIT_FAILURE == task_update_states_by_phase(tasks, tasks_amt, active_phase)) {
														fprintf(stderr, "%s:%i: task_update_states_by_phase failed\n", __FILE__, __LINE__);
														rv = EXIT_FAILURE;
														goto ERR_TASKS_FREE;
													}
												}
											}
//											printf("s_name = %s\nheight_cur = %lf\nv_velo = %lf\nv_movement_seq = 0x%04x\n\n", sensor->reg->name, height_cur, v_velo, v_movement_seq); // TODO-DEL
										}
									}
								}
							}
							break;
						case FAILURE:
							if (EXIT_FAILURE == sensor->reg->is_connected()) { // Sensor still dead
								fprintf(stderr, "%s:%i: reconnect for %s failed\n", __FILE__, __LINE__, sensor->reg->name);
								sensor->err_total++;
								sensor->processing_time = ERR_PERIOD_MS;
								task->next_time = timespec_add_ms(task->next_time, sensor->processing_time);
							} else { // Sensor recovered
								fprintf(stderr, "%s:%i: reconnect for %s successful\n", __FILE__, __LINE__, sensor->reg->name);
								sensor->err_seq = 0;
								sensor->state = INIT;
								sensor->it_rem = -1;
								if (EXIT_SUCCESS == sensor_has_date_type(*sensor, HEIGHT)) {
									sensor_get_best_by_date_type(s_con, s_con_amt, HEIGHT, &sensor_height);
								}
								sensor->processing_time = 0;
							}
							break;
						default:
							fprintf(stderr, "%s:%i: sensor \"%s\" has illegal state: %d. Stopped sensor task\n", __FILE__, __LINE__, sensor->reg->name, sensor->state);
							task->state = STOPPED;
					}
					// TODO-DEL - Signal-Blocking while sensors running not useful in case of program freezing
//					// Reset signal mask.
//					tmp = sigprocmask(SIG_SETMASK, &normal, NULL);
//					if (-1 == tmp) {
//						fprintf(stderr, "%s:%i: returning to normal signal mask failed\n", __FILE__, __LINE__);
//					}
				} while (SLEEP != sensor->state && 0 >= sensor->processing_time);
				break;

			case LED_MASTER:
				if (START == active_phase) {
					// Blinks if master is running.
					master_LED_on = !master_LED_on;
					set_taskLED(task->type, master_LED_on);

					task->next_time = timespec_add_ms(task->next_time, MASTER_LED_MS);
				} else {
					set_taskLED(task->type, 0);
					task->state = STOPPED;
				}
				break;

			case LED_SENSORS:
				if (START == active_phase) {
					// Count current failures.
					tmp = 0;
					for (size_t i = 0; i < s_con_amt; ++i) {
						if (FAILURE == s_con[i].state) {
							++tmp;
						}
					}

					// Disconnected sensors or current failures -> off.
					if (SENSOR_AMT != s_con_amt || tmp) {
						sensors_LED_on = 0;
						// Any past failures -> blinking; no failures at all -> on.
					} else {
						tmp = 0;
						for (size_t i = 0; i < s_con_amt; ++i) {
							tmp += s_con[i].failure_total;
						}
						sensors_LED_on = tmp ? !sensors_LED_on : 1;
					}
					set_taskLED(task->type, sensors_LED_on);

					task->next_time = timespec_add_ms(task->next_time, SENSORS_LED_MS);
				} else {
					set_taskLED(task->type, 0);
					task->state = STOPPED;
				}
				break;

			case CAM_RASPI_TASK:
				switch (active_phase) {
					case RECOVERY:
						if (raspicam_is_active) {
							CAMERAS[0].stop();
							raspicam_is_active = 0;
						}
						break;
					default: // START, FLIGHT, etc
						if (!raspicam_is_active) {
							CAMERAS[0].start(RASPICAM_MODE, RASPICAM_FILENAME, RASPICAM_PARM);
							raspicam_is_active = 1;
						}
				}
				task->state = STOPPED;
				break;

			case CAM_USB_TASK:
				switch (active_phase) {
					case RECOVERY:
						if (raspicam_is_active) {
							CAMERAS[1].stop();
							usb_camera_is_active = 0;
						}
						break;
					default: // START, FLIGHT, etc
						if (!usb_camera_is_active) {
							CAMERAS[1].start(USB_CAMERA_MODE, USB_CAMERA_FILENAME, USB_CAMERA_PARM);
							usb_camera_is_active = 1;
						}
				}
				task->state = STOPPED;
				break;

			case GSM_TASK:
//				send_GSM(&sensor_gps->measurements); // TODO-UC
				task->next_time = timespec_add_ms(task->next_time, GSM_SEND_MS);
				break;

			case FALLBACK_STATE_TO_RECOVERY_CHANGER:
				if (EXIT_FAILURE == fallback_recovery_task(task, &active_phase, cur_time)) {
					fprintf(stderr, "%s:%s:%i: fallback_recovery_task failed\n", __FILE__, __func__, __LINE__);
					rv = EXIT_FAILURE;
					goto ERR_TASKS_FREE;
				}
				break;

			case WATCHDOG:
				wake_watchdog();
				task->next_time = timespec_add_ms(task->next_time, WATCHDOG_KEEPALIVE_MS);
				break;

			case WATCHPUPPY:
				system(WATCHPUPPY_SIGNALING_CMD);
				task->next_time = timespec_add_ms(task->next_time, WATCHPUPPY_SIGNALING_CYCLE_S * 1000);
				break;

			default:
				fprintf(stderr, "%s:%i: task with faulty task_type\n", __FILE__, __LINE__);
		}
	}

	// Cleanup in reverse order.
ERR_TASKS_FREE:
	free(tasks);
ERR_FILE_NAME_FREE:
	free(file_name);
ERR_S_CON_DESTROY:
	for (size_t i = 0; s_con_amt > i; i++) {
		measurement_destroy(&(s_con[i].measurements));
	}
	free(s_con);
ERR_BCM_I2C_END:
	bcm2835_i2c_end();
ERR_BCM_LIB_CLOSE:
	if (!bcm2835_close()) {
		rv = EXIT_FAILURE;
	}
ERR_SIG_UPDT:
	tmp = sigaction(UPDATE_SIGNAL, &update_action_default, NULL);
	if (0 > tmp) {
		fprintf(stderr, "%s:%d: resetting phase update handler to default failed\n", __FILE__, __LINE__);
		rv = EXIT_FAILURE;
	}
ERR_SIG_MAINT:
	tmp = sigaction(MAINTENANCE_SIGNAL, &maintenance_action_default, NULL);
	if (0 > tmp) {
		fprintf(stderr, "%s:%d: resetting maintenance handler to default failed\n", __FILE__, __LINE__);
		rv = EXIT_FAILURE;
	}
ERR:
	return rv;
}


int read_phase(phase *const p) {
	FILE *file = NULL;
	int tmp = 0;
	int rv = EXIT_SUCCESS;

	if (NULL == p) {
		fprintf(stderr, "%s:%i: no phase given\n", __FILE__, __LINE__);
		return EXIT_FAILURE;
	}

	/* Automated creation not specified in header.
	// File doesn't exist.
	tmp = access(file_name, F_OK);
	if (tmp) {
	 *p = START;
	 return write_phase(*p);
	 }
	 */

	file = fopen(PERSISTENCE_PATH, "r");
	if (NULL == file) {
		fprintf(stderr, "%s:%i: opening persistence file for reading failed: %s\n", __FILE__, __LINE__, strerror(errno));
		return EXIT_FAILURE;
	}

	errno = 0;
	tmp = fscanf(file, "%u", p);
	if (1 != tmp) {
		fprintf(stderr, "%s:%i: scanning for phase failed: %s\n", __FILE__, __LINE__, 0 == errno ? "no value" : strerror(errno));
		rv = EXIT_FAILURE;
	}

	tmp = fclose(file);
	if (tmp) {
		fprintf(stderr, "%s:%i: closing persistence file after reading failed: %s\n", __FILE__, __LINE__, strerror(errno));
		return EXIT_FAILURE;
	}

	return rv;
}


int write_phase(phase p) {
	FILE *file = NULL;
	int tmp = 0;

	file = fopen(PERSISTENCE_PATH, "w");
	if (NULL == file) {
		fprintf(stderr, "%s:%i: opening persistence file for writing failed: %s\n", __FILE__, __LINE__, strerror(errno));
		return EXIT_FAILURE;
	}

	tmp = fprintf(file, "%i", p);
	if (0 >= tmp) {
		fprintf(stderr, "%s:%i: printing phase failed\n", __FILE__, __LINE__);
		return EXIT_FAILURE;
	}

	tmp = fclose(file);
	if (tmp) {
		fprintf(stderr, "%s:%i: closing persistence file after writing failed: %s\n", __FILE__, __LINE__, strerror(errno));
		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}



void sh_maintenance(int sig) {
	unsigned long total_failures = 0;
	size_t in_failure = 0;

	size_t i_all = 0;

	if (MAINTENANCE_SIGNAL == sig) {
		// Collect combined metrics.
		for (size_t i = 0; i < s_con_amt; ++i) {
			total_failures += s_con[i].failure_total;
			if (FAILURE == s_con[i].state) {
				++in_failure;
			}
		}

		// TODO-IMPL - Show master status as following:
		// Master-Status CRITICAL, all height sensors in FAILURE, or no height sensor
		printf("\n[%s][%s] %-10s (", 0 == total_failures ? "\033[0;92m GOOD \033[0m" : (0 == in_failure ? "\033[0;93m OKAY \033[0m" : "\033[0;33m ALRM \033[0m"), 0 < s_con_amt ? "\033[0;36mACTIVE\033[0m" : "\033[0;30m DEAD \033[0m", "master");
		switch (active_phase) {
			// Single phases.
			case START:
				printf("START");
				break;
			case ASCEND:
				printf("ASCEND");
				break;
			case DESCEND:
				printf("DESCEND");
				break;
			case RECOVERY:
				printf("RECOVERY");
				break;
				// Combined phases (name strings ending with '*').
			case FLIGHT:
				printf("FLIGHT*");
				break;
			default:
				printf("\033[0;90m?active_phase=%u?\033[0m", active_phase);
		}
		printf(" phase, %zu/%zu sensors connected, %lu failures total, %zu sensors in FAILURE)\n\n", s_con_amt, SENSOR_AMT, total_failures, in_failure);

		printf("[ COND ][  TASK   ][STATE ]    NAME    (ERROR STATISTICS)\n");
		for (size_t i = 0; i < s_con_amt; ++i) {
			printf("[%s][%s][%s] %-10s (%lu failures total, %lu errors total, %u erorrs in sequence)\n",
					0 == s_con[i].err_total ? "\033[0;92m GOOD \033[0m" : (FAILURE == s_con[i].state ? "\033[0;91m FAIL \033[0m" : (0 == s_con[i].err_seq ? "\033[0;33m OKAY \033[0m" : "\033[0;33mINTRUP\033[0m")),
					RUNNING == s_con[i].t->state ? "\033[0;92m RUNNING \033[0m" : "\033[0;34m STOPPED \033[0m",
					INIT == s_con[i].state ? "\033[0;95m INIT \033[0m" : (!s_con[i].reg->active_phases ? "\033[0;35mHYBERN\033[0m" : (s_con[i].reg->active_phases & active_phase ? "\033[0;36mACTIVE\033[0m" : "\033[0;34mASLEEP\033[0m")),
					s_con[i].reg->name, s_con[i].failure_total, s_con[i].err_total, s_con[i].err_seq);
		}
		if (SENSOR_AMT > s_con_amt) {
			for (size_t i_con = 0; SENSOR_AMT > i_all && i_con < s_con_amt; ++i_all) {
				if (&SENSORS[i_all] == s_con[i_con].reg) {
					++i_con;
				} else {
					printf("[\033[0;30mDISCON\033[0m][      ] %-10s\n", SENSORS[i_all].name);
				}
			}
			for (; SENSOR_AMT > i_all; ++i_all) {
				printf("[\033[0;30mDISCON\033[0m][      ] %-10s\n", SENSORS[i_all].name);
			}
		}

		fflush(stdout);
	}
}

void sh_update_phase(int sig) {
	sigset_t all;
	sigset_t normal;
	int tmp = 0;

	tmp = sigfillset(&all);
	if (-1 == tmp) {
		fprintf(stderr, "%s:%i: filling blocking signal mask failed\n", __FILE__, __LINE__);
	}
	tmp = sigprocmask(SIG_BLOCK, &all, &normal);
	if (-1 == tmp) {
		fprintf(stderr, "%s:%i: setting blocking signal mask failed\n", __FILE__, __LINE__);
	}

	if (UPDATE_SIGNAL == sig) {
		tmp = read_phase(&active_phase);
		if (EXIT_FAILURE == tmp) {
			fprintf(stderr, "%s:%i: reading phase on signal failed\n", __FILE__, __LINE__);
		}
		tmp = task_update_states_by_phase(tasks, tasks_amt, active_phase);
		if (EXIT_FAILURE == tmp) {
			fprintf(stderr, "%s:%s:%i: task_update_states_by_phase failed\n", __FILE__, __func__, __LINE__);
		}
	}

	tmp = sigprocmask(SIG_SETMASK, &normal, NULL);
	if (-1 == tmp) {
		fprintf(stderr, "%s:%i: returning to normal signal mask failed\n", __FILE__, __LINE__);
	}
}


int fallback_recovery_task(task *thiz, phase *cur_phase, struct timespec cur_time) {
	FILE *file = NULL;
	int rv = EXIT_SUCCESS;
	int rv_f = 0;

	if (NULL == thiz) {
		fprintf(stderr, "%s:%s:%i: Input thiz is NULL\n", __FILE__, __func__, __LINE__);
		return EXIT_FAILURE;
	}
	if (NULL == cur_phase) {
		fprintf(stderr, "%s:%s:%i: Input cur_phase is NULL\n", __FILE__, __func__, __LINE__);
		return EXIT_FAILURE;
	}

	switch (*cur_phase) {
		case START: // Set next_time and save it
			// Set next_time
			thiz->next_time.tv_sec = cur_time.tv_sec + MAX_FLIGHT_TIME_S;
			thiz->next_time.tv_nsec = cur_time.tv_nsec;
			// Write next_time to file
			file = fopen(MAX_FLIGHT_TIME_PATH, "w");
			if (NULL == file) {
				fprintf(stderr, "%s:%s:%i: fopen failed: %s\n", __FILE__, __func__, __LINE__, strerror(errno));
				return EXIT_FAILURE;
			}
			rv_f = fprintf(file, "%ld\n%ld", thiz->next_time.tv_sec, thiz->next_time.tv_nsec);
			if (0 >= rv_f) {
				fprintf(stderr, "%s:%s:%i: fprintf failed and returned %d\n", __FILE__, __func__, __LINE__, rv_f);
				rv = EXIT_FAILURE;
			}
			rv_f = fclose(file);
			if (EOF == rv_f) {
				fprintf(stderr, "%s:%s:%i: fclose failed: %s\n", __FILE__, __func__, __LINE__, strerror(errno));
				return EXIT_FAILURE;
			}
			break;

		case RECOVERY: // Stopp own task
			thiz->state = STOPPED;
		break;

		default: // Check for passed MAX_FLIGHT_TIME
			// Is next_time not set?
			if (0 == thiz->next_time.tv_sec && 0 == thiz->next_time.tv_nsec) {
				// Read next_time from file
				file = fopen(MAX_FLIGHT_TIME_PATH, "r");
				if (NULL == file) {
					fprintf(stderr, "%s:%s:%i: fopen failed: %s\n", __FILE__, __func__, __LINE__, strerror(errno));
					return EXIT_FAILURE;
				}
				rv_f = fscanf(file, "%ld\n%ld", &(thiz->next_time.tv_sec), &(thiz->next_time.tv_nsec));
				errno = 0;
				if (2 != rv_f) {
					fprintf(stderr, "%s:%i: fscanf failed: %s\n", __FILE__, __LINE__, 0 == errno ? "faulty file content" : strerror(errno));
					rv = EXIT_FAILURE;
				}
				rv_f = fclose(file);
				if (EOF == rv_f) {
					fprintf(stderr, "%s:%s:%i: fclose failed: %s\n", __FILE__, __func__, __LINE__, strerror(errno));
					return EXIT_FAILURE;
				}
			}
			// Is MAX_FLIGHT_TIME reached?
			if (timespec_lt(thiz->next_time, cur_time)) {
				fprintf(stderr, "%s:%s:%i: MAX_FLIGHT_TIME_S = %ld s reached. Switched state to RECOVERY.\n", __FILE__, __func__, __LINE__, MAX_FLIGHT_TIME_S);
				*cur_phase = RECOVERY;
				// Update phase
				rv_f = task_update_states_by_phase(tasks, tasks_amt, active_phase);
				if (EXIT_FAILURE == rv_f) {
					fprintf(stderr, "%s:%s:%i: task_update_states_by_phase failed\n", __FILE__, __func__, __LINE__);
					return EXIT_FAILURE;
				}
				rv_f = write_phase(*cur_phase);
				if (EXIT_FAILURE == rv_f) {
					fprintf(stderr, "%s:%s:%i: write_phase failed\n", __FILE__, __func__, __LINE__);
					return EXIT_FAILURE;
				}
				// Stopp own task
				thiz->state = STOPPED;
			}
	}
	
	return rv;
}

Quellcode Sensoren

Eine kleine Funktion zum Testen:

Versenden einer SMS über den Pi

Um die Bergung nach der Landung zu vereinfachen, haben wir die GPS-Daten regelmäßig per SMS übermittelt. Neben dem Pi und dem GPS-Modul benötigten wir einen USB-Dongle (Huawei E3372), eine Sim-Karte und folgende Softwarepakete für den Pi:

usb-modeswitch usb-modeswitch-data smstools

Da der USB-Surfstick vom Pi nur als Massenspeicher erkannt wird, muss dieser in den Modem-Modus gewechselt werden:

sudo usb_modeswitch -v 12d1 -p 1f01 -M '55534243123456780000000000000011062000000100000000000000000000'

Die passenden Werte für -v und -p können über das Terminal mittels lsusb ermittelt werden.

Leider hat diese Methode unseren Stick nicht in den passenden Modus gebracht. Es musste zusätzlich unter /ets/usb_modeswitch.d/ eine Datei mit dem Namen 12d1:1f01 und folgendem Inhalt angelegt werden:

# Huawei E3372 (fallback mode)
TargetVendor=  0x12d1
TargetProduct= 0x155f
MessageContent="55534243123456780000000000000011063000000100010000000000000000"

Es muss außerdem die Datei /etc/smsd.conf mit folgendem Inhalt vorhanden sein (die Datei wird in der Regel automatisch mit korrektem Inhalt angelegt, die Auflistung hier erfolgt nur der Vollständigkeit halber):

devices = GSM1
outgoing = /var/spool/sms/outgoing
checked = /var/spool/sms/checked
incoming = /var/spool/sms/incoming
logfile = /var/log/smstools/smsd.log
infofile = /var/run/smstools/smsd.working
pidfile = /var/run/smstools/smsd.pid
outgoing = /var/spool/sms/outgoing
checked = /var/spool/sms/checked
failed = /var/spool/sms/failed
incoming = /var/spool/sms/incoming
sent = /var/spool/sms/sent
stats = /var/log/smstools/smsd_stats
loglevel = 7

#
# Hier stehen u.U. eine Menge auskommentierter Einträge, die relevanten Zeilen befinden sich ganz am Ende der Datei
#

[GSM1]
device = /dev/ttyUSB0 # Das korrekte device kann mit ls -la /dev/serial/by-id ermittelt werden
incoming = yes 
baudrate = 9600

Nun kann der Pi eine SMS verschicken. Dazu muss einfach nur eine Datei in den Ordner /var/spool/sms/outgoing/ verschoben werden. Zu beachten ist dabei, dass die Datei nach folgendem Schema aufgebaut ist.

To: Empfängernummer(Bsp: 4915201234567)

Text, der übermittelt werden soll.

Eine kleine Funktion zum Testen:

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <sys/types.h>
#include <string.h>

void send(char *message) {

	int fd_to, geschrieben;
	char *path = "/var/spool/sms/outgoing/data.txt";

	fd_to = open(path, O_WRONLY|O_CREAT, S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH);

	if (fd_to == -1) {

		printf("Error opening file");
		return;
	}

	printf("Größe: %d\n", strlen(message));
	printf("Geöffnet\n");
	char *msg = "To: xxxxxxxxxx\n\n";
	
	char *fullmsg = malloc(strlen(msg) + strlen(message) + 1);
	strcpy(fullmsg, msg);
	strcat(fullmsg, message);
	strcat(fullmsg, "\n\0");
	 	
	geschrieben = write(fd_to, fullmsg, strlen(fullmsg));
	free(fullmsg);
}

int main(int argc, char *argv[]) {

	send("Hallo Welt");
	return 0;
}

Auswertungsprogramm

Auswertung der Messdaten

Um die aufgezeichneten Daten nach dem Flug grafisch aufzuarbeiten, wurde ein eigenes Tool geschrieben. Hier gab es keine großen Einschränkungen, was die Programmiersprache betrifft, aufgrund von Erfahrung und persönlicher Präferenz wurde sich für Java entschieden. Das Tool ist speziell auf die von uns verwendeten Sensoren und das Vormat der CSV-Dateien zugeschnitten, alledings Modular aufgebaut und somit leicht erweiterbar.

Über eine grafische Oberfläche kann ausgewählt werden, welche Werte auf x- und y-Achse abgebildet werden sollen. Danach muss für jeden ausgewählten Datentyp eine Datei mit den entsprechenden Daten angegeben werden. Hierbei merkt sich das Programm die letzten gewählten Dateien um bei häufiger Benutzung Zeit zu sparen. Des Weiteren können Grenzwerte für beide Achsen fesgelegt werden und es steht die Wahl offen, ob alle ausgewählten Werte in einem oder in mehreren Diagrammen dargestellt werden sollen.

Die Erzeugung der Diagramme folgt dem Factory-Method Designpattern. In einer abstrakten Klasse wird die Standardprozedur zur Erstellung eines Diagramms definiert. Für jeden Typ von Daten, den wir aufzeichnen (Temperatur, Luftdruck, etc.) gibt es dann eine konkrete Implementierung dieser Klasse, die entweder die Standardimplementierung übernehmen kann (was i.d.R. der Fall ist) oder diese überschreibt.

Auswertung der GPS-Daten

Zuerst wurden die ebenfalls im CSV-Format gespeicherten GPS-Daten als Route in einer GPX-Datei umgewandelt. Dazu werden die Aufzeichnungsdaten zeilenweise von einem Javaprogramm eingelsen und in eine neue Datei im GPX-Format, das vom Aufbau einer XML-Datei gleicht, geschrieben. Diese Route sollte ursprünglich mittels eines XSLT-Skripts und einem OSM-Kartenausschnitt zu einer SVG-Datei transformiert werden, allerdings scheiterte dieses Vorhaben daran, dass der Kartenauschnitt für unsere Flugroute über 50 GB an Dateigröße überschritten hat und somit nicht mehr zu bearbeiten war. Die Darstellung der in der GPX-Datei erfassten Route wurde dann von Online-Tools wie Google-Maps übernommen.

Zusätzlich wurde zu Testzwecken eine Funktion eingebaut, die eine CSV-Datei mit GPS-Koordinaten erzeugt.

Bedienung
In das obere Textfeld wird der Pfad zum Ordner eingetragen, in der sich die auszuwertende CSV-Datei befindet
In das untere Textfeld wird der Dateiname inkl. Dateiendung eingetragen
Sollte der Haken in die Checkbox gesetzt werden, wird eine CSV-Datei erzeugt mit dem in dem Textfeld darüber angegebenen Namen in dem im Textfeld angegebenen Ordner erzeugt.

Wichtig: Sollte bereits eine Datei mit gleichem Namen vorhanden sein, wird diese überschrieben.

Code
import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.File;
import java.io.FileReader;
import java.io.FileWriter;
import java.io.IOException;
import java.util.Random;
import java.awt.Desktop;
import java.awt.event.ActionEvent;
import java.net.URI;
import java.net.URISyntaxException;
import javax.swing.*;
import java.awt.event.ActionListener;

public class Auswertung extends JFrame implements ActionListener {
	/**
	 * 
	 */
	private static final long serialVersionUID = 1L;
	static String path, inFile;
	static String outFile = "route.gpx";
	boolean genericFile = false;
	static String htmlFile = "file:///C:/Users/Manue/Desktop/Auswertung/Auswertung.html";
	JTextField tfFolder = new JTextField(50);
	JTextField tfName = new JTextField(50);

	static double lat = 50.026411;
	static double lon = 8.007177;
	static double h = 310.00;
	static double time = 6513.264;
	static Random rng = new Random();
	static int rangeMin = 0;
	static int rangeMax = 1000;
	static int anzahlDaten = 12000;
	static boolean abstieg = false;

	JLabel label;
	JPanel panel;
	JCheckBox gen;
	JButton buttonOK;

	public static double rng() {
		return rng.nextDouble() / 3600;
	}

	// Erzeugt Flugaufzeichnungen als txt
	public static void generateGpsData(String in) throws IOException {
		BufferedWriter writer = new BufferedWriter(new FileWriter(in));
		System.out.println("Generator: Datei erzeugt");
		writer.write("\"Unix Time (s)\";\"Latitude (DD)\";\"Longitude (DD)\";\"Height (m)\"\r\n");
		System.out.println("Generator: Header geschrieben");
		for (int i = 0; i < anzahlDaten; i++) {
			writer.write(time + ";" + lat + ";" + lon + ";" + h + "\r\n");
			lat -= rng();
			lon += rng() / 3;
			time += 1.0;
			if (h < 30000 && abstieg == false) {
				h += 7.0;
			} else {
				abstieg = true;
				if (h > 5) {
					h -= 5.0;
				} else {
					h = 0.0;
				}
			}
		}
		System.out.println("Generator: Datensäte erzeugt");
		writer.close();
		System.out.println("Generator: Ende");
	}

	// GUI
	public Auswertung() {
		JFrame meinJFrame = new JFrame();
		meinJFrame.setTitle("GPS-Daten auswerten");
		meinJFrame.setSize(700, 150);
		panel = new JPanel();

		label = new JLabel("Dateiordner angeben");
		panel.add(label);

		panel.add(tfFolder);

		label = new JLabel("Dateinname angeben");
		panel.add(label);
		panel.add(tfName);

		gen = new JCheckBox("Generische Daten erzeugen", genericFile);
		gen.addActionListener(this);
		panel.add(gen);

		buttonOK = new JButton("OK");
		buttonOK.addActionListener(this);
		panel.add(buttonOK);

		meinJFrame.add(panel);
		meinJFrame.setVisible(true);

	}

	// Erzeugt aus der txt eine .gpx Datei
	public static void convertToGpx(String in, String out) throws IOException {
		FileReader reader = new FileReader(in);
		System.out.println("Converter: Datendatei gefunden");
		
		BufferedReader inBuffer = new BufferedReader(reader);
		System.out.println("Converter: BufferedReader initialisiert");
		
		BufferedWriter writer = new BufferedWriter(new FileWriter(out));
		System.out.println("Converter: Writer erzeugt ");
		
		String line = inBuffer.readLine();
		System.out.println("Converter: Erste Line gelesen");
		
		line = inBuffer.readLine();
		System.out.println("Converter: Header übersprungen");
		
		writer.write("<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\" ?>\r\n"
				+ "<gpx xmlns=\"http://www.topografix.com/GPX/1/1\" version=\"1.1\" creator=\"Wikipedia\"\r\n"
				+ "    xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\"\r\n"
				+ "    xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd\">\r\n"
				+ " <!-- Kommentare sehen so aus -->\r\n" + " <metadata>\r\n" + "  <name>" + outFile + "</name>\r\n"
				+ "  <desc>GPS-Aufzeichnung vom 11.03.2020</desc>\r\n" + "  <author>\r\n" + "   <name>MF</name>\r\n"
				+ "  </author>\r\n" + " </metadata>\r\n" + " <trk>\r\n" + "  <name>Trackname1</name>\r\n"
				+ "  <desc>Trackbeschreibung</desc>\r\n" + "  <trkseg>\r\n");
		System.out.println("Converter: GPX-File Header geschrieben");
		
		while (line != null) {
			String[] splitString = line.split(";");
			for (int i = 0; i < splitString.length; i++) {
				System.out.println(splitString[i]);
				
			}

			writer.write("   <trkpt lat=\"" + splitString[1] + "\" lon=\"" + splitString[2] + "\">\r\n" + "    <time>"
					+ splitString[0] + "</time>\r\n" + "    <ele>" + splitString[3] + "</ele>\r\n" + "   </trkpt>\r\n");

			line = inBuffer.readLine();
		}
		System.out.println("Converter: GPX-File Trackpts geschrieben");
		
		writer.write("  </trkseg>\r\n" + " </trk>\r\n" + "</gpx>");
		System.out.println("Converter: GPX-File geöffnete Tags geschlossen");
		
		writer.close();
		inBuffer.close();
		reader.close();
		System.out.println("Converter: Ende ");
	}

	// Soll die Lokale Datei unter dem Pfad "htmlFile" in MS Edge öffnen, da nur
	// dieser Browser
	// die Auswertungsseite korrekt darstellt
	public static void open() throws IOException {
		System.out.println("Versuche, Browser zu öffnen");
		try {
			System.out.println("cmd.exe /C start microsoft-edge:" + htmlFile);
			Runtime.getRuntime().exec("cmd.exe /C start microsoft-edge:" + htmlFile);
		} catch (IOException e) {
			// TODO Auto-generated catch block
			System.out.println("Error beim Öffnen von Edge");
		}
		System.out.println("Browser geöffnet");
	}

	// Alternative Methode, die einen GPX Viewer im Standardbrowser und den
	// Speicherort der gpx öffnet.
	// Die gpx Datei muss nur ins Browserfenster gezogen werden
	public static void open2() throws IOException {
		System.out.println("Versuche, Browser zu öfnnen");
		try {
			Desktop.getDesktop().browse(new URI("https://www.j-berkemeier.de/ShowGPX.html"));
		} catch (IOException | URISyntaxException e) {
			// TODO Auto-generated catch block
			System.out.println("Error beim Öffnen des Browsers");
		}
		System.out.println("Browser geöffnet");
		Desktop.getDesktop().open(new File(path));
	}

	public static void main(String[] args) {
		new Auswertung();
	}

	// Actionhandler, der prüft, ob die Checkbox zur Datenerzeugung angekreuzt
	// wurde. Falls ja, werden Flugdaten
	// erzeugt beim Klick auf OK erzeugt.
	@Override
	public void actionPerformed(ActionEvent e) {
		if (e.getSource() == this.gen) {
			genericFile = !genericFile;
		} else {
			path = tfFolder.getText();
			inFile = tfName.getText();
			System.out.println(path);
			if (genericFile) {
				try {
					generateGpsData(path + inFile);
				} catch (IOException e1) {
					// TODO Auto-generated catch block
					System.out.println("Error on generate()");
				}
			}
			try {
				convertToGpx(path + inFile, path + outFile);
			} catch (IOException e1) {
				// TODO Auto-generated catch block
				System.out.println("Error on convert()");
			}
			try {
				open();
			} catch (IOException e1) {
				// TODO Auto-generated catch block
				System.out.println("Error open()");
			}
		}
		System.out.println(genericFile);
	}
}