From 2e3b696896a287ef24d2b3fc072f3dac347a1fc0 Mon Sep 17 00:00:00 2001 From: Phil Tokumaru Date: Thu, 14 Nov 2024 13:36:08 -0800 Subject: [PATCH] Some fixes after testing the code --- boards/varmint_h7 | 2 +- comms/mavlink/mavlink.cpp | 4 +++- include/board.h | 1 - include/mixer.h | 10 +++++++--- include/sensors.h | 6 ++---- src/comm_manager.cpp | 4 ++-- src/mixer.cpp | 2 -- 7 files changed, 15 insertions(+), 14 deletions(-) diff --git a/boards/varmint_h7 b/boards/varmint_h7 index 70f37f7c..6a7754a9 160000 --- a/boards/varmint_h7 +++ b/boards/varmint_h7 @@ -1 +1 @@ -Subproject commit 70f37f7c356954a5a452e19633c95fc053a63864 +Subproject commit 6a7754a9a42eba8a6f9b2b16b575243db49cc592 diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index 707bd93e..d48756d7 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -269,8 +269,10 @@ void Mavlink::send_command_ack(uint8_t system_id, uint64_t timestamp_us, void Mavlink::send_diff_pressure(uint8_t system_id, const PressureStruct & p) { + + float ias = (p.pressure<0.0?-1.0:1.0)*sqrt(fabs(p.pressure) / (0.5 * 1.225)); mavlink_message_t msg; - mavlink_msg_diff_pressure_pack(system_id, compid_, &msg, sqrt(p.pressure / (0.5 * 1.225)), + mavlink_msg_diff_pressure_pack(system_id, compid_, &msg, ias, p.pressure, p.temperature); send_message(msg); } diff --git a/include/board.h b/include/board.h index 31ed4366..cf640fc0 100644 --- a/include/board.h +++ b/include/board.h @@ -77,7 +77,6 @@ class Board // IMU virtual bool imu_read(rosflight_firmware::ImuStruct * imu) = 0; - virtual void imu_not_responding_error() = 0; // Mag virtual bool mag_read(rosflight_firmware::MagStruct * mag) = 0; diff --git a/include/mixer.h b/include/mixer.h index 88d9ef45..89448cb9 100644 --- a/include/mixer.h +++ b/include/mixer.h @@ -37,6 +37,8 @@ #include #include +#include + namespace rosflight_firmware { class ROSflight; @@ -47,7 +49,6 @@ class Mixer : public ParamListenerInterface public: static constexpr uint8_t NUM_TOTAL_OUTPUTS = 14; static constexpr uint8_t NUM_MIXER_OUTPUTS = 10; - float * raw_outputs(void); enum { @@ -102,7 +103,9 @@ class Mixer : public ParamListenerInterface private: ROSflight & RF_; - float raw_outputs_[NUM_TOTAL_OUTPUTS]; + RcStruct output_raw_ = {}; + //float raw_outputs_[NUM_TOTAL_OUTPUTS]; + float *raw_outputs_ = output_raw_.chan; float outputs_[NUM_TOTAL_OUTPUTS]; aux_command_t aux_command_; output_type_t combined_output_type_[NUM_TOTAL_OUTPUTS]; @@ -258,7 +261,8 @@ class Mixer : public ParamListenerInterface void mix_output(); void param_change_callback(uint16_t param_id) override; void set_new_aux_command(aux_command_t new_aux_command); - inline const float * get_outputs() const { return raw_outputs_; } + float * raw_outputs() { return raw_outputs_; } + RcStruct * get_output_raw(void) { return &output_raw_; } }; } // namespace rosflight_firmware diff --git a/include/sensors.h b/include/sensors.h index 661d508c..b448bbed 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -100,8 +100,8 @@ typedef struct //__attribute__((packed)) float temperature; // K } MagStruct; -#define RC_STRUCT_CHANNELS \ - 24 // 16 analog + 8 digital MUST BE > 14 (Mavlink message size is hardware to 14) +// 16 analog + 8 digital MUST BE > 14 (Mavlink message size is hardware to 14) +#define RC_STRUCT_CHANNELS 24 typedef struct //__attribute__((packed)) { uint64_t timestamp; // us, time of data read complete @@ -194,7 +194,6 @@ class Sensors : public ParamListenerInterface RangeStruct * get_sonar(void) { return &sonar_; } ImuStruct * get_imu(void) { return &imu_; } BatteryStruct * get_battery(void) { return &battery_; } - RcStruct * get_output_raw(void) { return &output_raw_; } RcStruct * get_rc_(void) { return &rc_; } MagStruct * get_mag(void) { return &mag_; } GnssStruct * get_gnss(void) { return &gnss_; } @@ -221,7 +220,6 @@ class Sensors : public ParamListenerInterface RangeStruct sonar_ = {}; ImuStruct imu_ = {}; BatteryStruct battery_ = {}; - RcStruct output_raw_ = {}; RcStruct rc_ = {}; MagStruct mag_ = {}; GnssStruct gnss_ = {}; diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 4b570204..555a064d 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -180,7 +180,7 @@ void CommManager::send_imu(void) { comm_link_.send_imu(sysid_, *RF_.sensors_.get void CommManager::send_output_raw(void) { - comm_link_.send_output_raw(sysid_, *RF_.sensors_.get_output_raw()); + comm_link_.send_output_raw(sysid_, *RF_.mixer_.get_output_raw()); } void CommManager::send_rc_raw(void) { comm_link_.send_rc_raw(sysid_, *RF_.sensors_.get_rc_()); } @@ -192,7 +192,7 @@ void CommManager::send_diff_pressure(void) void CommManager::send_baro(void) { - comm_link_.send_diff_pressure(sysid_, *RF_.sensors_.get_baro()); + comm_link_.send_baro(sysid_, *RF_.sensors_.get_baro()); } void CommManager::send_sonar(void) { comm_link_.send_sonar(sysid_, *RF_.sensors_.get_sonar()); } diff --git a/src/mixer.cpp b/src/mixer.cpp index 40280bd4..c58f0804 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -45,8 +45,6 @@ Mixer::Mixer(ROSflight & _rf) void Mixer::init() { init_mixing(); } -float * Mixer::raw_outputs(void) { return raw_outputs_; } - void Mixer::param_change_callback(uint16_t param_id) { switch (param_id) {