diff options
-rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.c | 1 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 42 | ||||
-rw-r--r-- | apps/sdlog/sdlog.c | 52 | ||||
-rw-r--r-- | apps/uORB/topics/actuator_controls.h | 1 | ||||
-rw-r--r-- | apps/uORB/topics/actuator_outputs.h | 1 |
5 files changed, 55 insertions, 42 deletions
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index dd7a5655d..787db1877 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -331,6 +331,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor static uint64_t last_motor_time = 0; static struct actuator_outputs_s outputs; + outputs.timestamp = hrt_absolute_time(); outputs.output[0] = motor1; outputs.output[1] = motor2; outputs.output[2] = motor3; diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index ecaf6452c..ee3879b41 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -777,10 +777,10 @@ static void *uorb_receiveloop(void *arg) last_sensor_timestamp = buf.raw.timestamp; /* send raw imu data */ - mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.raw.accelerometer_raw[0], - buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], - buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], - buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]); + // mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.raw.accelerometer_raw[0], + // buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], + // buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], + // buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]); /* mark individual fields as changed */ uint16_t fields_updated = 0; @@ -835,7 +835,7 @@ static void *uorb_receiveloop(void *arg) orb_copy(ORB_ID(vehicle_attitude), subs->att_sub, &buf.att); /* send sensor values */ - mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed); + mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed); attitude_counter++; } @@ -1073,7 +1073,7 @@ static void *uorb_receiveloop(void *arg) /* copy local position data into local buffer */ orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control); mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll, - buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 1, 1, 1, 1); + buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 0); } /* --- ACTUATOR ARMED --- */ @@ -1320,23 +1320,23 @@ void handleMessage(mavlink_message_t *msg) memset(&rc_hil, 0, sizeof(rc_hil)); static orb_advert_t rc_pub = 0; - rc_hil.chan[0].raw = 1510 + man.roll * 500; - rc_hil.chan[1].raw = 1520 + man.pitch * 500; - rc_hil.chan[2].raw = 1590 + man.yaw * 500; - rc_hil.chan[3].raw = 1420 + man.thrust * 500; + rc_hil.chan[0].raw = 1510 + man.x * 500; + rc_hil.chan[1].raw = 1520 + man.y * 500; + rc_hil.chan[2].raw = 1590 + man.r * 500; + rc_hil.chan[3].raw = 1420 + man.z * 500; - rc_hil.chan[0].scaled = man.roll; - rc_hil.chan[1].scaled = man.pitch; - rc_hil.chan[2].scaled = man.yaw; - rc_hil.chan[3].scaled = man.thrust; + rc_hil.chan[0].scaled = man.x; + rc_hil.chan[1].scaled = man.y; + rc_hil.chan[2].scaled = man.r; + rc_hil.chan[3].scaled = man.z; struct manual_control_setpoint_s mc; static orb_advert_t mc_pub = 0; - mc.roll = man.roll; - mc.pitch = man.roll; - mc.yaw = man.roll; - mc.roll = man.roll; + mc.roll = man.x; + mc.pitch = man.y; + mc.yaw = man.r; + mc.roll = man.z; /* fake RC channels with manual control input from simulator */ @@ -1578,15 +1578,15 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200); } else if (baudrate >= 230400) { /* 200 Hz / 5 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 100); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20); /* 50 Hz / 20 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); /* 20 Hz / 50 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (baudrate >= 115200) { /* 50 Hz / 20 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 1ef838850..eaf609090 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -195,10 +195,12 @@ int sdlog_thread_main(int argc, char *argv[]) { errx(1, "unable to create logging folder, exiting"); /* create sensorfile */ - int sensorfile; + int sensorfile = -1; unsigned sensor_combined_bytes = 0; - int actuator_outputs_file; + int actuator_outputs_file = -1; unsigned actuator_outputs_bytes = 0; + int actuator_controls_file = -1; + unsigned actuator_controls_bytes = 0; FILE *gpsfile; unsigned blackbox_file_bytes = 0; FILE *blackbox_file; @@ -208,18 +210,23 @@ int sdlog_thread_main(int argc, char *argv[]) { printf("[sdlog] logging to directory %s\n", folder_path); - /* set up file path: e.g. /mnt/sdcard/session0001/sensors_combined.bin */ - sprintf(path_buf, "%s/%s.bin", folder_path, "sensors_combined"); + /* set up file path: e.g. /mnt/sdcard/session0001/sensor_combined.bin */ + sprintf(path_buf, "%s/%s.bin", folder_path, "sensor_combined"); if (0 == (sensorfile = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { errx(1, "opening %s failed.\n", path_buf); } - /* set up file path: e.g. /mnt/sdcard/session0001/actuator_outputs0.bin */ - sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_outputs0"); - if (0 == (actuator_outputs_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { + // /* set up file path: e.g. /mnt/sdcard/session0001/actuator_outputs0.bin */ + // sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_outputs0"); + // if (0 == (actuator_outputs_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { + // errx(1, "opening %s failed.\n", path_buf); + // } + + /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ + sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_controls0"); + if (0 == (actuator_controls_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { errx(1, "opening %s failed.\n", path_buf); } - int actuator_outputs_file_no = actuator_outputs_file; /* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */ sprintf(path_buf, "%s/%s.txt", folder_path, "gps"); @@ -249,7 +256,7 @@ int sdlog_thread_main(int argc, char *argv[]) { struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; struct actuator_outputs_s act_outputs; - struct actuator_controls_s actuators; + struct actuator_controls_s act_controls; struct vehicle_command_s cmd; } buf; @@ -259,7 +266,7 @@ int sdlog_thread_main(int argc, char *argv[]) { int att_sub; int spa_sub; int act_0_sub; - int actuators_sub; + int controls0_sub; } subs; /* --- MANAGEMENT - LOGGING COMMAND --- */ @@ -299,8 +306,8 @@ int sdlog_thread_main(int argc, char *argv[]) { /* --- ACTUATOR CONTROL VALUE --- */ /* subscribe to ORB for actuator control */ - subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - fds[fdsc_count].fd = subs.actuators_sub; + subs.controls0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + fds[fdsc_count].fd = subs.controls0_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -338,9 +345,10 @@ int sdlog_thread_main(int argc, char *argv[]) { int ifds = 0; - if (poll_count % 2000 == 0) { + if (poll_count % 5000 == 0) { fsync(sensorfile); - fsync(actuator_outputs_file_no); + fsync(actuator_outputs_file); + fsync(actuator_controls_file); fsync(blackbox_file_no); } poll_count++; @@ -349,7 +357,7 @@ int sdlog_thread_main(int argc, char *argv[]) { if (fds[ifds++].revents & POLLIN) { /* copy command into local buffer */ orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); - blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f-%f-%f-%f-%f-%f-%f]\n", hrt_absolute_time()/1000000.0d, + blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d, buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4, (double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7); } @@ -360,7 +368,7 @@ int sdlog_thread_main(int argc, char *argv[]) { /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); /* write out */ - sensor_combined_bytes += write(sensorfile, (const char*)&buf.raw, sizeof(buf.raw)); + sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw)); } /* --- ATTITUDE VALUE --- */ @@ -384,18 +392,19 @@ int sdlog_thread_main(int argc, char *argv[]) { /* copy actuator data into local buffer */ orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); /* write out */ - actuator_outputs_bytes += write(actuator_outputs_file, (const char*)&buf.raw, sizeof(buf.raw)); + // actuator_outputs_bytes += write(actuator_outputs_file, (const char*)&buf.act_outputs, sizeof(buf.act_outputs)); } /* --- ACTUATOR CONTROL --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.actuators_sub, &buf.actuators); - + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls0_sub, &buf.act_controls); + /* write out */ + actuator_controls_bytes += write(actuator_controls_file, (const char*)&buf.act_controls, sizeof(buf.act_controls)); } } } - unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes; + 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; @@ -404,7 +413,8 @@ int sdlog_thread_main(int argc, char *argv[]) { printf("[sdlog] exiting.\n"); close(sensorfile); - close(gpsfile); + close(actuator_outputs_file); + close(actuator_controls_file); fclose(gpsfile); fclose(blackbox_file); diff --git a/apps/uORB/topics/actuator_controls.h b/apps/uORB/topics/actuator_controls.h index 8d4972bae..0b50a29ac 100644 --- a/apps/uORB/topics/actuator_controls.h +++ b/apps/uORB/topics/actuator_controls.h @@ -53,6 +53,7 @@ #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ struct actuator_controls_s { + uint64_t timestamp; float control[NUM_ACTUATOR_CONTROLS]; }; diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h index 202f72b54..accd560af 100644 --- a/apps/uORB/topics/actuator_outputs.h +++ b/apps/uORB/topics/actuator_outputs.h @@ -53,6 +53,7 @@ #define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ struct actuator_outputs_s { + uint64_t timestamp; float output[NUM_ACTUATOR_OUTPUTS]; }; |