void pos_init(void)
{
ASSERT_ONCE();
/* read configuration and initialize scl gates: */
opcd_param_t params[] =
{
{"process_noise", &process_noise},
{"ultra_noise", &ultra_noise},
{"baro_noise", &baro_noise},
{"gps_noise", &gps_noise},
{"use_gps_speed", &use_gps_speed},
OPCD_PARAMS_END
};
opcd_params_apply("kalman_pos.", params);
LOG(LL_DEBUG, "process noise: %f, ultra noise: %f, baro noise: %f, gps noise: %f",
tsfloat_get(&process_noise),
tsfloat_get(&ultra_noise),
tsfloat_get(&baro_noise),
tsfloat_get(&gps_noise));
/* set-up kalman filters: */
kalman_init(&e_kalman, &process_noise, &gps_noise, 0, 0, tsint_get(&use_gps_speed));
kalman_init(&n_kalman, &process_noise, &gps_noise, 0, 0, tsint_get(&use_gps_speed));
kalman_init(&baro_u_kalman, &process_noise, &baro_noise, 0, 0, false);
kalman_init(&ultra_u_kalman, &process_noise, &ultra_noise, 0, 0, false);
}
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);
}
float yaw_ctrl_step(float *err_out, float yaw, float _speed, float dt)
{
float err;
float yaw_ctrl;
if (tsint_get(&manual))
{
yaw_ctrl = 0.0f;
err = 0.0; /* we control nothing, so the error is always 0 */
}
else
{
err = circle_err(yaw, tsfloat_get(&pos));
float speed_setpoint = -speed_func(err);
float speed_err = speed_setpoint - _speed;
yaw_ctrl = pid_control(&controller, speed_err, dt);
}
*err_out = err;
return yaw_ctrl;
}
int channels_init(void)
{
ASSERT_ONCE();
opcd_param_t params[] =
{
{"pitch.index", &pitch_index},
{"pitch.max", &pitch_max},
{"pitch.min", &pitch_min},
{"roll.index", &roll_index},
{"roll.max", &roll_max},
{"roll.min", &roll_min},
{"yaw.index", &yaw_index},
{"yaw.max", &yaw_max},
{"yaw.min", &yaw_min},
{"gas.index", &gas_index},
{"gas.max", &gas_max},
{"gas.min", &gas_min},
{"two_state.index", &two_state_index},
{"two_state.max", &two_state_max},
{"two_state.min", &two_state_min},
{"three_state.index", &three_state_index},
{"three_state.max", &three_state_max},
{"three_state.min", &three_state_min},
OPCD_PARAMS_END
};
opcd_params_apply(".", params);
/* check if channels are configured: */
if (tsint_get(&pitch_index) == -1)
{
LOG(LL_INFO, "channels not configured; use pp_rc_cal and start the service again");
return -1;
}
linfunc_init_points(&pitch_linfunc, tsfloat_get(&pitch_min), -1.0f, tsfloat_get(&pitch_max), 1.0f);
linfunc_init_points(&roll_linfunc, tsfloat_get(&roll_min), -1.0f, tsfloat_get(&roll_max), 1.0f);
linfunc_init_points(&yaw_linfunc, tsfloat_get(&yaw_min), -1.0f, tsfloat_get(&yaw_max), 1.0f);
linfunc_init_points(&gas_linfunc, tsfloat_get(&gas_min), -1.0f, tsfloat_get(&gas_max), 1.0f);
linfunc_init_points(&two_state_linfunc, tsfloat_get(&two_state_min), 0.0f, tsfloat_get(&two_state_max), 1.0f);
linfunc_init_points(&three_state_linfunc, tsfloat_get(&three_state_min), 0.0f, tsfloat_get(&three_state_max), 1.0f);
return 0;
}
void channels_update(float out[PP_MAX_CHANNELS], float in[16])
{
memset(out, 0, sizeof(float) * PP_MAX_CHANNELS);
out[CH_PITCH] = linfunc_calc(&pitch_linfunc, in[tsint_get(&pitch_index)]);
out[CH_ROLL] = linfunc_calc(&roll_linfunc, in[tsint_get(&roll_index)]);
out[CH_YAW] = linfunc_calc(&yaw_linfunc, in[tsint_get(&yaw_index)]);
out[CH_GAS] = linfunc_calc(&gas_linfunc, in[tsint_get(&gas_index)]);
int twsi = tsint_get(&two_state_index);
if (twsi != -1)
out[CH_TWO_STATE] = linfunc_calc(&two_state_linfunc, in[twsi]);
int trsi = tsint_get(&three_state_index);
if (trsi != -1)
out[CH_THREE_STATE] = linfunc_calc(&three_state_linfunc, in[trsi]);
}
int _main(void)
{
THROW_BEGIN()
/* init scl and get sockets:: */
THROW_ON_ERR(scl_init("arduino"));
void *rc_socket = scl_get_socket("rc_raw");
THROW_IF(rc_socket == NULL, -ENODEV);
void *power_socket = scl_get_socket("power");
THROW_IF(power_socket == NULL, -ENODEV);
/* allocate msgpack buffers: */
msgpack_sbuffer *msgpack_buf = msgpack_sbuffer_new();
THROW_IF(msgpack_buf == NULL, -ENOMEM);
msgpack_packer *pk = msgpack_packer_new(msgpack_buf, msgpack_sbuffer_write);
THROW_IF(pk == NULL, -ENOMEM);
/* fetch parameters: */
char *dev_path;
tsint_t dev_speed;
opcd_params_init("exynos_quad.arduino_serial.", 0);
opcd_param_t params[] =
{
{"path", &dev_path},
{"speed", &dev_speed},
OPCD_PARAMS_END
};
opcd_params_apply("", params);
/* opern serial port: */
serialport_t port;
THROW_ON_ERR(serial_open(&port, dev_path, tsint_get(&dev_speed), O_RDONLY));
uint16_t channels_raw[PPM_CHAN_MAX];
uint32_t voltage_raw, current_raw;
float channels[PPM_CHAN_MAX];
while (running)
{
int c = serial_read_char(&port);
if (c < 0)
msleep(1);
/* parse channels: */
int status = ppm_parse_frame(channels_raw, (uint8_t)(c));
if (status)
{
int sig_valid = 0;
int invalid_count = 0;
FOR_N(i, PPM_CHAN_MAX)
{
uint16_t chan_in = channels_raw[i];
if (chan_in >= PPM_VALUE_INVALID)
invalid_count++;
if (chan_in > PPM_VALUE_MAX)
chan_in = PPM_VALUE_MAX;
if (chan_in < PPM_VALUE_MIN)
chan_in = PPM_VALUE_MIN;
channels[i] = (float)(chan_in - PPM_VALUE_MIN) / (PPM_VALUE_MAX - PPM_VALUE_MIN) * 2.f - 1.f;
}
sig_valid = (invalid_count < (PPM_CHAN_MAX / 3));
/* send channels: */
msgpack_sbuffer_clear(msgpack_buf);
msgpack_pack_array(pk, 1 + PPM_CHAN_MAX);
PACKI(sig_valid); /* index 0: valid */
PACKFV(channels, PPM_CHAN_MAX); /* index 1, .. : channels */
scl_copy_send_dynamic(rc_socket, msgpack_buf->data, msgpack_buf->size);
}
/* parse adc voltage/current: */
status = power_parse_frame(&voltage_raw, ¤t_raw, (uint8_t)(c));
if (status)
{
float voltage = (float)(voltage_raw) / 1000.0;
float current = (float)(current_raw) / 1000.0;
/* send voltage / current: */
msgpack_sbuffer_clear(msgpack_buf);
msgpack_pack_array(pk, 2);
PACKF(voltage); /* index 0 */
PACKF(current); /* index 1 */
scl_copy_send_dynamic(power_socket, msgpack_buf->data, msgpack_buf->size);
}
}
void _main(int argc, char *argv[])
{
(void)argc;
(void)argv;
if (scl_init("gpsp") != 0)
{
syslog(LOG_CRIT, "could not init scl module");
exit(EXIT_FAILURE);
}
gps_socket = scl_get_socket("data");
if (gps_socket == NULL)
{
syslog(LOG_CRIT, "could not get scl gate");
exit(EXIT_FAILURE);
}
int64_t hwm = 1;
zmq_setsockopt(gps_socket, ZMQ_SNDHWM, &hwm, sizeof(hwm));
opcd_param_t params[] =
{
{"serial_path", &serial_path},
{"serial_speed", &serial_speed},
OPCD_PARAMS_END
};
opcd_params_init("sensors.gps.", 0);
opcd_params_apply("", params);
int status = serial_open(&port, serial_path, tsint_get(&serial_speed), 0, 0, 0);
if (status < 0)
{
syslog(LOG_CRIT, "could not open serial port: %s", serial_path);
exit(EXIT_FAILURE);
}
nmeaPARSER parser;
nmea_parser_init(&parser);
nmeaINFO info;
nmea_zero_INFO(&info);
int time_set = 0;
int smask = 0; /* global smask collects all sentences and is never reset,
in contrast to info.smask */
while (running)
{
int c = serial_read_char(&port);
if (c < 0)
continue;
char b = c;
/* parse NMEA frame: */
if (nmea_parse(&parser, &b, 1, &info) == 1)
{
smask |= info.smask;
if ( (info.smask & GPGGA) /* check for new position update */
&& (smask & (GPGSA | GPRMC))) /* go sure that we collect all sentences for first output*/
{
GpsData gps_data = GPS_DATA__INIT;
/* set general data: */
char time_str[TIME_STR_LEN];
generate_time_str(time_str, &info.utc);
gps_data.fix = 0;
gps_data.time = time_str;
/* set system time to gps time once: */
if (!time_set && info.fix >= 2)
{
char shell_date_cmd[TIME_STR_LEN + 8];
linux_sys_set_timezone(convert(info.lat), convert(info.lon));
sprintf(shell_date_cmd, "date -s \"%s\"", time_str);
time_set = system(shell_date_cmd) == 0;
}
/* set position data if a minimum of satellites is seen: */
if (info.fix >= 2)
{
gps_data.fix = 2;
PB_SET(gps_data, hdop, info.HDOP);
PB_SET(gps_data, lat, convert(info.lat));
PB_SET(gps_data, lon, convert(info.lon));
PB_SET(gps_data, sats, info.satinfo.inuse);
PB_SET(gps_data, course, info.track);
PB_SET(gps_data, speed, info.speed);
}
/* set data for 3d fix: */
if (info.fix == 3)
{
gps_data.fix = 3;
PB_SET(gps_data, vdop, info.VDOP);
PB_SET(gps_data, alt, info.elv);
}
/* add satellit info: */
unsigned int i;
gps_data.n_satinfo = info.satinfo.inview;
SatInfo **satinfo = malloc(gps_data.n_satinfo * sizeof(SatInfo *));
for (i = 0; i < gps_data.n_satinfo; i++)
{
/* fill SatInfo structure: */
nmeaSATELLITE *nmea_satinfo = &info.satinfo.sat[i];
satinfo[i] = malloc(gps_data.n_satinfo * sizeof(SatInfo));
sat_info__init(satinfo[i]);
satinfo[i]->id = nmea_satinfo->id;
satinfo[i]->in_use = info.satinfo.in_use[i] ? 1 : 0;
satinfo[i]->elv = nmea_satinfo->elv;
satinfo[i]->azimuth = nmea_satinfo->azimuth;
satinfo[i]->sig = nmea_satinfo->sig;
}
gps_data.satinfo = satinfo;
/* send the data: */
SCL_PACK_AND_SEND_DYNAMIC(gps_socket, gps_data, gps_data);
/* free allocated memory: */
for (i = 0; i < gps_data.n_satinfo; i++)
{
free(satinfo[i]);
}
free(satinfo);
}
nmea_zero_INFO(&info);
}
}
}
