Skip to content

Commit b361e9e

Browse files
Yoursole1Yoursole1
authored
Merge pull request #3 from SARP-UW/fire-state
Fire state + packets
2 parents b7352ab + 2c4a021 commit b361e9e

12 files changed

Lines changed: 884 additions & 107 deletions

File tree

‎src/app/states/fire_state.c‎

Lines changed: 217 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,233 @@
11
#include "app/state.h"
2-
2+
#include "app/utils/devices.h"
3+
#include "app/utils/packets.h"
4+
#include "devices/imu.h"
5+
#include "devices/gnss.h"
6+
#include "devices/barometer.h"
7+
#include "devices/actuator.h"
8+
#include "devices/adc.h"
9+
#include "devices/magnetometer.h"
10+
#include "devices/temperature.h"
11+
#include "devices/radio.h"
12+
#include "peripheral/spi.h"
13+
#include "peripheral/errc.h"
14+
#include "peripheral/systick.h"
315

416
// -- ( ) FIRING: Deliver propelants to manifold
517
// ~~~ ( ) Open main propelant valves
618
// ~~~ ( ) Execute control loop for engine burn
719
// ~~~ ( ) Send and record telemetry
820

21+
#define ADC_PACKET_INDEX 0x00
22+
#define LOOP_PERIOD_MS 100U
23+
#define COMM_PERIOD_MS 1000U
24+
#define SENSOR_CYCLES_PER_COMM (COMM_PERIOD_MS / LOOP_PERIOD_MS)
25+
26+
/** Buffers for packets */
27+
static uint8_t gnss_packet[PACKET_GNSS_SIZE];
28+
static uint8_t sensor_packet[PACKET_SENSOR_SIZE];
29+
static uint8_t adc_packet[PACKET_ADC_MAX_SIZE];
30+
static uint8_t state_packet[PACKET_STATE_MAX_SIZE];
31+
static uint8_t comm_packet[PACKET_COMM_MAX_SIZE];
32+
static uint8_t rx_packet[PACKET_RX_MAX_SIZE];
33+
static comm_packet_t uplink_comm;
34+
35+
static uint32_t elapsed_ms;
36+
static uint32_t processor_time_ms;
37+
static uint16_t ping_id;
38+
939
bool fire_state_init(){
40+
enum ti_errc_t errc;
41+
42+
radio_init(&radio_dev, &radio_spi_config, &radio_config, &errc);
43+
if (errc && errc != TI_ERRC_NONE) {
44+
TI_SET_ERRC(&errc, errc, "Failed to initialize radio");
45+
}
46+
errc = imu_init(&imu_dev1);
47+
if (errc && errc != TI_ERRC_NONE) {
48+
TI_SET_ERRC(&errc, errc, "Failed to initialize IMU 1");
49+
}
50+
errc = imu_init(&imu_dev2);
51+
if (errc && errc != TI_ERRC_NONE) {
52+
TI_SET_ERRC(&errc, errc, "Failed to initialize IMU 2");
53+
}
54+
gnss_init(&gnss_dev, &errc);
55+
if (errc && errc != TI_ERRC_NONE) {
56+
TI_SET_ERRC(&errc, errc, "Failed to initialize GNSS");
57+
}
58+
barometer_init(&barometer_dev1, &errc);
59+
if (errc && errc != TI_ERRC_NONE) {
60+
TI_SET_ERRC(&errc, errc, "Failed to initialize barometer 1");
61+
}
62+
barometer_init(&barometer_dev2, &errc);
63+
if (errc && errc != TI_ERRC_NONE) {
64+
TI_SET_ERRC(&errc, errc, "Failed to initialize barometer 2");
65+
}
66+
temperature_init(&temperature_dev1, &errc);
67+
if (errc && errc != TI_ERRC_NONE) {
68+
TI_SET_ERRC(&errc, errc, "Failed to initialize temperature 1");
69+
}
70+
temperature_init(&temperature_dev2, &errc);
71+
if (errc && errc != TI_ERRC_NONE) {
72+
TI_SET_ERRC(&errc, errc, "Failed to initialize temperature 2");
73+
}
74+
magnetometer_init(&magnetometer_dev1);
75+
if (errc && errc != TI_ERRC_NONE) {
76+
TI_SET_ERRC(&errc, errc, "Failed to initialize magnetometer 1");
77+
}
78+
magnetometer_init(&magnetometer_dev2);
79+
if (errc && errc != TI_ERRC_NONE) {
80+
TI_SET_ERRC(&errc, errc, "Failed to initialize magnetometer 2");
81+
}
82+
83+
adc_init(&adc_dev, &errc);
84+
if (errc && errc != TI_ERRC_NONE) {
85+
TI_SET_ERRC(&errc, errc, "Failed to initialize ADC");
86+
}
1087

1188
return 1;
1289
}
1390

