Skip to content

Commit

Permalink
Merge pull request #438 from rosflight/432-new-mixing-system
Browse files Browse the repository at this point in the history 
432 new mixing system
  • Loading branch information
@JMoore5353
JMoore5353 authored Jan 22, 2025
2 parents d16f4ac + 6f2f889 commit 34a0540
Show file tree
Hide file tree
Showing 25 changed files with 1,502 additions and 434 deletions.
2 changes: 1 addition & 1 deletion .github/workflows/unit_tests.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ jobs:
- name: clone
run: git submodule update --init --recursive
- name: apt install
run: sudo apt-get install -y build-essential cmake libgtest-dev libeigen3-dev
run: sudo apt-get install -y build-essential cmake libgtest-dev
- name: install gtest
run: |
cd /usr/src/gtest
Expand Down
3 changes: 3 additions & 0 deletions .gitmodules
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,3 +4,6 @@
[submodule "boards/varmint_h7"]
path = boards/varmint_h7
url = https://github.com/rosflight/varmint_h7.git
[submodule "lib/eigen"]
path = lib/eigen
url = https://gitlab.com/libeigen/eigen.git
2 changes: 0 additions & 2 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,15 +29,13 @@ if("${GIT_VERSION_HASH}" STREQUAL "")
endif()

### source files ###

include_directories(
include
include/interface
lib
comms/mavlink
comms/mavlink/v1.0
comms/mavlink/v1.0/common
comms/mavlink/v1.0/rosflight
)

file(GLOB_RECURSE ROSFLIGHT_SOURCES
Expand Down
22 changes: 13 additions & 9 deletions comms/mavlink/mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -487,15 +487,19 @@ void Mavlink::handle_msg_offboard_control(const mavlink_message_t * const msg)
return;
}

control.x.value = ctrl.x;
control.y.value = ctrl.y;
control.z.value = ctrl.z;
control.F.value = ctrl.F;

control.x.valid = !(ctrl.ignore & IGNORE_VALUE1);
control.y.valid = !(ctrl.ignore & IGNORE_VALUE2);
control.z.valid = !(ctrl.ignore & IGNORE_VALUE3);
control.F.valid = !(ctrl.ignore & IGNORE_VALUE4);
control.Qx.value = ctrl.Qx;
control.Qy.value = ctrl.Qy;
control.Qz.value = ctrl.Qz;
control.Fx.value = ctrl.Fx;
control.Fy.value = ctrl.Fy;
control.Fz.value = ctrl.Fz;

control.Qx.valid = !(ctrl.ignore & IGNORE_VALUE1);
control.Qy.valid = !(ctrl.ignore & IGNORE_VALUE2);
control.Qz.valid = !(ctrl.ignore & IGNORE_VALUE3);
control.Fx.valid = !(ctrl.ignore & IGNORE_VALUE4);
control.Fy.valid = !(ctrl.ignore & IGNORE_VALUE5);
control.Fz.valid = !(ctrl.ignore & IGNORE_VALUE6);

if (listener_ != nullptr) { listener_->offboard_control_callback(control); }
}
Expand Down
2 changes: 1 addition & 1 deletion comms/mavlink/v1.0
Submodule v1.0 updated 7 files
+1 −1 common/version.h
+145 −97 rosflight/mavlink_msg_named_command_struct.h
+139 −91 rosflight/mavlink_msg_offboard_control.h
+3 −3 rosflight/mavlink_msg_rosflight_version.h
+6 −4 rosflight/rosflight.h
+20 −16 rosflight/testsuite.h
+1 −1 rosflight/version.h
56 changes: 41 additions & 15 deletions include/command_manager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ typedef enum
{
RATE, // Channel is is in rate mode (mrad/s)
ANGLE, // Channel command is in angle mode (mrad)
THROTTLE, // Channel is direcly controlling throttle max/1000
THROTTLE, // Channel is controlling throttle setting, which will be converted to force
PASSTHROUGH, // Channel directly passes PWM input to the mixer
} control_type_t;

Expand All @@ -58,13 +58,22 @@ typedef struct
float value; // The value of the channel
} control_channel_t;

typedef enum
{
X_AXIS,
Y_AXIS,
Z_AXIS,
} rc_f_axis_t;

typedef struct
{
uint32_t stamp_ms;
control_channel_t x;
control_channel_t y;
control_channel_t z;
control_channel_t F;
control_channel_t Qx;
control_channel_t Qy;
control_channel_t Qz;
control_channel_t Fx;
control_channel_t Fy;
control_channel_t Fz;
} control_t;

class CommandManager : public ParamListenerInterface
Expand All @@ -77,38 +86,50 @@ class CommandManager : public ParamListenerInterface
control_channel_t * combined;
} mux_t;

