35 #include "pprzlink/messages.h"
36 #include "pprzlink/dl_protocol.h"
37 #include "pprzlink/pprz_transport.h"
38 #include "generated/airframe.h"
57 #define NPS_MAX_MSG_SIZE 512
59 int main(
int argc,
char **argv)
110 struct timespec requestStart;
111 struct timespec requestEnd;
112 struct timespec waitFor;
114 long int task_ns = 0;
119 int fd = open(INS_DEV, O_WRONLY | O_NOCTTY | O_SYNC);
121 printf(
"INS THREAD: data loop error opening port %i\n", fd);
125 struct termios new_settings;
126 tcgetattr(fd, &new_settings);
127 memset(&new_settings, 0,
sizeof(new_settings));
128 new_settings.c_iflag = 0;
129 new_settings.c_cflag = 0;
130 new_settings.c_lflag = 0;
131 new_settings.c_cc[VMIN] = 0;
132 new_settings.c_cc[VTIME] = 0;
133 cfsetispeed(&new_settings, (speed_t)INS_BAUD);
134 cfsetospeed(&new_settings, (speed_t)INS_BAUD);
135 tcsetattr(fd, TCSANOW, &new_settings);
147 memcpy(&fdm_ins, &
fdm,
sizeof(
fdm));
162 printf(
"INS THREAD: Warning - sent only %u bytes to the autopilot, instead of expected %u\n", wlen, idx);
168 task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
171 if (task_ns < period_ns) {
173 waitFor.tv_nsec = period_ns - task_ns;
174 nanosleep(&waitFor, NULL);
178 printf(
"INS THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
179 (
double)task_ns / 1E6, (
double)period_ns / 1E6);
191 int fd = open(AP_DEV, O_RDONLY | O_NOCTTY);
193 printf(
"AP data loop error opening port %i\n", fd);
197 struct termios new_settings;
198 tcgetattr(fd, &new_settings);
199 memset(&new_settings, 0,
sizeof(new_settings));
200 new_settings.c_iflag = 0;
201 new_settings.c_cflag = 0;
202 new_settings.c_lflag = 0;
203 new_settings.c_cc[VMIN] = 1;
204 new_settings.c_cc[VTIME] = 5;
205 cfsetispeed(&new_settings, (speed_t)AP_BAUD);
206 cfsetospeed(&new_settings, (speed_t)AP_BAUD);
207 tcsetattr(fd, TCSANOW, &new_settings);
214 struct pprz_transport pprz_tp_logger;
216 pprz_transport_init(&pprz_tp_logger);
223 if ((rx_msgs % 100 == 0) && show_stats && (rx_msgs > 0) ) {
224 printf(
"AP data loop received %i messages.\n", rx_msgs);
225 printf(
"From those, %i were COMMANDS messages, and %i were MOTOR_MIXING messages.\n",
226 rx_commands, rx_motor_mixing);
235 rdlen = read(fd, buf,
sizeof(buf) - 1);
237 for (
int i = 0; i < rdlen; i++) {
239 parse_pprz(&pprz_tp_logger, buf[i]);
242 if (pprz_tp_logger.trans_rx.msg_received) {
244 for (
int k = 0; k < pprz_tp_logger.trans_rx.payload_len; k++) {
245 buf[k] = pprz_tp_logger.trans_rx.payload[k];
248 uint8_t sender_id = SenderIdOfPprzMsg(buf);
249 uint8_t msg_id = IdOfPprzMsg(buf);
252 if (sender_id != AC_ID) {
264 cmd_len = DL_COMMANDS_values_length(buf);
265 memcpy(&cmd_buf, DL_COMMANDS_values(buf), cmd_len *
sizeof(
int16_t));
275 case DL_MOTOR_MIXING:
278 cmd_len = DL_MOTOR_MIXING_values_length(buf);
280 if (cmd_len > NPS_COMMANDS_NB) {
283 memcpy(&cmd_buf, DL_MOTOR_MIXING_values(buf), cmd_len *
sizeof(
int16_t));
295 pprz_tp_logger.trans_rx.msg_received =
false;
307 struct timespec requestStart;
308 struct timespec requestEnd;
309 struct timespec waitFor;
310 long int period_ns =
SIM_DT * 1000000000L;
311 long int task_ns = 0;
315 struct timespec startTime;
316 struct timespec realTime;
319 double real_secs = 0;
320 double real_time = 0;
331 real_time = real_secs - start_secs;
348 task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
351 if (task_ns < period_ns) {
353 waitFor.tv_nsec = period_ns - task_ns;
354 nanosleep(&waitFor, NULL);
358 printf(
"MAIN THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
359 (
double)task_ns / 1E6, (
double)period_ns / 1E6);
int main(int argc, char **argv)
void * nps_main_display(void *data)
pthread_mutex_t fdm_mutex
struct NpsAutopilot nps_autopilot
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
void nps_sensors_run_step(double time)
int nps_main_init(int argc, char **argv)
#define NPS_COMMANDS_NB
Number of commands sent to the FDM of NPS.
void nps_update_launch_from_dl(uint8_t value)
#define clock_get_current_time(_x)
Architecture independent timing functions.
void * nps_main_loop(void *data)
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
Update the simulation state.
void * nps_flight_gear_loop(void *data)
void * nps_ap_data_loop(void *data)
void * nps_ins_data_loop(void *data)
void nps_ins_fetch_data(struct NpsFdm *fdm_ins)
Fetch data from FDM and store them into vectornav packet NOTE: some noise is being added...
void nps_atmosphere_update(double dt)
double ntime_to_double(struct timespec *t)
void nps_radio_and_autopilot_init(void)
uint16_t nps_ins_fill_buffer(void)
Atmosphere model (pressure, wind) for NPS.
double commands[NPS_COMMANDS_NB]
void nps_main_run_sim_step(void)
void nps_autopilot_init(enum NpsRadioControlType type, int num_script, char *js_dev)