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;
}
}
}
