aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/modules/att_pos_estimator_ekf/KalmanNav.cpp12
-rw-r--r--src/modules/sdlog2/sdlog2.c31
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h34
-rw-r--r--src/modules/uORB/topics/vehicle_global_position_setpoint.h2
4 files changed, 63 insertions, 16 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 97d7fdd75..191d20f30 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -661,10 +661,10 @@ int KalmanNav::correctPos()
Vector y(6);
y(0) = _gps.vel_n_m_s - vN;
y(1) = _gps.vel_e_m_s - vE;
- y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
- y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
- y(4) = double(_gps.alt) / 1.0e3 - alt;
- y(5) = double(_sensors.baro_alt_meter) - alt;
+ y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
+ y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG;
+ y(4) = _gps.alt / 1.0e3f - alt;
+ y(5) = _sensors.baro_alt_meter - alt;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -698,7 +698,7 @@ int KalmanNav::correctPos()
vD += xCorrect(VD);
lat += double(xCorrect(LAT));
lon += double(xCorrect(LON));
- alt += double(xCorrect(ALT));
+ alt += xCorrect(ALT);
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -710,7 +710,7 @@ int KalmanNav::correctPos()
static int counter = 0;
if (beta > _faultPos.get() && (counter % 10 == 0)) {
warnx("fault in gps: beta = %8.4f", (double)beta);
- warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
+ warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f",
double(y(0) / sqrtf(RPos(0, 0))),
double(y(1) / sqrtf(RPos(1, 1))),
double(y(2) / sqrtf(RPos(2, 2))),
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 1eb2c7b98..ec1b7628d 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -72,6 +72,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/optical_flow.h>
@@ -615,7 +616,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
- const ssize_t fdsc = 18;
+ const ssize_t fdsc = 19;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
@@ -637,6 +638,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
+ struct vehicle_global_position_setpoint_s global_pos_sp;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;
@@ -660,6 +662,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int local_pos_sub;
int local_pos_sp_sub;
int global_pos_sub;
+ int global_pos_sp_sub;
int gps_pos_sub;
int vicon_pos_sub;
int flow_sub;
@@ -689,6 +692,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_ARSP_s log_ARSP;
struct log_FLOW_s log_FLOW;
struct log_GPOS_s log_GPOS;
+ struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
} body;
} log_msg = {
@@ -775,6 +779,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
+ /* --- GLOBAL POSITION SETPOINT--- */
+ subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
+ fds[fdsc_count].fd = subs.global_pos_sp_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
/* --- VICON POSITION --- */
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
fds[fdsc_count].fd = subs.vicon_pos_sub;
@@ -1077,6 +1087,25 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}
+ /* --- GLOBAL POSITION SETPOINT --- */
+ if (fds[ifds++].revents & POLLIN) {
+ orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
+ log_msg.msg_type = LOG_GPSP_MSG;
+ log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
+ log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
+ log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
+ log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
+ log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
+ log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
+ log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
+ log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
+ log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
+ log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
+ log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
+ log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
+ LOGBUFFER_WRITE_AND_COUNT(GPSP);
+ }
+
/* --- VICON POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index d9f1cd634..0e479b524 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -149,15 +149,15 @@ struct log_ATTC_s {
/* --- STAT - VEHICLE STATE --- */
#define LOG_STAT_MSG 10
struct log_STAT_s {
- unsigned char state;
- unsigned char flight_mode;
- unsigned char manual_control_mode;
- unsigned char manual_sas_mode;
- unsigned char armed;
+ uint8_t state;
+ uint8_t flight_mode;
+ uint8_t manual_control_mode;
+ uint8_t manual_sas_mode;
+ uint8_t armed;
float battery_voltage;
float battery_current;
float battery_remaining;
- unsigned char battery_warning;
+ uint8_t battery_warning;
};
/* --- RC - RC INPUT CHANNELS --- */
@@ -210,13 +210,29 @@ struct log_GPOS_s {
float vel_d;
};
+/* --- GPSP - GLOBAL POSITION SETPOINT --- */
+#define LOG_GPSP_MSG 17
+struct log_GPSP_s {
+ uint8_t altitude_is_relative;
+ int32_t lat;
+ int32_t lon;
+ float altitude;
+ float yaw;
+ float loiter_radius;
+ int8_t loiter_direction;
+ uint8_t nav_cmd;
+ float param1;
+ float param2;
+ float param3;
+ float param4;
+};
+
/* --- ESC - ESC STATE --- */
-#define LOG_ESC_MSG 64
+#define LOG_ESC_MSG 18
struct log_ESC_s {
uint16_t counter;
uint8_t esc_count;
uint8_t esc_connectiontype;
-
uint8_t esc_num;
uint16_t esc_address;
uint16_t esc_version;
@@ -227,6 +243,7 @@ struct log_ESC_s {
float esc_setpoint;
uint16_t esc_setpoint_raw;
};
+
#pragma pack(pop)
/* construct list of all message formats */
@@ -248,6 +265,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
+ LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
};
diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
index 3ae3ff28c..5c8ce1e4d 100644
--- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
@@ -66,7 +66,7 @@ struct vehicle_global_position_setpoint_s
float altitude; /**< altitude in meters */
float yaw; /**< in radians NED -PI..+PI */
float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */
- uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */
+ int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */
enum NAV_CMD nav_cmd; /**< true if loitering is enabled */
float param1;
float param2;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-13 00:31:26 +0000