quad_app.c

/*
 * main.c
 *
 *  Created on: Nov 11, 2015
 *      Author: ucart
 */
#include <stdio.h>
#include "timer.h"
#include "log_data.h"
#include "initialize_components.h"
#include "user_input.h"
#include "sensor.h"
#include "sensor_processing.h"
#include "control_algorithm.h"
#include "send_actuator_commands.h"
#include "update_gui.h"
#include "communication.h"
#include "mio7_led.h"

int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
{
	// Structures to be used throughout
	modular_structs_t structs = { };

	// Wire up hardware
	setup_hardware(&structs.hardware_struct);

	// Initialize all required components and structs:
	// Uart, PWM receiver/generator, I2C, Sensor Board
	// Xilinx Platform, Loop Timer, Control Algorithm
	int init_error = init_structs(&(structs));

	if (init_error != 0) {
		return -1;
	}

	// Loops to make sure the quad is responding correctly before starting the control loop
	protection_loops(&structs);

	int last_kill_condition = kill_condition(&(structs.user_input_struct));

	// Main control loop
	while (1)
	{
		// Kill quad if kill switch or more than 10 GAM
		int this_kill_condition = kill_condition(&(structs.user_input_struct));

		// Processing of loop timer at the beginning of the control loop
		timer_start_loop();

		// Process all received data

		process_received(&structs);

		// Get the user input and put it into user_input_struct
		get_user_input(&(structs.hardware_struct), &(structs.log_struct), &(structs.user_input_struct));

		// Get data from the sensors and put it into raw_sensor_struct
		get_sensors(&(structs.hardware_struct), &(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct));

		// Process the sensor data and put it into sensor_struct
		sensor_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct), &(structs.sensor_struct));

		if (!this_kill_condition) {
			// Run the control algorithm
			control_algorithm(&(structs.log_struct), &(structs.user_input_struct), &(structs.sensor_struct), &(structs.setpoint_struct),
					&(structs.parameter_struct), &(structs.user_defined_struct), &(structs.actuator_command_struct), &structs);

			// send the actuator commands
			send_actuator_commands(&(structs.hardware_struct.motors), &(structs.log_struct), &(structs.actuator_command_struct));
		}
		else {
			kill_motors(&(structs.hardware_struct.motors));
		}
		// update the GUI
		update_GUI(&(structs.log_struct));

		if (!this_kill_condition) {
			// Log the data collected in this loop
			log_data(&(structs.log_struct), &(structs.parameter_struct));

			if(structs.user_defined_struct.flight_mode == AUTO_FLIGHT_MODE)
			{
				static int loop_counter = 0;
				loop_counter++;

				// toggle the MIO7 on and off to show that the quad is in AUTO_FLIGHT_MODE
				if(loop_counter == 10)
				{
					MIO7_led_off();
				}
				else if(loop_counter >= 20)
				{

					MIO7_led_on();
					loop_counter = 0;
				}

			}
			if(structs.user_defined_struct.flight_mode == MANUAL_FLIGHT_MODE) {
				MIO7_led_on();
			}
		}

		if (this_kill_condition == 1 && last_kill_condition == 0) {
		  // Just disabled
		  printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct));
		  resetLogging();
		  MIO7_led_off();
		}

		last_kill_condition = this_kill_condition;

		// Processing of loop timer at the end of the control loop
		timer_end_loop(&(structs.log_struct));

		if (structs.raw_sensor_struct.gam.error.consErrorCount > 10) {
			kill_motors(&(structs.hardware_struct.motors));
			printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct));
			break;
		}
	}

	kill_motors(&(structs.hardware_struct.motors));

	flash_MIO_7_led(10, 100);

	return 0;
}