bool CRSFTelemetry::update(const hrt_abstime &now)
{
const int update_rate_hz = 10;
if (now - _last_update <= 1_s / (update_rate_hz * num_data_types)) {
return false;
}
bool sent = false;
switch (_next_type) {
case 0:
sent = send_battery();
break;
case 1:
sent = send_gps();
break;
case 2:
sent = send_attitude();
break;
case 3:
sent = send_flight_mode();
break;
}
_last_update = now;
_next_type = (_next_type + 1) % num_data_types;
return sent;
}
// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
{
int16_t payload_space = serial_free();
if (telemetry_delayed(chan)) {
return false;
}
switch(id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
send_heartbeat(chan);
break;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_location(chan);
break;
case MSG_AHRS:
CHECK_PAYLOAD_SIZE(AHRS);
send_ahrs(chan);
break;
case MSG_STATUSTEXT:
CHECK_PAYLOAD_SIZE(STATUSTEXT);
send_statustext(chan);
break;
default:
break;
}
return true;
}
void vIMU_tasks()
{
uint8_t time_count;
portTickType ticks_now,ticks_old = 0;
uint8_t IMU_update_count = 0;
IMU_setup();
Baro_init();
if(RCC_GetFlagStatus(RCC_FLAG_SFTRST) == RESET) //if it was a hardware reset
{
NVIC_SystemReset(); //force software reset so that MPU sensitivity returns to normal range
}
NVIC_Configuration();
SD_Init();
//////////////////////////****************************//////////////////////////////
// create_log_file(); //Uncomment to enable log on start, else log will start only when the button on GCS is pressed
//////////////////////////****************************//////////////////////////////
for(int y=0; y<=5; y++) // Read first initial ADC values for offset.
AN_OFFSET[y] = 0;
for(;;)
{
WDT_status |= 1; //update status for IMU tasks
if(log_init_flag)
{
log_data(); //append new data to buffer
}
ticks_now = xTaskGetTickCount()*2;
if((ticks_now - ticks_old) >= 17) //do the following every 17ms
{
G_Dt = (ticks_now - ticks_old)/1000.0;
ticks_old = ticks_now;
if((!calibrating_IMU) && (!calibrating_mag)) //update IMU only if not calibrating IMU or MAG
{
IMU_Update_ARDU();
HMC5883_calculate(roll,pitch);
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
update_variables();
}
else if(calibrating_mag) //if calibrating MAG then do not process IMU, only get raw values and obtain MAG offsets
{
if(IMU_update_count >= 5)
{
IMU_Update_ARDU();
IMU_update_count = 0;
}
else
IMU_update_count++;
}
altitude = ((Baro_update(G_Dt*1000)) - ground_alt_offset); //time in ms
innerloop(); //set servo output
if(time_count == 5) //do this every 5*17ms ~= 100ms
{
send_attitude();
send_attitude_RAW();
time_count = 0;
}
else
time_count++;
}
vTaskDelay( 17/ portTICK_RATE_MS ); //3ms less to compensate for IMU update time
}
}
