void dbgFlush(void)
{
while (!dbgEmpty())
dbgPoll();
delay_us(200); // wait until the char is sent
}
int main(void)
{
uint8_t more, ack;
uint8_t rf_pckt_ok = 0, rf_pckt_lost = 0;
uint8_t voltage_counter = 0, temperature_counter = 0;
bool read_result;
mpu_packet_t pckt;
hw_init();
for (;;)
{
pckt.flags = 0; // reset the flags
// get the battery voltage every VOLTAGE_READ_EVERY iterations
if (++voltage_counter == VOLTAGE_READ_EVERY)
{
pckt.flags |= FLAG_VOLTAGE_VALID;
pckt.voltage = get_battery_voltage();
voltage_counter = 0;
}
// same as with the voltage, send the temperature
if (++temperature_counter == TEMPERATURE_READ_EVERY)
{
pckt.flags |= FLAG_TEMPERATURE_VALID;
mpu_get_temperature(&pckt.temperature);
temperature_counter = 0;
}
// Waits for the interrupt from the MPU-6050.
// Instead of polling, I should put the MCU to sleep and then have it awaken by the MPU-6050.
// However, I have not succeeded in the making the wakeup work reliably.
while (MPU_IRQ)
dbgPoll();
while (!MPU_IRQ)
;
do {
// read all the packets in the MPU fifo
do {
read_result = dmp_read_fifo(&pckt, &more);
} while (more);
if (read_result)
{
pckt.flags |= (RECENTER_BTN == 0 ? FLAG_RECENTER : 0);
// send the message
if (rf_head_send_message(&pckt, sizeof(pckt)))
++rf_pckt_ok;
else
++rf_pckt_lost;
// update the LEDs
if (rf_pckt_lost + rf_pckt_ok == LED_PCKT_TOTAL)
{
if (rf_pckt_ok > rf_pckt_lost)
LED_GREEN = 1;
else
LED_RED = 1;
} else if (rf_pckt_lost + rf_pckt_ok == LED_PCKT_TOTAL + LED_PCKT_LED_ON) {
LED_RED = 0;
LED_GREEN = 0;
rf_pckt_ok = rf_pckt_lost = 0;
}
// check for an ACK payload
if (rf_head_read_ack_payload(&ack, 1))
{
if (ack == CMD_CALIBRATE)
{
mpu_calibrate_bias();
} else if (ack == CMD_READ_TRACKER_SETTINGS) {
rf_head_send_message(get_tracker_settings(), sizeof(tracker_settings_t));
} else if (ack >= CMD_RF_PWR_LOWEST && ack <= CMD_RF_PWR_HIGHEST) {
tracker_settings_t new_settings;
memcpy(&new_settings, get_tracker_settings(), sizeof(tracker_settings_t));
new_settings.rf_power = ack;
save_tracker_settings(&new_settings);
}
}
}
} while (more);
}
}
