aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/commander/commander.cpp2
-rw-r--r--src/modules/dataman/dataman.c19
-rw-r--r--src/modules/dataman/dataman.h2
-rw-r--r--src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp2
-rw-r--r--src/modules/mavlink/mavlink_main.cpp6
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp49
-rw-r--r--src/modules/mavlink/module.mk2
-rw-r--r--src/modules/sdlog2/sdlog2.c18
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h51
9 files changed, 105 insertions, 46 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c2d24a883..f1c245991 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -969,7 +969,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
- if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
+ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 34d20e485..c132b0040 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -44,7 +44,9 @@
#include <stdlib.h>
#include <fcntl.h>
#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
#include <queue.h>
+#include <string.h>
#include "dataman.h"
@@ -594,6 +596,20 @@ task_main(int argc, char *argv[])
sem_init(&g_work_queued_sema, 1, 0);
+ /* See if the data manage file exists and is a multiple of the sector size */
+ g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
+ if (g_task_fd >= 0) {
+ /* File exists, check its size */
+ int file_size = lseek(g_task_fd, 0, SEEK_END);
+ if ((file_size % k_sector_size) != 0) {
+ warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path);
+ close(g_task_fd);
+ unlink(k_data_manager_device_path);
+ }
+ else
+ close(g_task_fd);
+ }
+
/* Open or create the data manager file */
g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
@@ -603,7 +619,7 @@ task_main(int argc, char *argv[])
return -1;
}
- if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
+ if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
close(g_task_fd);
warnx("Could not seek data manager file %s", k_data_manager_device_path);
sem_post(&g_init_sema); /* Don't want to hang startup */
@@ -776,4 +792,3 @@ dataman_main(int argc, char *argv[])
exit(1);
}
-
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index a70638ccc..33c9fcd15 100644
--- a/src/modules/dataman/dataman.h
+++ b/src/modules/dataman/dataman.h
@@ -79,7 +79,7 @@ extern "C" {
} dm_reset_reason;
/* Maximum size in bytes of a single item instance */
- #define DM_MAX_DATA_SIZE 126
+ #define DM_MAX_DATA_SIZE 124
/* Retrieve from the data manager store */
__EXPORT ssize_t
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index 32a6029fe..739f6fb1f 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -964,7 +964,7 @@ FixedwingEstimator::task_main()
}
// Publish results
- if (_initialized) {
+ if (_initialized && (check == OK)) {
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c60c85495..30d400f31 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -574,6 +574,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
/* open uart */
_uart_fd = open(uart_name, O_RDWR | O_NOCTTY);
+ if (_uart_fd < 0) {
+ return _uart_fd;
+ }
+
+
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
@@ -1964,6 +1969,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
+ configure_stream("DISTANCE_SENSOR", 0.5f);
break;
case MAVLINK_MODE_CAMERA:
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 3cf69bf7c..27b1af046 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -72,6 +72,7 @@
#include <uORB/topics/navigation_capabilities.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_range_finder.h>
#include "mavlink_messages.h"
@@ -271,7 +272,7 @@ protected:
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
- status->battery_remaining,
+ status->battery_remaining * 100.0f,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
@@ -1313,6 +1314,51 @@ protected:
}
};
+class MavlinkStreamDistanceSensor : public MavlinkStream
+{
+public:
+ const char *get_name()
+ {
+ return "DISTANCE_SENSOR";
+ }
+
+ MavlinkStream *new_instance()
+ {
+ return new MavlinkStreamDistanceSensor();
+ }
+
+private:
+ MavlinkOrbSubscription *range_sub;
+ struct range_finder_report *range;
+
+protected:
+ void subscribe(Mavlink *mavlink)
+ {
+ range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
+ range = (struct range_finder_report *)range_sub->get_data();
+ }
+
+ void send(const hrt_abstime t)
+ {
+ (void)range_sub->update(t);
+
+ uint8_t type;
+
+ switch (range->type) {
+ case RANGE_FINDER_TYPE_LASER:
+ type = MAV_DISTANCE_SENSOR_LASER;
+ break;
+ }
+
+ uint8_t id = 0;
+ uint8_t orientation = 0;
+ uint8_t covariance = 20;
+
+ mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
+ range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
+ }
+};
+
MavlinkStream *streams_list[] = {
new MavlinkStreamHeartbeat(),
new MavlinkStreamSysStatus(),
@@ -1339,6 +1385,7 @@ MavlinkStream *streams_list[] = {
new MavlinkStreamAttitudeControls(),
new MavlinkStreamNamedValueFloat(),
new MavlinkStreamCameraCapture(),
+ new MavlinkStreamDistanceSensor(),
new MavlinkStreamViconPositionEstimate(),
nullptr
};
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index c44354ff0..dcca11977 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -47,4 +47,4 @@ SRCS += mavlink_main.cpp \
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
-MAXOPTIMIZATION = -Os
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index cba39c5b1..543f15093 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -908,9 +908,6 @@ int sdlog2_thread_main(int argc, char *argv[])
hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0;
- /* track changes in distance status */
- bool dist_bottom_present = false;
-
/* enable logging on start if needed */
if (log_on_start) {
/* check GPS topic to get GPS time */
@@ -1099,6 +1096,8 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.x = buf.local_pos.x;
log_msg.body.log_LPOS.y = buf.local_pos.y;
log_msg.body.log_LPOS.z = buf.local_pos.z;
+ log_msg.body.log_LPOS.ground_dist = buf.local_pos.dist_bottom;
+ log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate;
log_msg.body.log_LPOS.vx = buf.local_pos.vx;
log_msg.body.log_LPOS.vy = buf.local_pos.vy;
log_msg.body.log_LPOS.vz = buf.local_pos.vz;
@@ -1108,19 +1107,8 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
log_msg.body.log_LPOS.landed = buf.local_pos.landed;
+ log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
LOGBUFFER_WRITE_AND_COUNT(LPOS);
-
- if (buf.local_pos.dist_bottom_valid) {
- dist_bottom_present = true;
- }
-
- if (dist_bottom_present) {
- log_msg.msg_type = LOG_DIST_MSG;
- log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom;
- log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate;
- log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
- LOGBUFFER_WRITE_AND_COUNT(DIST);
- }
}
/* --- LOCAL POSITION SETPOINT --- */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index e38681a39..63386a5d7 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -101,6 +101,8 @@ struct log_LPOS_s {
float x;
float y;
float z;
+ float ground_dist;
+ float ground_dist_rate;
float vx;
float vy;
float vz;
@@ -110,6 +112,7 @@ struct log_LPOS_s {
uint8_t xy_flags;
uint8_t z_flags;
uint8_t landed;
+ uint8_t ground_dist_flags;
};
/* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -340,30 +343,30 @@ struct log_PARM_s {
/* construct list of all message formats */
static const struct log_format_s log_formats[] = {
/* business-level messages, ID < 0x80 */
- LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
- LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
- LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
- LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
- LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
- LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
- LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
- LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
- LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
- LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
- LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
- LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
- LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
- LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
- LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
- LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
- LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
- LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
- LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
- LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
- LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
- LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
- LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
- LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
+ LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
+ LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
+ LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
+ LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
+ LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"),
+ LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
+ LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
+ LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
+ LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
+ LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
+ LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
+ LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
+ LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
+ LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
+ LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
+ LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
+ LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+ LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
+ LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
+ LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
+ LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
+ LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
+ LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
+ LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
/* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-17 21:52:37 +0000