aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/fw_att_pos_estimator/estimator.cpp17
-rw-r--r--src/modules/fw_att_pos_estimator/estimator.h30
-rw-r--r--src/modules/sdlog2/sdlog2.c27
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h50
-rw-r--r--src/modules/uORB/objects_common.cpp3
-rw-r--r--src/modules/uORB/topics/system_power.h71
6 files changed, 160 insertions, 38 deletions
diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp
index 7ab06e85d..c31217393 100644
--- a/src/modules/fw_att_pos_estimator/estimator.cpp
+++ b/src/modules/fw_att_pos_estimator/estimator.cpp
@@ -106,7 +106,22 @@ void swap_var(float &d1, float &d2)
d2 = tmp;
}
-AttPosEKF::AttPosEKF()
+AttPosEKF::AttPosEKF() :
+ fusionModeGPS(0),
+ covSkipCount(0),
+ EAS2TAS(1.0f),
+ statesInitialised(false),
+ fuseVelData(false),
+ fusePosData(false),
+ fuseHgtData(false),
+ fuseMagData(false),
+ fuseVtasData(false),
+ onGround(true),
+ staticMode(true),
+ useAirspeed(true),
+ useCompass(true),
+ numericalProtection(true),
+ storeIndex(0)
{
}
diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h
index 7edb3c714..e62f1a98a 100644
--- a/src/modules/fw_att_pos_estimator/estimator.h
+++ b/src/modules/fw_att_pos_estimator/estimator.h
@@ -108,7 +108,7 @@ public:
Vector3f dVelIMU;
Vector3f dAngIMU;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
- uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
+ uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
float innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output
@@ -127,8 +127,8 @@ public:
float lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
- uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
- float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed
+ uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
+ float EAS2TAS; // ratio f true to equivalent airspeed
// GPS input data variables
float gpsCourse;
@@ -141,25 +141,25 @@ public:
// Baro input
float baroHgt;
- bool statesInitialised = false;
+ bool statesInitialised;
- bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused
- bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused
- bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused
- bool fuseMagData = false; // boolean true when magnetometer data is to be fused
- bool fuseVtasData = false; // boolean true when airspeed data is to be fused
+ bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
+ bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
+ bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
+ bool fuseMagData; // boolean true when magnetometer data is to be fused
+ bool fuseVtasData; // boolean true when airspeed data is to be fused
- bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying)
- bool staticMode = true; ///< boolean true if no position feedback is fused
- bool useAirspeed = true; ///< boolean true if airspeed data is being used
- bool useCompass = true; ///< boolean true if magnetometer data is being used
+ bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
+ bool staticMode; ///< boolean true if no position feedback is fused
+ bool useAirspeed; ///< boolean true if airspeed data is being used
+ bool useCompass; ///< boolean true if magnetometer data is being used
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
- bool numericalProtection = true;
+ bool numericalProtection;
- unsigned storeIndex = 0;
+ unsigned storeIndex;
void UpdateStrapdownEquationsNED();
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 13981ac54..d2cf6d662 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -84,6 +84,8 @@
#include <uORB/topics/esc_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/system_power.h>
+#include <uORB/topics/servorail_status.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@@ -796,6 +798,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct telemetry_status_s telemetry;
struct range_finder_report range_finder;
struct estimator_status_report estimator_status;
+ struct system_power_s system_power;
+ struct servorail_status_s servorail_status;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -828,6 +832,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_DIST_s log_DIST;
struct log_TELE_s log_TELE;
struct log_ESTM_s log_ESTM;
+ struct log_PWR_s log_PWR;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -859,6 +864,8 @@ int sdlog2_thread_main(int argc, char *argv[])
int telemetry_sub;
int range_finder_sub;
int estimator_status_sub;
+ int system_power_sub;
+ int servorail_status_sub;
} subs;
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -884,6 +891,8 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
+ subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
+ subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
thread_running = true;
@@ -1226,6 +1235,24 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
+ /* --- SYSTEM POWER RAILS --- */
+ if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
+ log_msg.msg_type = LOG_PWR_MSG;
+ log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
+ log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
+ log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid;
+ log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid;
+ log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC;
+ log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC;
+
+ /* copy servo rail status topic here too */
+ orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status);
+ log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v;
+ log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v;
+
+ LOGBUFFER_WRITE_AND_COUNT(PWR);
+ }
+
/* --- TELEMETRY --- */
if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
log_msg.msg_type = LOG_TELE_MSG;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 22a695872..354bb08e8 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -277,6 +277,29 @@ struct log_TELE_s {
uint8_t txbuf;
};
+/* --- ESTM - ESTIMATOR STATUS --- */
+#define LOG_ESTM_MSG 23
+struct log_ESTM_s {
+ float s[10];
+ uint8_t n_states;
+ uint8_t states_nan;
+ uint8_t covariance_nan;
+ uint8_t kalman_gain_nan;
+};
+
+/* --- PWR - ONBOARD POWER SYSTEM --- */
+#define LOG_PWR_MSG 24
+struct log_PWR_s {
+ float peripherals_5v;
+ float servo_rail_5v;
+ float servo_rssi;
+ uint8_t usb_ok;
+ uint8_t brick_ok;
+ uint8_t servo_ok;
+ uint8_t low_power_rail_overcurrent;
+ uint8_t high_power_rail_overcurrent;
+};
+
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@@ -299,23 +322,6 @@ struct log_PARM_s {
float value;
};
-/* --- ESTM - ESTIMATOR STATUS --- */
-#define LOG_ESTM_MSG 132
-struct log_ESTM_s {
- float s[10];
- uint8_t n_states;
- uint8_t states_nan;
- uint8_t covariance_nan;
- uint8_t kalman_gain_nan;
-};
-// struct log_ESTM_s {
-// float s[32];
-// uint8_t n_states;
-// uint8_t states_nan;
-// uint8_t covariance_nan;
-// uint8_t kalman_gain_nan;
-// };
-
#pragma pack(pop)
/* construct list of all message formats */
@@ -342,16 +348,16 @@ static const struct log_format_s log_formats[] = {
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"),
/* system-level messages, ID >= 0x80 */
- // FMT: don't write format of format message, it's useless
+ /* FMT: don't write format of format message, it's useless */
LOG_FORMAT(TIME, "Q", "StartTime"),
LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
- LOG_FORMAT(PARM, "Nf", "Name,Value"),
- LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
- //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"),
+ LOG_FORMAT(PARM, "Nf", "Name,Value")
};
-static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
+static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]);
#endif /* SDLOG2_MESSAGES_H_ */
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 4b31cc8a4..c8a589bb7 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s);
#include "topics/servorail_status.h"
ORB_DEFINE(servorail_status, struct servorail_status_s);
+#include "topics/system_power.h"
+ORB_DEFINE(system_power, struct system_power_s);
+
#include "topics/vehicle_global_position.h"
ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
diff --git a/src/modules/uORB/topics/system_power.h b/src/modules/uORB/topics/system_power.h
new file mode 100644
index 000000000..7763b6004
--- /dev/null
+++ b/src/modules/uORB/topics/system_power.h
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file system_power.h
+ *
+ * Definition of the system_power voltage and power status uORB topic.
+ */
+
+#ifndef SYSTEM_POWER_H_
+#define SYSTEM_POWER_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * voltage and power supply status
+ */
+struct system_power_s {
+ uint64_t timestamp; /**< microseconds since system boot */
+ float voltage5V_v; /**< peripheral 5V rail voltage */
+ uint8_t usb_connected:1; /**< USB is connected when 1 */
+ uint8_t brick_valid:1; /**< brick power is good when 1 */
+ uint8_t servo_valid:1; /**< servo power is good when 1 */
+ uint8_t periph_5V_OC:1; /**< peripheral overcurrent when 1 */
+ uint8_t hipower_5V_OC:1; /**< hi power peripheral overcurrent when 1 */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(system_power);
+
+#endif
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-06 17:37:36 +0000