Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
/*
* initialize_components.c
*
* Created on: Feb 20, 2016
* Author: ucart
*/
#include "initialize_components.h"
#include "communication.h"
extern int Xil_AssertWait;
int protection_loops()
{
int rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap
read_rec_all(rc_commands);
// wait for RC receiver to connect to transmitter
while(rc_commands[THROTTLE] < 75000)
read_rec_all(rc_commands);
// wait until throttle is low and the gear switch is engaged (so you don't immediately break out of the main loop below)
// also wait for the flight mode to be set to manual
while(rc_commands[THROTTLE] > 125000 || read_kill(rc_commands[GEAR]) || !read_flap(rc_commands[FLAP]))
read_rec_all(rc_commands);
// wait until the ground station has connected to the quad and acknowledged that its ready to start
stringBuilder_t * ack_packet = stringBuilder_create();
while(1)
{
// --------------------------------------
// Send request to ground station to start sending VRPN
char buf[255] = {};
int i;
// Debug print statement
//printf("function for yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
// Send a reply to the ground station
snprintf(buf, sizeof(buf), "The quad is ready to receive VRPN data.\r\n");
unsigned char *responsePacket;
metadata_t metadata =
{
BEGIN_CHAR,
MessageTypes[4].ID,
MessageTypes[4].subtypes[1].ID,
0,
(strlen(buf) + 1)
};
formatPacket(&metadata, buf, &responsePacket);
// Send each byte of the packet individually
for(i = 0; i < 8 + metadata.data_len; i++) {
// Debug print statement for all of the bytes being sent
printf("%d: 0x%x\n", i, responsePacket[i]);
uart0_sendByte(responsePacket[i]);
}
usleep(10000);
tryReceivePacket(ack_packet, 0);
unsigned char * data;
metadata_t md;
parse_packet((unsigned char *) ack_packet->buf, &data, &md);
if(md.msg_type == 0x04 && md.msg_subtype == 0x01)
break;
// --------------------------------------
}
// let the pilot/observers know that the quad is now active
MIO7_led_on();
return 0;
}
int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct, raw_sensor_t * raw_sensor_struct,
sensor_t * sensor_struct, setpoint_t * setpoint_struct, parameter_t * parameter_struct, user_defined_t *user_defined_struct,
raw_actuator_t * raw_actuator_struct, actuator_command_t * actuator_command_struct)
{
// Turn off LED 7 to let observers know that the quad is not yet active
MIO7_led_off();
// Use the stringbuilder to keep track of data received
if(!(user_input_struct->sb = stringBuilder_create()))
return -1;
// Initialize the controller
control_algorithm_init(parameter_struct);
// Xilinx given initialization
init_platform();
//disable blocking asserts
//Xil_AssertWait = FALSE;
// Initialize UART0 (Bluetooth)
if(!uart0_init(XPAR_PS7_UART_0_DEVICE_ID, 921600))
return -1;
uart0_clearFIFOs();
//Enable blocking asserts
//Xil_AssertWait = TRUE;
// Initialize I2C controller and start the sensor board
if (initI2C0() == -1) {
return -1;
}
// Initialize PWM Recorders and Motor outputs
pwm_init();
// Initialize loop timers
timer_init();
//manual flight mode
user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
// Get the first loop data from accelerometer for the gyroscope to use
if(sensor_init(raw_sensor_struct, sensor_struct) == -1)
return -1;
return 0;
}