void SONAR_MAXBOTIX12_IRQ_HANDLER(void)
{
if (TIM_GetITStatus(SONAR_MAXBOTIX12_TIM, TIM_IT_CC1) == SET)
{
TIM_ClearITPendingBit(SONAR_MAXBOTIX12_TIM, TIM_IT_CC1);
int32_t sonar_meas = TIM_GetCapture1(SONAR_MAXBOTIX12_TIM);
/* pulse_cnts to actual pulse width in us:
* pulse width = pulse_cnts * (prescaler+1)/(actual clock)
* with 58us per centimeter, the alt in cm is:
*/
//int32_t alt_mm = pulse_cnts * 10 * (69/72) / 58;
//sonar_alt
//if ((sonar_meas > (sonar_filter_val + 1500) || sonar_meas < (sonar_filter_val - 1500))
if ((sonar_meas > (sonar_filter_val + 3000) || sonar_meas < (sonar_filter_val - 3000))
&& (sonar_spike_cnt < 5)) {
sonar_spike_cnt++;
sonar_meas = sonar_filter_val;
}
else {
sonar_spike_cnt = 0;
}
//@TODO: check for angle > 10 deg ==> don't use value
sonar_filter_val = SONAR_MAXBOTIX12_BUTTER_NUM_1*sonar_meas
+ SONAR_MAXBOTIX12_BUTTER_NUM_2*sonar_meas_prev
// + SONAR_MAXBOTIX12_BUTTER_NUM_3*sonar_meas_prev_prev
- SONAR_MAXBOTIX12_BUTTER_DEN_2*sonar_filter_val_prev;
// - SONAR_MAXBOTIX12_BUTTER_DEN_3*sonar_filter_val_prev_prev;
sonar_meas_prev_prev = sonar_meas_prev;
sonar_meas_prev = sonar_meas;
sonar_filter_val_prev_prev = sonar_filter_val_prev;
sonar_filter_val_prev = sonar_filter_val;
/** using float
float factor_cm = 0.0165229885;
float alt_cm_flt = sonar_meas * factor_cm * (1<<8);
sonar_meas_real = alt_cm_flt;*/
//((69 / 72) / 58) * (2^16) = 1082.85057
//uint16_t conv_factor_cm = 1083;
//sonar_alt_cm_bfp = conv_factor_cm*sonar_filter_val;
//sonar_filtered = conv_factor_cm*sonar_filter_val;
//((69 / 72) / 58) / 100 * (2^8) = 0.0422988506
float conv_factor_m = 0.0422988506;
float alt_m_bfp_fl = conv_factor_m*sonar_filter_val;
sonar_filtered = alt_m_bfp_fl;
//ins_sonar_initialised = TRUE;
ins_update_sonar();
//sonar_filtered = conv_factor_cm*sonar_filter_val;
//DOWNLINK_SEND_VFF(DefaultChannel, &alt_mm_flt,0,0,0,0,0,0);
DOWNLINK_SEND_INS_Z(DefaultChannel, &sonar_filtered,&sonar_spike_cnt,0, 0);
}
}
void navdata_update()
{
static bool_t last_checksum_wrong = FALSE;
// Check if initialized
if (!nav_port.isInitialized) {
navdata_init();
return;
}
// Start reading
navdata_read();
// while there is something interesting to do...
while (nav_port.bytesRead >= NAVDATA_PACKET_SIZE)
{
if (nav_port.buffer[0] == NAVDATA_START_BYTE)
{
assert(sizeof navdata == NAVDATA_PACKET_SIZE);
memcpy(&navdata, nav_port.buffer, NAVDATA_PACKET_SIZE);
// Calculating the checksum
uint16_t checksum = 0;
for(int i = 2; i < NAVDATA_PACKET_SIZE-2; i += 2) {
checksum += nav_port.buffer[i] + (nav_port.buffer[i+1] << 8);
}
// When checksum is incorrect
if(navdata.chksum != checksum) {
printf("Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",checksum , navdata.chksum, checksum-navdata.chksum);
nav_port.checksum_errors++;
}
// When we already dropped a packet or checksum is correct
if(last_checksum_wrong || navdata.chksum == checksum) {
// Invert byte order so that TELEMETRY works better
uint8_t tmp;
uint8_t* p = (uint8_t*) &(navdata.pressure);
tmp = p[0];
p[0] = p[1];
p[1] = tmp;
p = (uint8_t*) &(navdata.temperature_pressure);
tmp = p[0];
p[0] = p[1];
p[1] = tmp;
baro_update_logic();
#ifdef USE_SONAR
if (navdata.ultrasound < 10000)
{
sonar_meas = navdata.ultrasound;
ins_update_sonar();
}
#endif
navdata_imu_available = TRUE;
last_checksum_wrong = FALSE;
nav_port.packetsRead++;
}
// Crop the buffer
navdata_cropbuffer(NAVDATA_PACKET_SIZE);
}
else
{
// find start byte, copy all data from startbyte to buffer origin, update bytesread
uint8_t * pint;
pint = memchr(nav_port.buffer, NAVDATA_START_BYTE, nav_port.bytesRead);
if (pint != NULL) {
navdata_cropbuffer(pint - nav_port.buffer);
} else {
// if the start byte was not found, it means there is junk in the buffer
nav_port.bytesRead = 0;
}
}
}
}
