static void mav_heartbeat(struct timer *t, void *d) { mavlink_message_t msg; LED = ~LED; if (get_millis() - uav_last_seen > UAV_LAST_SEEN_TIMEOUT) { set_timer_period(t, 5); return; } else { set_timer_period(t, 10); } mavlink_msg_heartbeat_pack(config.mav.osd_sysid, MAV_COMP_ID_OSD, &msg, MAV_TYPE_ALCEOSD, MAV_AUTOPILOT_INVALID, MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, // base_mode 0, //custom_mode MAV_STATE_ACTIVE); mavlink_send_msg(&msg); }
/*! \brief Extract the data packet from QTouch Studio and set global config. * \note Should only be called from the command handler. */ void Set_Global_Config(void) { touch_ret_t touch_ret; #ifdef DEF_TOUCH_QDEBUG_ENABLE_MUTLCAP touch_config_t *p_touch_config = &touch_config; touch_mutlcap_config_t *p_mutlcap_config = p_touch_config->p_mutlcap_config; p_mutlcap_config->global_param.recal_threshold = (recal_threshold_t)GetChar(); p_mutlcap_config->global_param.di = GetChar(); p_mutlcap_config->global_param.drift_hold_time = GetChar(); p_mutlcap_config->global_param.max_on_duration = GetChar(); p_mutlcap_config->global_param.tch_drift_rate = GetChar(); p_mutlcap_config->global_param.atch_drift_rate = GetChar(); p_mutlcap_config->global_param.atch_recal_delay = GetChar(); #endif #ifdef DEF_TOUCH_QDEBUG_ENABLE_SELFCAP touch_config_t *p_touch_config = &touch_config; touch_selfcap_config_t *p_selfcap_config = p_touch_config->p_selfcap_config; p_selfcap_config->global_param.recal_threshold = GetChar(); p_selfcap_config->global_param.di = GetChar(); p_selfcap_config->global_param.drift_hold_time = GetChar(); p_selfcap_config->global_param.max_on_duration = GetChar(); p_selfcap_config->global_param.tch_drift_rate = GetChar(); p_selfcap_config->global_param.atch_drift_rate = GetChar(); p_selfcap_config->global_param.atch_recal_delay = GetChar(); #endif touch_time.measurement_period_ms = (GetChar() << 8u); touch_time.measurement_period_ms |= GetChar(); #if ((BOARD == SAMD20_XPLAINED_PRO) || (BOARD == SAMD21_XPLAINED_PRO)) touch_time_counter = 0u; #else set_timer_period(touch_time.measurement_period_ms); #endif #ifdef DEF_TOUCH_QDEBUG_ENABLE_SELFCAP /* update the global parameter value inside the SAMD library */ touch_ret = QDEBUG_UPDATE_GLOBAL_PARAM_FUNC( &p_selfcap_config->global_param); if (touch_ret != TOUCH_SUCCESS) { while (1) { } } #endif #ifdef DEF_TOUCH_QDEBUG_ENABLE_MUTLCAP /* update the global parameter value inside the SAMD library */ touch_ret = QDEBUG_UPDATE_GLOBAL_PARAM_FUNC( &p_mutlcap_config->global_param); if (touch_ret != TOUCH_SUCCESS) { while (1) { } } #endif }
static void calc_home(struct timer *t, void *d) { mavlink_heartbeat_t *hb = mavdata_get(MAVLINK_MSG_ID_HEARTBEAT); mavlink_global_position_int_t *gpi; mavlink_message_t this_msg; switch (home.lock) { case HOME_NONE: default: if (mavdata_age(MAVLINK_MSG_ID_HEARTBEAT) > 5000) return; /* check arming status */ if (hb->base_mode & MAV_MODE_FLAG_SAFETY_ARMED) home.lock = HOME_WAIT; break; case HOME_WAIT: if (mavdata_age(MAVLINK_MSG_ID_MISSION_ITEM) > 2000) { /* when UAV is armed, home is WP0 */ mavlink_msg_mission_request_pack(config.mav.osd_sysid, MAV_COMP_ID_OSD, &this_msg, config.mav.uav_sysid, MAV_COMP_ID_ALL, 0); mavlink_send_msg(&this_msg); } else { mavlink_mission_item_t *mi = mavdata_get(MAVLINK_MSG_ID_MISSION_ITEM); priv.home_coord.lat = DEG2RAD(mi->x); priv.home_coord.lon = DEG2RAD(mi->y); priv.home_altitude = (unsigned int) mi->z; home.lock = HOME_GOT; } break; case HOME_GOT: home.lock = HOME_LOCKED; set_timer_period(t, 200); break; case HOME_LOCKED: { gpi = mavdata_get(MAVLINK_MSG_ID_GLOBAL_POSITION_INT); priv.uav_coord.lat = DEG2RAD(gpi->lat / 10000000.0); priv.uav_coord.lon = DEG2RAD(gpi->lon / 10000000.0); priv.altitude = (unsigned int) (gpi->alt / 1000); priv.heading = (int) (gpi->hdg / 100); home.uav_bearing = (int) get_bearing(&priv.home_coord, &priv.uav_coord); home.direction = home.uav_bearing + 180; home.direction -= priv.heading; if (home.direction < 0) home.direction += 360; home.distance = earth_distance(&priv.home_coord, &priv.uav_coord); home.altitude = priv.altitude - priv.home_altitude; break; case HOME_RESET: home.lock = HOME_NONE; set_timer_period(t, 1000); break; case HOME_FORCE: gpi = mavdata_get(MAVLINK_MSG_ID_GLOBAL_POSITION_INT); priv.home_coord.lat = DEG2RAD(gpi->lat / 10000000.0); priv.home_coord.lon = DEG2RAD(gpi->lon / 10000000.0); priv.home_altitude = (unsigned int) (gpi->alt / 1000); home.lock = HOME_GOT; break; } } }