bool auto_logic_run(bool *hard_off, bool is_full_auto, uint16_t sensor_status, bool flying, float channels[MAX_CHANNELS], float yaw, vec2_t *ne_gps_pos, float u_baro_pos, float u_ultra_pos)
{
int rc_valid = sensor_status & RC_VALID;
if (is_full_auto || rc_valid)
{
float gas_stick = channels[CH_GAS];
if (is_full_auto || gas_stick > 0.5)
{
pthread_mutex_lock(&mutex);
if (mode_is_ground)
cm_u_set_ultra_pos(setp_u);
else
cm_u_set_baro_pos(setp_u);
pthread_mutex_unlock(&mutex);
}
else
cm_u_set_spd((gas_stick - 0.5) * 5.0);
}
else
cm_u_set_spd(-0.5);
if (u_ultra_pos > 1.0)
{
float yaw_stick = channels[CH_YAW];
if (is_full_auto || (rc_valid && fabs(yaw_stick) < 0.05))
cm_yaw_set_pos(tsfloat_get(&setp_yaw));
else
cm_yaw_set_spd(yaw_stick * 2.0f);
}
else
cm_yaw_set_spd(0.0f);
float pitch = channels[CH_PITCH];
float roll = channels[CH_ROLL];
float sw_l = channels[CH_SWITCH_L];
if (!is_full_auto && rc_valid && !is_full_auto && sqrt(pitch * pitch + roll * roll) > 0.1)
hyst_gps_override = 1.0;
hyst_gps_override -= 0.006;
if (hyst_gps_override > 0.0)
{
vec2_t angles;
vec2_set(&angles, pitch, roll);
cm_att_set_angles(&angles);
}
else
{
vec2_t ne_gps_setpoint;
vec2_set(&ne_gps_setpoint, tsfloat_get(&setp_n), tsfloat_get(&setp_e));
cm_att_set_gps_pos(&ne_gps_setpoint);
}
if (!is_full_auto && (sensor_status & RC_VALID) && sw_l > 0.5)
*hard_off = true;
return tsint_get(&motors_enabled);
}
bool auto_logic_run(bool *hard_off, bool is_full_auto, uint16_t sensor_status, bool flying, float channels[PP_MAX_CHANNELS], float yaw, vec2_t *ne_gps_pos, float u_baro_pos, float u_ultra_pos)
{
/* set u position: */
pthread_mutex_lock(&mutex);
if (mode_is_ground)
cm_u_set_ultra_pos(setp_u);
else
cm_u_set_baro_pos(setp_u);
pthread_mutex_unlock(&mutex);
/* set gps position: */
vec2_t ne_gps_setpoint;
vec2_set(&ne_gps_setpoint, tsfloat_get(&setp_n), tsfloat_get(&setp_e));
cm_att_set_gps_pos(&ne_gps_setpoint);
/* set yaw position: */
cm_yaw_set_pos(tsfloat_get(&setp_yaw));
/* safe_auto code: */
if (!is_full_auto && (sensor_status & RC_VALID))
{
printf("SAFE_AUTO\n");
float sw_l = channels[CH_SWITCH_L];
if (sw_l > 0.5)
{
*hard_off = true;
}
float gas_stick = channels[CH_GAS];
if (gas_stick < 0.8)
cm_u_set_acc((gas_stick - 0.5) * 5.0);
float pitch = channels[CH_PITCH];
float roll = channels[CH_ROLL];
if (sqrt(pitch * pitch + roll * roll) > 0.1)
hyst_gps_override = 1.0;
hyst_gps_override -= 0.006;
if (hyst_gps_override > 0.0)
{
vec2_t angles;
vec2_set(&angles, pitch, roll);
cm_att_set_angles(&angles);
}
}
return tsint_get(&motors_enabled);
}