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); } } }
void _main(int argc, char *argv[]) { (void)argc; (void)argv; syslog(LOG_INFO, "initializing core"); /* init SCL subsystem: */ syslog(LOG_INFO, "initializing signaling and communication link (SCL)"); if (scl_init("core") != 0) { syslog(LOG_CRIT, "could not init scl module"); exit(EXIT_FAILURE); } /* init params subsystem: */ syslog(LOG_INFO, "initializing opcd interface"); opcd_params_init("core.", 1); /* initialize logger: */ syslog(LOG_INFO, "opening logger"); if (logger_open() != 0) { syslog(LOG_CRIT, "could not open logger"); exit(EXIT_FAILURE); } syslog(LOG_CRIT, "logger opened"); sleep(1); /* give scl some time to establish a link between publisher and subscriber */ LOG(LL_INFO, "+------------------+"); LOG(LL_INFO, "| core startup |"); LOG(LL_INFO, "+------------------+"); LOG(LL_INFO, "initializing system"); /* set-up real-time scheduling: */ struct sched_param sp; sp.sched_priority = sched_get_priority_max(SCHED_FIFO); sched_setscheduler(getpid(), SCHED_FIFO, &sp); if (mlockall(MCL_CURRENT | MCL_FUTURE)) { LOG(LL_ERROR, "mlockall() failed"); exit(EXIT_FAILURE); } /* initialize hardware/drivers: */ omap_i2c_bus_init(); baro_altimeter_init(); ultra_altimeter_init(); ahrs_init(); motors_init(); voltage_reader_start(); //gps_init(); LOG(LL_INFO, "initializing model/controller"); model_init(); ctrl_init(); /* initialize command interface */ LOG(LL_INFO, "initializing cmd interface"); cmd_init(); /* prepare main loop: */ for (int i = 0; i < NUM_AVG; i++) { output_avg[i] = sliding_avg_create(OUTPUT_RATIO, 0.0f); } LOG(LL_INFO, "system up and running"); struct timespec ts_curr; struct timespec ts_prev; struct timespec ts_diff; clock_gettime(CLOCK_REALTIME, &ts_curr); /* run model and controller: */ while (1) { /* calculate dt: */ ts_prev = ts_curr; clock_gettime(CLOCK_REALTIME, &ts_curr); TIMESPEC_SUB(ts_diff, ts_curr, ts_prev); float dt = (float)ts_diff.tv_sec + (float)ts_diff.tv_nsec / (float)NSEC_PER_SEC; /* read sensor values into model input structure: */ model_input_t model_input; model_input.dt = dt; ahrs_read(&model_input.ahrs_data); gps_read(&model_input.gps_data); model_input.ultra_z = ultra_altimeter_read(); model_input.baro_z = baro_altimeter_read(); /* execute model step: */ model_state_t model_state; model_step(&model_state, &model_input); /* execute controller step: */ mixer_in_t mixer_in; ctrl_step(&mixer_in, dt, &model_state); /* set up mixer input: */ mixer_in.pitch = sliding_avg_calc(output_avg[AVG_PITCH], mixer_in.pitch); mixer_in.roll = sliding_avg_calc(output_avg[AVG_ROLL], mixer_in.roll); mixer_in.yaw = sliding_avg_calc(output_avg[AVG_YAW], mixer_in.yaw); mixer_in.gas = sliding_avg_calc(output_avg[AVG_GAS], mixer_in.gas); /* write data to motor mixer: */ EVERY_N_TIMES(OUTPUT_RATIO, motors_write(&mixer_in)); } }
void main_init(int argc, char *argv[]) { bool override_hw = false; if (argc > 1) { if (strcmp(argv[1], "calibrate") == 0) calibrate = true; else override_hw = true; } /* init data structures: */ memset(&pos_in, 0, sizeof(pos_in_t)); vec3_init(&pos_in.acc); /* init SCL subsystem: */ syslog(LOG_INFO, "initializing signaling and communication link (SCL)"); if (scl_init("pilot") != 0) { syslog(LOG_CRIT, "could not init scl module"); die(); } /* init params subsystem: */ syslog(LOG_INFO, "initializing opcd interface"); opcd_params_init("pilot.", 1); /* initialize logger: */ syslog(LOG_INFO, "opening logger"); if (logger_open() != 0) { syslog(LOG_CRIT, "could not open logger"); die(); } syslog(LOG_CRIT, "logger opened"); LOG(LL_INFO, "initializing platform"); if (arcade_quad_init(&platform, override_hw) < 0) { LOG(LL_ERROR, "could not initialize platform"); die(); } acc_mag_cal_init(); cmc_init(); const size_t array_len = sizeof(float) * platform.n_motors; setpoints = malloc(array_len); ASSERT_NOT_NULL(setpoints); memset(setpoints, 0, array_len); rpm_square = malloc(array_len); ASSERT_NOT_NULL(rpm_square); memset(rpm_square, 0, array_len); LOG(LL_INFO, "initializing model/controller"); pos_init(); ne_speed_ctrl_init(REALTIME_PERIOD); att_ctrl_init(); yaw_ctrl_init(); u_ctrl_init(); u_speed_init(); navi_init(); LOG(LL_INFO, "initializing command interface"); cmd_init(); motors_state_init(); blackbox_init(); /* init flight logic: */ flight_logic_init(); /* init calibration data: */ cal_init(&gyro_cal, 3, 1000); cal_ahrs_init(); flight_state_init(50, 150, 4.0); piid_init(REALTIME_PERIOD); interval_init(&gyro_move_interval); gps_data_init(&gps_data); mag_decl_init(); cal_init(&rc_cal, 3, 500); tsfloat_t acc_fg; opcd_param_t params[] = { {"acc_fg", &acc_fg}, OPCD_PARAMS_END }; opcd_params_apply("main.", params); filter1_lp_init(&lp_filter, tsfloat_get(&acc_fg), 0.06, 3); cm_init(); mon_init(); LOG(LL_INFO, "entering main loop"); }