drone.cpp

#include "./drone.h"

void start_drone(const long duration) {
	// time management
	auto  start_time = std::chrono::high_resolution_clock::now();
	auto now = std::chrono::high_resolution_clock::now();
	auto past_now = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double> elapsed;

	// get the simulator initial values that were preset
	double altitude = INITIAL_ALTITUDE;
	double error = Linker::get_error();
	double target = altitude - error;
	int past_command = Linker::get_command();
	int command = past_command;
	double accel = GRAVITY;
	double speed = INITIAL_SPEED;

	// output file
	std::ofstream fout(FILE_OUT);
	int no_x = 0;
	int out_counter = 0;

	while (1) {
		// take the info from the linker
		command = Linker::get_command();

		//calculate the acceleration from command
		if (past_command != command) {
			accel = set_accel(past_command, command, speed);
		}

		// calculate the speed from the acceleration
		speed = speed + accel * FROM_US_TO_S(TIME_STAMP_US);

		// sleep until the time stamp is finished and we continue with the next frame
		struct timespec req = {0, 0};
		req.tv_sec = 0;
		auto nsec = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - past_now);
		req.tv_nsec = TIME_STAMP_US - nsec.count();
		nanosleep(&req, (struct timespec *) NULL);

		// the new past time reference
		past_now = now;

		// the new position is calculated just now, after the wait
		altitude += speed * FROM_US_TO_S(TIME_STAMP_US);
		if (altitude < 0) altitude = 0;
		error = altitude - target;

		// send the new position
		Linker::give_error(error);

		// TO_DO
		// vant random sa bata

		// TO_DO
		// output
		if (out_counter >= PRINT_INTERVAl) {
			out_counter = 0;
			// std::cout << "A: " << std::setw(12) << accel << " | V: " <<
			// 	std::setw(12) << speed << " | Err: " <<
			// 	std::setw(12) << error << " | Alt: " <<
			// 	std::setw(12) << altitude << "\n";
			fout << no_x++ << "," << (int)error << "\n";
		}
		++out_counter;
		// timer to finish the simulation
		nsec = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start_time);
		if (nsec.count() > duration) {
			break;
		} 
	}

	fout.close();
}

inline
double set_accel(int &past_command, int command, double speed) {
	// ensure that the comand is between certain values
	border_command(command);

	// the motors are not ideal so they can not change their response imidietlly
	if (std::abs(past_command - command) > ((MAX_COMMAND - MIN_COMMAND) * IDEAL)) {
		if (command < past_command) {
			command = past_command - ((MAX_COMMAND - MIN_COMMAND) * IDEAL);
		} else {
			command = past_command + ((MAX_COMMAND - MIN_COMMAND) * IDEAL);
		}
		border_command(command);
	}
	past_command = command;

	// the command is transformed to force (Newtons)
	double force = FROM_COMMAND_TO_FORCE * command + GRAVITY * DRONE_MASS -
									0.5 * AIR_DENSITY * speed * speed  * DRONE_DRAG_COEFF *
									DRONE_TOP_AREA;
	// a = F / m
	double acceleration = force / DRONE_MASS;
	return acceleration;
}

inline
void border_command(int &command) {
	// Brenchless version of the if code, but the compiler is optimizing the code
	// better at compile time as we can see if we analize the assembly code
	// generated: 3 instructions generated by the compiler for the normal version
	// VS. 8 more complex instructions generated by our "smart" version
	// command = (command * (command > MIN_COMMAND && command < MAX_COMMAND)) +
	// 						(MIN_COMMAND * (command < MIN_COMMAND)) +
	// 						(MAX_COMMAND * (command > MAX_COMMAND));
	if (command < MIN_COMMAND) {
		command = MIN_COMMAND;
	} else if (command > MAX_COMMAND) {
		command = MAX_COMMAND;
	}
}