1491
int fire_state_run(){
92+
static const uint8_t comm_tags[] = {0};
93+
94+
for (uint8_t cycle = 0; cycle < SENSOR_CYCLES_PER_COMM; cycle++) {
95+
enum ti_errc_t errc;
96+
size_t adc_packet_len;
97+
size_t state_packet_len;
98+
99+
gnss_pvt_t gnss_pvt;
100+
gnss_get_pvt(&gnss_dev, &gnss_pvt, &errc);
101+
if (errc && errc != TI_ERRC_NONE) {
102+
TI_SET_ERRC(&errc, errc, "Failed to get GNSS PVT data");
103+
}
104+
105+
build_gnss_packet(&gnss_pvt, gnss_packet, sizeof(gnss_packet), &errc);
106+
send_packet_radio_flash(&radio_dev, gnss_packet, sizeof(gnss_packet), &errc);
107+
if (errc && errc != TI_ERRC_NONE) {
108+
TI_SET_ERRC(&errc, errc, "Failed to transmit GNSS packet");
109+
}
110+
111+
struct imu_result imu_result1;
112+
struct imu_result imu_result2;
113+
errc = imu_transfer(&imu_dev1, &imu_result1);
114+
if (errc && errc != TI_ERRC_NONE) {
115+
TI_SET_ERRC(&errc, errc, "Failed to transfer IMU 1 data");
116+
}
117+
errc = imu_transfer(&imu_dev2, &imu_result2);
118+
if (errc && errc != TI_ERRC_NONE) {
119+
TI_SET_ERRC(&errc, errc, "Failed to transfer IMU 2 data");
120+
}
121+
122+
temperature_result_t temperature_result1;
123+
temperature_result_t temperature_result2;
124+
temperature_read_temp(&temperature_dev1, &temperature_result1, &errc);
125+
if (errc && errc != TI_ERRC_NONE) {
126+
TI_SET_ERRC(&errc, errc, "Failed to read temperature 1 data");
127+
}
128+
temperature_read_temp(&temperature_dev2, &temperature_result2, &errc);
129+
if (errc && errc != TI_ERRC_NONE) {
130+
TI_SET_ERRC(&errc, errc, "Failed to read temperature 2 data");
131+
}
132+
barometer_result_t barometer_result1;
133+
barometer_result_t barometer_result2;
134+
barometer_result1 = get_barometer_data(&barometer_dev1, &errc);
135+
if (errc && errc != TI_ERRC_NONE) {
136+
TI_SET_ERRC(&errc, errc, "Failed to read barometer 1 data");
137+
}
138+
barometer_result2 = get_barometer_data(&barometer_dev2, &errc);
139+
if (errc && errc != TI_ERRC_NONE) {
140+
TI_SET_ERRC(&errc, errc, "Failed to read barometer 2 data");
141+
}
142+
struct magnetometer_result_t magnetometer_result1;
143+
struct magnetometer_result_t magnetometer_result2;
144+
errc = magnetometer_read(&magnetometer_dev1, &magnetometer_result1);
145+
if (errc && errc != TI_ERRC_NONE) {
146+
TI_SET_ERRC(&errc, errc, "Failed to transfer magnetometer 1 data");
147+
}
148+
errc = magnetometer_read(&magnetometer_dev2, &magnetometer_result2);
149+
if (errc && errc != TI_ERRC_NONE) {
150+
TI_SET_ERRC(&errc, errc, "Failed to transfer magnetometer 2 data");
151+
}
152+
153+
build_sensor_packet(&imu_result1,
154+
&imu_result2,
155+
&barometer_result1,
156+
&barometer_result2,
157+
&magnetometer_result1,
158+
&magnetometer_result2,
159+
&temperature_result1,
160+
&temperature_result2,
161+
sensor_packet,
162+
sizeof(sensor_packet),
163+
&errc);
164+
send_packet_radio_flash(&radio_dev, sensor_packet, sizeof(sensor_packet), &errc);
165+
if (errc && errc != TI_ERRC_NONE) {
166+
TI_SET_ERRC(&errc, errc, "Failed to transmit sensor data packet");
167+
}
168+
169+
build_adc_packet(adc_channels, adc_channel_count, ADC_PACKET_INDEX, adc_packet, sizeof(adc_packet), &errc);
170+
if (errc && errc != TI_ERRC_NONE) {
171+
TI_SET_ERRC(&errc, errc, "Failed to build ADC packet");
172+
}
173+
174+
adc_packet_len = (size_t)(10U + (3U * adc_channel_count));
175+
send_packet_radio_flash(&radio_dev, adc_packet, adc_packet_len, &errc);
176+
if (errc && errc != TI_ERRC_NONE) {
177+
TI_SET_ERRC(&errc, errc, "Failed to transmit ADC packet");
178+
}
179+
180+
build_state_packet(valve_states,
181+
VALVE_COUNT,
182+
servo_states,
183+
SERVO_COUNT,
184+
state_packet,
185+
sizeof(state_packet),
186+
&errc);
187+
state_packet_len = (size_t)(9U + ((VALVE_COUNT + 7U) / 8U) + (2U * SERVO_COUNT));
188+
send_packet_radio_flash(&radio_dev, state_packet, state_packet_len, &errc);
189+
if (errc && errc != TI_ERRC_NONE) {
190+
TI_SET_ERRC(&errc, errc, "Failed to transmit state packet");
191+
}
192+
193+
elapsed_ms += LOOP_PERIOD_MS;
194+
processor_time_ms += LOOP_PERIOD_MS;
195+
systick_delay(LOOP_PERIOD_MS);
196+
}
197+
198+
{
199+
enum ti_errc_t errc;
200+
size_t comm_packet_len;
201+
202+
build_comm_packet(ping_id,
203+
0,
204+
processor_time_ms,
205+
uplink_comm.command_id,
206+
0,
207+
comm_tags,
208+
1,
209+
comm_packet,
210+
sizeof(comm_packet),
211+
&comm_packet_len,
212+
&errc);
213+
if (errc && errc != TI_ERRC_NONE) {
214+
TI_SET_ERRC(&errc, errc, "Failed to build comm packet");
215+
}
216+
217+
send_packet_radio_flash(&radio_dev, comm_packet, comm_packet_len, &errc);
218+
if (errc && errc != TI_ERRC_NONE) {
219+
TI_SET_ERRC(&errc, errc, "Failed to transmit comm packet");
220+
}
221+
222+
receive_uplink_comm_packet(&radio_dev, rx_packet, sizeof(rx_packet), &uplink_comm, &errc);
223+
if (errc && errc != TI_ERRC_NONE) {
224+
TI_SET_ERRC(&errc, errc, "Failed to receive or parse uplink comm packet");
225+
}
226+
}
227+
228+
ping_id++;
229+
elapsed_ms = 0;
230+
15231
return 6;
16232
}
17233

