void vTask10HZ(void *pvParameters) { for(;;) { vTaskDelay( 100 / portTICK_RATE_MS ); // calculate osd_curr_consumed_mah(simulation) osd_curr_consumed_mah += (osd_curr_A * 0.00027777778f); // calculate osd_total_trip_dist(simulation) if (osd_groundspeed > 1.0f) osd_total_trip_dist += (osd_groundspeed * 0.1f); // jmmods > for calculation of trip , Groundspeed is better than airspeed //trigger video switch if(eeprom_buffer.params.PWM_Video_en) { triggerVideo(); } //trigger panel switch if(eeprom_buffer.params.PWM_Panel_en) { triggerPanel(); } //if no mavlink update for 2 secs, show waring and request mavlink rate again if(GetSystimeMS() > (lastMAVBeat + 2200)) { heatbeat_start_time = 0; waitingMAVBeats = 1; } if(enable_mav_request == 1) { for(int n = 0; n < 3; n++){ request_mavlink_rates();//Three times to certify it will be readed vTaskDelay(50/portTICK_RATE_MS); } enable_mav_request = 0; waitingMAVBeats = 0; lastMAVBeat = GetSystimeMS(); } if(enable_mission_count_request == 1) { request_mission_count(); enable_mission_count_request = 0; } if(enable_mission_item_request == 1) { request_mission_item(current_mission_item_req_index); } } }
void read_mavlink(){ mavlink_message_t msg; mavlink_status_t status; while (mavlink_serial_port::in_avail() > 0) { uint8_t ch = mavlink_serial_port::get(); if(mavlink_parse_char(MAVLINK_COMM_0, ch, &msg, &status)) { mavlink_active = true; switch(msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: do_mavlink_heartbeat(&msg); if ( rate_request_sent_flag == false){ request_mavlink_rates(); } break; case MAVLINK_MSG_ID_SYS_STATUS: do_mavlink_sys_status(&msg); break; #ifndef MAVLINK10 case MAVLINK_MSG_ID_GPS_RAW: do_mavlink_gps_raw(&msg); break; case MAVLINK_MSG_ID_GPS_STATUS: do_mavlink_gps_status(&msg); break; #else case MAVLINK_MSG_ID_GPS_RAW_INT: do_mavlink_gps_raw_int(&msg); break; #endif case MAVLINK_MSG_ID_VFR_HUD: do_mavlink_vfr_hud(&msg); break; case MAVLINK_MSG_ID_ATTITUDE: do_mavlink_attitude(&msg); break; default: break; } } } packet_drops += status.packet_rx_drop_count; parse_error += status.parse_error; }
void vTask10HZ(void *pvParameters) { for (;; ) { vTaskDelay(100 / portTICK_RATE_MS); update_current_consumed_estimate(); update_total_trip_distance(); //trigger video switch if (eeprom_buffer.params.PWM_Video_en) { triggerVideo(); } //trigger panel switch if (eeprom_buffer.params.PWM_Panel_en) { triggerPanel(); } //if no mavlink update for 2 secs, show warning and request mavlink rate again if (GetSystimeMS() > (get_lastMAVBeat() + 2200)) { set_heartbeat_start_time(0); set_waitingMAVBeats(1); } if (get_enable_mav_request() == 1) { for (int n = 0; n < 3; n++) { request_mavlink_rates(); //Three times to certify it will be readed vTaskDelay(50 / portTICK_RATE_MS); } set_enable_mav_request(0); set_waitingMAVBeats(0); set_lastMAVBeat(GetSystimeMS()); } update_mission_counts(); } }