diff options
Diffstat (limited to 'apps')
-rw-r--r-- | apps/commander/commander.c | 20 | ||||
-rw-r--r-- | apps/sdlog/sdlog.c | 51 |
2 files changed, 49 insertions, 22 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index c569a6aa4..78fac5c22 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -296,7 +296,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) int calibration_interval_ms = 30 * 1000; unsigned int calibration_counter = 0; - float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + float mag_max[3] = {FLT_MIN, FLT_MIN, FLT_MIN}; float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; int fd = open(MAG_DEVICE_PATH, 0); @@ -353,6 +353,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) } } + usleep(200000); + mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); /* disable calibration mode */ @@ -377,7 +379,15 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - printf("mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]); + if (!isfinite(mag_offset[0]) || !isfinite(mag_offset[1]) || !isfinite(mag_offset[2])) + { + mavlink_log_info(mavlink_fd, "[commander] mag cal aborted: NaN"); + } + + //char buf[52]; + + //sprintf(buf, "mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]); + //mavlink_log_info(mavlink_fd, buf); /* announce and set new offset */ @@ -412,7 +422,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) warn("WARNING: auto-save of params to EEPROM failed"); } - mavlink_log_info(mavlink_fd, "[commander] magnetometer calibration finished"); + mavlink_log_info(mavlink_fd, "[commander] mag cal finished"); close(sub_sensor_combined); } @@ -519,7 +529,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); - const int calibration_count = 5000; + const int calibration_count = 2500; int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s raw; @@ -954,7 +964,7 @@ int commander_main(int argc, char *argv[]) deamon_task = task_spawn("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 50, - 4096, + 8000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 22b8d82ee..c02cf2fcf 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -91,6 +91,11 @@ static void usage(const char *reason); static int file_exist(const char *filename); /** + * Print the current status. + */ +static void print_sdlog_status(); + +/** * Create folder for current logging session. */ static int create_logfolder(char* folder_path); @@ -103,6 +108,14 @@ usage(const char *reason) errx(1, "usage: sdlog {start|stop|status} [-p <additional params>]\n\n"); } +// XXX turn this into a C++ class +unsigned sensor_combined_bytes = 0; +unsigned actuator_outputs_bytes = 0; +unsigned actuator_controls_bytes = 0; +unsigned sysvector_bytes = 0; +unsigned blackbox_file_bytes = 0; +uint64_t starttime = 0; + /** * The sd log deamon app only briefly exists to start * the background job. The stack size assigned in the @@ -145,7 +158,7 @@ int sdlog_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tsdlog is running\n"); + print_sdlog_status(); } else { printf("\tsdlog not started\n"); } @@ -194,7 +207,7 @@ int create_logfolder(char* folder_path) { int sdlog_thread_main(int argc, char *argv[]) { - printf("[sdlog] starting\n"); + warnx("starting\n"); if (file_exist(mountpoint) != OK) { errx(1, "logging mount point %s not present, exiting.", mountpoint); @@ -206,21 +219,16 @@ int sdlog_thread_main(int argc, char *argv[]) { /* create sensorfile */ int sensorfile = -1; - unsigned sensor_combined_bytes = 0; int actuator_outputs_file = -1; - unsigned actuator_outputs_bytes = 0; int actuator_controls_file = -1; - unsigned actuator_controls_bytes = 0; int sysvector_file = -1; - unsigned sysvector_bytes = 0; FILE *gpsfile; - unsigned blackbox_file_bytes = 0; FILE *blackbox_file; // FILE *vehiclefile; char path_buf[64] = ""; // string to hold the path to the sensorfile - printf("[sdlog] logging to directory %s\n", folder_path); + warnx("logging to directory %s\n", folder_path); /* set up file path: e.g. /mnt/sdcard/session0001/sensor_combined.bin */ sprintf(path_buf, "%s/%s.bin", folder_path, "sensor_combined"); @@ -347,14 +355,14 @@ int sdlog_thread_main(int argc, char *argv[]) { /* --- GLOBAL POSITION --- */ /* subscribe to ORB for global position */ - subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); fds[fdsc_count].fd = subs.global_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- GPS POSITION --- */ /* subscribe to ORB for global position */ - subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); fds[fdsc_count].fd = subs.gps_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -378,7 +386,7 @@ int sdlog_thread_main(int argc, char *argv[]) { int poll_count = 0; - uint64_t starttime = hrt_absolute_time(); + starttime = hrt_absolute_time(); while (!thread_should_exit) { @@ -484,6 +492,7 @@ int sdlog_thread_main(int argc, char *argv[]) { float vbat; float adc[3]; float local_pos[3]; + int32_t gps_pos[3]; } sysvector = { .timestamp = buf.raw.timestamp, .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, @@ -497,7 +506,8 @@ int sdlog_thread_main(int argc, char *argv[]) { buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]}, .vbat = buf.raw.battery_voltage_v, .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, - .local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z} + .local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, + .gps_pos = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt} }; #pragma pack(pop) @@ -506,13 +516,11 @@ int sdlog_thread_main(int argc, char *argv[]) { usleep(10000); } - unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes; - float mebibytes = bytes / 1024.0f / 1024.0f; - float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f; + fsync(sysvector_file); - printf("[sdlog] wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds)); + print_sdlog_status(); - printf("[sdlog] exiting.\n"); + warnx("exiting.\n"); close(sensorfile); close(actuator_outputs_file); @@ -525,6 +533,15 @@ int sdlog_thread_main(int argc, char *argv[]) { return 0; } +void print_sdlog_status() +{ + unsigned bytes = sysvector_bytes + sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes; + float mebibytes = bytes / 1024.0f / 1024.0f; + float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f; + + warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds)); +} + /** * @return 0 if file exists */ |