void EXTI0_IRQHandler(void) {
static uint32_t transmit = 0;
if (EXTI_GetITStatus(EXTI_Line0) != RESET) {
EXTI_ClearITPendingBit(EXTI_Line0); /* Clear interrupt flag */
}
if (transmit == 1) {
return;
}
transmit = 1;
send_pos();
printf("Hello World\n");
}
/* jump here when a NAVIGATION message is received */
void on_NAV_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]) {
double hpos, vpos;
double hservo, vservo;
if (mode == AUTO) {
gps_pos_x = atof(argv[2]);
gps_pos_y = atof(argv[3]);
/* calculate azimuth */
ant_azim = atan2(gps_pos_x, gps_pos_y) * 180. / M_PI;
if (ant_azim < 0)
ant_azim += 360.;
/* calculate elevation */
ant_elev = atan2((gps_alt - home_alt), sqrt(atof(argv[5]))) * 180.
/ M_PI;
if (ant_elev < 0)
ant_elev = 0.;
gtk_range_set_value(azim_scale, ant_azim);
gtk_range_set_value(elev_scale, ant_elev);
}
// The magic is done here
// First take the horizontal angle relative to the neutral point "hnp"
hpos = ant_azim - hnp;
// Keep the range between (-180,180). this is done so that it consistently swaps sides
if (hpos < -180) {
hpos += 360;
} else if (hpos > 180) {
hpos -= 360;
}
// keep the range within the field of view "hfov"
if (hpos > (hfov / 2)) {
hpos = hfov / 2;
} else if (-hpos > (hfov / 2)) {
hpos = -hfov / 2;
}
// Take the vertical angle relative to the neutral point "vnp"
vpos = ant_elev - vnp;
// keep within the field of view "vfov"
if (vpos > (vfov / 2)) {
vpos = vfov / 2;
} else if (-vpos > (vfov / 2)) {
vpos = -vfov / 2;
}
// make outputs relative to limits for the Pololu board
vservo = (((vpos + (vfov / 2)) / vfov) * (vlim2 - vlim1)) + vlim1;
hservo = (((hpos + (hfov / 2)) / hfov) * (hlim2 - hlim1)) + hlim1;
/*g_message("home_alt %f gps_alt %f azim %f elev %f", home_alt, gps_alt, ant_azim, ant_elev); */
// Send servo position.
send_pos(vservo, hservo);
g_message("vservo %f hservo %f", vservo, hservo);
}