mux_t muxes[4] = {{&rc_command_.x, &offboard_command_.x, &combined_command_.x},
{&rc_command_.y, &offboard_command_.y, &combined_command_.y},
{&rc_command_.z, &offboard_command_.z, &combined_command_.z},
{&rc_command_.F, &offboard_command_.F, &combined_command_.F}};
mux_t muxes[6] = {{&rc_command_.Qx, &offboard_command_.Qx, &combined_command_.Qx},
{&rc_command_.Qy, &offboard_command_.Qy, &combined_command_.Qy},
{&rc_command_.Qz, &offboard_command_.Qz, &combined_command_.Qz},
{&rc_command_.Fx, &offboard_command_.Fx, &combined_command_.Fx},
{&rc_command_.Fy, &offboard_command_.Fy, &combined_command_.Fy},
{&rc_command_.Fz, &offboard_command_.Fz, &combined_command_.Fz}};

// clang-format off
control_t rc_command_ = {0,
{false, ANGLE, 0.0},
{false, ANGLE, 0.0},
{false, RATE, 0.0},
{false, THROTTLE, 0.0},
{false, THROTTLE, 0.0},
{false, THROTTLE, 0.0}};
control_t offboard_command_ = {0,
{false, ANGLE, 0.0},
{false, ANGLE, 0.0},
{false, RATE, 0.0},
{false, THROTTLE, 0.0},
{false, THROTTLE, 0.0},
{false, THROTTLE, 0.0}};
control_t combined_command_ = {0,
{false, ANGLE, 0.0},
{false, ANGLE, 0.0},
{false, RATE, 0.0},
{false, THROTTLE, 0.0},
{false, THROTTLE, 0.0},
{false, THROTTLE, 0.0}};

control_t multirotor_failsafe_command_ = {0,
{true, ANGLE, 0.0},
{true, ANGLE, 0.0},
{true, RATE, 0.0},
{true, THROTTLE, 0.0},
{true, THROTTLE, 0.0},
{true, THROTTLE, 0.0}};
control_t fixedwing_failsafe_command_ = {0,
{true, PASSTHROUGH, 0.0},
{true, PASSTHROUGH, 0.0},
{true, PASSTHROUGH, 0.0},
{true, THROTTLE, 0.0}};
{true, PASSTHROUGH, 0.0},
{true, PASSTHROUGH, 0.0},
{true, PASSTHROUGH, 0.0}};
// clang-format on

typedef enum
Expand All @@ -119,10 +140,12 @@ class CommandManager : public ParamListenerInterface

enum MuxChannel
{
MUX_X,
MUX_Y,
MUX_Z,
MUX_F,
MUX_QX,
MUX_QY,
MUX_QZ,
MUX_FX,
MUX_FY,
MUX_FZ,
};

typedef struct
Expand All @@ -138,7 +161,8 @@ class CommandManager : public ParamListenerInterface
ROSflight & RF_;

bool new_command_;
bool rc_override_;
bool rc_throttle_override_;
bool rc_attitude_override_;

control_t & failsafe_command_;

Expand All @@ -157,6 +181,8 @@ class CommandManager : public ParamListenerInterface
void init();
bool run();
bool rc_override_active();
bool rc_throttle_override_active();
bool rc_attitude_override_active();
bool offboard_control_active();
void set_new_offboard_command(control_t new_offboard_command);
void set_new_rc_command(control_t new_rc_command);
Expand Down
19 changes: 13 additions & 6 deletions include/controller.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,21 +51,26 @@ class Controller : public ParamListenerInterface
public:
struct Output
{
float F;
float x;
float y;
float z;
float Fx;
float Fy;
float Fz;
float Qx;
float Qy;
float Qz;
};

Controller(ROSflight & rf);

inline const Output & output() const { return output_; }
inline float max_thrust() const { return max_thrust_; }

void init();
void run();

void calculate_max_thrust();
void calculate_equilbrium_torque_from_rc();
void param_change_callback(uint16_t param_id) override;
bool is_throttle_high(float threshold);

private:
class PID
Expand All @@ -92,8 +97,8 @@ class Controller : public ParamListenerInterface

ROSflight & RF_;

turbomath::Vector run_pid_loops(uint32_t dt, const Estimator::State & state,
const control_t & command, bool update_integrators);
Controller::Output run_pid_loops(uint32_t dt, const Estimator::State & state,
const control_t & command, bool update_integrators);

Output output_;

Expand All @@ -103,6 +108,8 @@ class Controller : public ParamListenerInterface
PID pitch_rate_;
PID yaw_rate_;

float max_thrust_;

uint64_t prev_time_us_;
};

Expand Down
10 changes: 6 additions & 4 deletions include/interface/comm_link.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,10 +84,12 @@ class CommLinkInterface
};

Mode mode;
Channel x;
Channel y;
Channel z;
Channel F;
Channel Qx;
Channel Qy;
Channel Qz;
Channel Fx;
Channel Fy;
Channel Fz;
};

struct AuxCommand
Expand Down
Loading

0 comments on commit 34a0540

Please sign in to comment.