‎src/app/utils/devices.h‎

Lines changed: 126 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,126 @@
1+
#pragma once
2+
3+
#include <stdint.h>
4+
5+
#include "app/utils/pinout.h"
6+
#include "devices/adc.h"
7+
#include "devices/actuator.h"
8+
#include "devices/barometer.h"
9+
#include "devices/gnss.h"
10+
#include "devices/imu.h"
11+
#include "devices/magnetometer.h"
12+
#include "devices/radio.h"
13+
#include "devices/temperature.h"
14+
15+
#define VALVE_COUNT 12U
16+
#define SERVO_COUNT 8U
17+
18+
static radio_t radio_dev;
19+
static radio_spi_dev radio_spi_config = {
20+
.spi_inst = (uint8_t)RADIO_SPI_INST,
21+
.ss_pin = (uint8_t)RADIO_SPI_CS
22+
};
23+
24+
static radio_config_t radio_config = {
25+
.reset_pin = (uint8_t)RADIO_SHDN,
26+
.nirq_pin = (uint8_t)RADIO_IRQ,
27+
.reset_active_high = 0,
28+
.channel = 0
29+
};
30+
31+
static gnss_t gnss_dev = {
32+
.spi_config = {
33+
.spi_inst = (uint8_t)GNSS_SPI_INST,
34+
.ss_pin = (uint8_t)GNSS_SPI_CS
35+
},
36+
.config = {
37+
.meas_rate_ms = 200,
38+
.constellation_mask = GNSS_CONSTELLATION_GPS | GNSS_CONSTELLATION_GLONASS | GNSS_CONSTELLATION_BEIDOU,
39+
.dyn_model = GNSS_DYN_AIRBORNE_4G,
40+
.power_mode = GNSS_POWER_CONTINUOUS
41+
},
42+
.initialized = 0
43+
};
44+
45+
static struct imu_spi_dev imu_dev1 = {
46+
.inst = (uint8_t)SENSOR_SPI_INST,
47+
.ss_pin = (uint8_t)IMU_1_CS
48+
};
49+
50+
static struct imu_spi_dev imu_dev2 = {
51+
.inst = (uint8_t)SENSOR_SPI_INST,
52+
.ss_pin = (uint8_t)IMU_2_CS
53+
};
54+
55+
static barometer_t barometer_dev1 = {
56+
.spi_dev = {
57+
.inst = (uint8_t)SENSOR_SPI_INST,
58+
.ss_pin = (uint8_t)BARO_1_CS
59+
},
60+
.osr = OSR_4096,
61+
.calibration_data = {0},
62+
.result = {0}
63+
};
64+
65+
static barometer_t barometer_dev2 = {
66+
.spi_dev = {
67+
.inst = (uint8_t)SENSOR_SPI_INST,
68+
.ss_pin = (uint8_t)BARO_2_CS
69+
},
70+
.osr = OSR_4096,
71+
.calibration_data = {0},
72+
.result = {0}
73+
};
74+
75+
static struct magnetometer_spi_dev magnetometer_dev1 = {
76+
.inst = (uint8_t)SENSOR_SPI_INST,
77+
.ss_pin = (uint8_t)MAGNOTOMETER_CS
78+
};
79+
80+
static struct magnetometer_spi_dev magnetometer_dev2 = {
81+
.inst = (uint8_t)SENSOR_SPI_INST,
82+
.ss_pin = (uint8_t)MAGNOTOMETER_CS
83+
};
84+
85+
static temperature_t temperature_dev1 = {
86+
.spi_config = {
87+
.spi_inst = (uint8_t)SENSOR_SPI_INST,
88+
.ss_pin = (uint8_t)POWER_TMP_CS
89+
},
90+
.config = {
91+
.mode = temperature_MODE_CONTINUOUS,
92+
.resolution = temperature_RES_16_BIT
93+
}
94+
};
95+
96+
static temperature_t temperature_dev2 = {
97+
.spi_config = {
98+
.spi_inst = (uint8_t)SENSOR_SPI_INST,
99+
.ss_pin = (uint8_t)ANALOG_TMP_CS
100+
},
101+
.config = {
102+
.mode = temperature_MODE_CONTINUOUS,
103+
.resolution = temperature_RES_16_BIT
104+
}
105+
};
106+
107+
static struct adc_spi_dev adc_dev = {
108+
.inst = (uint8_t)SENSOR_SPI_INST,
109+
.ss_pin = (uint8_t)SENSOR_CS_1
110+
};
111+
112+
static const struct adc_channel adc_channels[] = {
113+
{
114+
.pos_pin = AIN0,
115+
.neg_pin = AINCOM,
116+
.gain = GAIN_1,
117+
.source = REF_INTERNAL,
118+
.ref_voltage = 25,
119+
.name = "adc0"
120+
}
121+
};
122+
123+
static const uint8_t adc_channel_count = (uint8_t)(sizeof(adc_channels) / sizeof(adc_channels[0]));
124+
125+
static uint8_t valve_states[VALVE_COUNT];
126+
static uint16_t servo_states[SERVO_COUNT];

0 commit comments

Comments
 (0)