int main(void) {
uint16_t diff;
int16_t count_deviation;
uint32_t count;
char count_string[COUNT_STRING_LEN + 1];
char pwm_string[3 + 1];
int16_t c_int = 0;
int16_t vc = 0;
WDTCTL = WDTPW + WDTHOLD;
hw_init();
gps_startup_delay();
while(!(gps_disable_nmea_output()));
while(!(gps_set_gps_only()));
while(1) {
if (P1IN & CLK_PPS) {
PJOUT |= LED4;
} else {
PJOUT &= ~LED4;
}
if (meas_finished == 1) {
meas_finished = 0;
diff = end_count - start_count;
count_deviation = (int16_t) (diff - SETPOINT_COUNT_MOD);
/* visual lock indication */
if (count_deviation < -FDEV_DEADZONE) {
PJOUT = LED1;
} else if (count_deviation > FDEV_DEADZONE) {
PJOUT = LED3;
} else {
PJOUT = LED2;
}
/* output absolute count number to USART */
count = SETPOINT_COUNT + count_deviation;
i32toa(count, COUNT_STRING_LEN, count_string);
count_string[COUNT_STRING_LEN] = ' ';
debug_transmit_string_fixed(count_string, COUNT_STRING_LEN + 1);
/* control algorithm, count_deviation contains the control error */
c_int = c_int + count_deviation;
if (c_int > 200) c_int = 200;
if (c_int < -200) c_int = -200;
vc = (32768 - 350 * c_int) >> 8;
if (vc > 255) vc = 255;
if (vc < 0) vc = 0;
TA1CCR2 = vc;
/* output PWM for debug purposes */
i32toa((uint32_t)vc, 3, pwm_string);
pwm_string[3] = '\n';
debug_transmit_string_fixed(pwm_string, 3 + 1);
}
} /* while(1) */
bool GPS_Init(void) {
// Initialize pins
TRACE_INFO("GPS > Init pins");
palSetPadMode(PORT(GPS_RESET), PIN(GPS_RESET), PAL_MODE_OUTPUT_PUSHPULL); // GPS reset
palSetPadMode(PORT(GPS_EN), PIN(GPS_EN), PAL_MODE_OUTPUT_PUSHPULL); // GPS off
palSetPadMode(PORT(GPS_TIMEPULSE), PIN(GPS_TIMEPULSE), PAL_MODE_INPUT); // GPS timepulse
// Switch MOSFET
TRACE_INFO("GPS > Switch on");
palSetPad(PORT(GPS_RESET), PIN(GPS_RESET)); // Pull up GPS reset
palSetPad(PORT(GPS_EN), PIN(GPS_EN)); // Switch on GPS
// Wait for GPS startup
chThdSleepMilliseconds(3000);
uint8_t status = 1;
// Configure GPS
TRACE_INFO("GPS > Initialize GPS");
if(gps_disable_nmea_output()) {
TRACE_INFO("GPS > Disable NMEA output OK");
} else {
TRACE_ERROR("GPS > Disable NMEA output FAILED");
status = 0;
}
#if GPS_TYPE == MAX7 || GPS_TYPE == MAX8
// MAX6 does not support anything else than GPS
if(gps_set_gps_only()) {
TRACE_INFO("GPS > Set GPS only OK");
} else {
TRACE_ERROR("GPS > Set GPS only FAILED");
status = 0;
}
#endif
if(gps_set_airborne_model()) {
TRACE_INFO("GPS > Set airborne model OK");
} else {
TRACE_ERROR("GPS > Set airborne model FAILED");
status = 0;
}
if(gps_set_power_save()) {
TRACE_INFO("GPS > Configure power save OK");
} else {
TRACE_ERROR("GPS > Configure power save FAILED");
status = 0;
}
if(gps_power_save(0)) {
TRACE_INFO("GPS > Disable power save OK");
} else {
TRACE_ERROR("GPS > Disable power save FAILED");
status = 0;
}
return status;
}
