int yaw_ctrl_set_pos(float _pos)
{
if ((_pos < 0) || (_pos > 2.0 * M_PI))
{
LOG(LL_ERROR, "invalid yaw setpoint: %f, out of bounds: (0.0, 2.0 * M_PI)", _pos);
return -1;
}
tsfloat_set(&pos, _pos);
return 0;
}
int yaw_ctrl_set_speed(float _speed)
{
float _speed_min = tsfloat_get(&speed_min);
float _speed_max = tsfloat_get(&speed_max);
if ((_speed < _speed_min) || (_speed > _speed_max))
{
LOG(LL_ERROR, "invalid yaw speed: %f, out of bounds: (%f, %f)",
_speed, _speed_min, _speed_max);
return -1;
}
tsfloat_set(&speed, _speed);
return 0;
}
int auto_logic_set_yaw(float val)
{
tsfloat_set(&setp_yaw, val);
return 0;
}
int auto_logic_set_e(float val)
{
tsfloat_set(&setp_e, val);
return 0;
}
void yaw_ctrl_std_speed(void)
{
tsfloat_set(&speed, tsfloat_get(&speed_std));
}
static void scmd_multiplex(in_cmd_t command, const plchar_t *payload)
{
switch (command)
{
case IN_FC_ACK:
{
pthread_mutex_lock(&ack_mutex);
pthread_cond_signal(&ack);
pthread_mutex_unlock(&ack_mutex);
break;
}
case IN_FC_DEBUG:
{
fc_debug_t *debug_data = (fc_debug_t *)payload;
/*
* read system health data:
*/
pthread_mutex_lock(&health_data.mutex);
health_data.voltage = (float)debug_data->analog[9] / 10.0;
health_data.signal = debug_data->analog[10];
health_data.current = (float)debug_data->analog[22] / 10.0;
pthread_mutex_unlock(&health_data.mutex);
/*
* read barometer data:
*/
if (start_baro == 0.0)
{
start_baro = debug_data->analog[5] * 0.06;
}
tsfloat_set(&altitude, debug_data->analog[5] * 0.06 - start_baro);
/*
* read motor rpm:
*/
pthread_mutex_lock(&rpm_mutex);
for (int i = 0; i < platform_motors(); i++)
{
rpm[i] = debug_data->analog[i + 26] * 100.0f;
}
pthread_mutex_unlock(&rpm_mutex);
break;
}
case IN_FC_EXTERN_CONTROL:
case IN_FC_DATA_3D:
case IN_FC_DISPLAY_REQ_KEY:
case IN_FC_DISPLAY_REQ_MENU:
case IN_FC_MIXER_QUERY:
case IN_FC_MIXER_WRITE:
case IN_FC_PPM_CHANNELS:
case IN_FC_SETTINGS_REQUEST:
case IN_FC_SETTINGS_WRITE:
case IN_FC_ENGINE_TEST:
case IN_FC_VERSION:
case IN_M3_YAW:
break;
}
}
