diff --git a/AirLib/include/controllers/simple_flight/firmware/Mixer.hpp b/AirLib/include/controllers/simple_flight/firmware/Mixer.hpp index 4e280faf30..621d9a2afb 100644 --- a/AirLib/include/controllers/simple_flight/firmware/Mixer.hpp +++ b/AirLib/include/controllers/simple_flight/firmware/Mixer.hpp @@ -20,7 +20,7 @@ class Mixer { //otherwise values in pitch/roll/yaw would get clipped randomly and can produce random results //in other words: we can't do angling if throttle is too low if (controls.throttle < params_->min_armed_output) { - motor_outputs.assign(params_->motor_count, controls.throttle); + motor_outputs.assign(params_->motor_count, params_->min_armed_output); return; } diff --git a/AirLib/include/controllers/simple_flight/firmware/Params.hpp b/AirLib/include/controllers/simple_flight/firmware/Params.hpp index 8c2f20cb84..1f3455056d 100644 --- a/AirLib/include/controllers/simple_flight/firmware/Params.hpp +++ b/AirLib/include/controllers/simple_flight/firmware/Params.hpp @@ -6,14 +6,13 @@ namespace simple_flight { struct Params { public: - uint16_t rc_channel_count = 12; - uint16_t rc_read_interval_ms = 10; - int16_t rc_thrust_channel = 2; - int16_t rc_pitch_channel = 3; - int16_t rc_roll_channel = 0; - int16_t rc_yaw_channel = 1; - - int16_t rc_rate_angle_channel = 6; //corresponds to switch 3 + struct Rc { + uint16_t channel_count = 12; + uint16_t read_interval_ms = 10; + int16_t rate_level_mode_channel = 6; //corresponds to switch 3 on FrSky Taranis + + Axis4 channels = Axis4(2, 0, 3, 1); + } rc; //this should match up with target board diff --git a/AirLib/include/controllers/simple_flight/firmware/RemoteControl.hpp b/AirLib/include/controllers/simple_flight/firmware/RemoteControl.hpp index 43bb32e5e9..d8b197c353 100644 --- a/AirLib/include/controllers/simple_flight/firmware/RemoteControl.hpp +++ b/AirLib/include/controllers/simple_flight/firmware/RemoteControl.hpp @@ -20,11 +20,10 @@ class RemoteControl : public IGoalInput { { IGoalInput::reset(); - rc_channels_.assign(params_->rc_channel_count, 0); - last_rec_read_ = 0; - goal_ = Axis4r(); goal_mode_ = params_->default_goal_mode; + channel_vals_.assign(params_->rc.channel_count, 0); + last_rec_read_ = 0; } virtual void update() override @@ -34,30 +33,30 @@ class RemoteControl : public IGoalInput { uint64_t time = clock_->millis(); //don't keep reading if not updated - if (time - last_rec_read_ <= params_->rc_read_interval_ms) + if (time - last_rec_read_ <= params_->rc.read_interval_ms) return; last_rec_read_ = time; //read rc - for (uint16_t channel = 0; channel < params_->rc_channel_count; ++channel) - rc_channels_[channel] = board_inputs_->readChannel(channel); + for (uint16_t channel = 0; channel < params_->rc.channel_count; ++channel) + channel_vals_[channel] = board_inputs_->readChannel(channel); - goal_.throttle = rc_channels_[params_->rc_thrust_channel]; - if (rc_channels_[params_->rc_rate_angle_channel] < 0.1f) { //approximately 0 + goal_.throttle = channel_vals_[params_->rc.channels.throttle]; + if (channel_vals_[params_->rc.rate_level_mode_channel] < 0.1f) { //approximately 0 goal_mode_ = GoalMode::getStandardAngleMode(); //we are in control-by-angles mode - goal_.axis3.pitch() = params_->max_angle_level.pitch() * rc_channels_[params_->rc_pitch_channel]; - goal_.axis3.roll() = params_->max_angle_level.roll() * rc_channels_[params_->rc_roll_channel]; - goal_.axis3.yaw() = params_->max_angle_level.yaw() * rc_channels_[params_->rc_yaw_channel]; + goal_.axis3.pitch() = params_->max_angle_level.pitch() * channel_vals_[params_->rc.channels.axis3.pitch()]; + goal_.axis3.roll() = params_->max_angle_level.roll() * channel_vals_[params_->rc_roll_channel]; + goal_.axis3.yaw() = params_->max_angle_level.yaw() * channel_vals_[params_->rc_yaw_channel]; } else { //we are in control-by-rate mode goal_mode_ = GoalMode::getAllRateMode(); - goal_.axis3.pitch() = params_->max_angle_rate.pitch() * rc_channels_[params_->rc_pitch_channel]; - goal_.axis3.roll() = params_->max_angle_rate.roll() * rc_channels_[params_->rc_roll_channel]; - goal_.axis3.yaw() = params_->max_angle_rate.yaw() * rc_channels_[params_->rc_yaw_channel]; + goal_.axis3.pitch() = params_->max_angle_rate.pitch() * channel_vals_[params_->rc_pitch_channel]; + goal_.axis3.roll() = params_->max_angle_rate.roll() * channel_vals_[params_->rc_roll_channel]; + goal_.axis3.yaw() = params_->max_angle_rate.yaw() * channel_vals_[params_->rc_yaw_channel]; } } @@ -72,6 +71,15 @@ class RemoteControl : public IGoalInput { return goal_mode_; } +private: + enum class RcRequestType { + None, ArmRequest, DisarmRequest + }; + + RcRequestType getRcRequest() + { + + } private: const IBoardClock* clock_; @@ -81,7 +89,7 @@ class RemoteControl : public IGoalInput { Axis4r goal_; GoalMode goal_mode_; - std::vector rc_channels_; + std::vector channel_vals_; uint64_t last_rec_read_; }; diff --git a/AirLib/include/controllers/simple_flight/firmware/interfaces/CommonStructs.hpp b/AirLib/include/controllers/simple_flight/firmware/interfaces/CommonStructs.hpp index bf8413ab8c..c0cb3c42e1 100644 --- a/AirLib/include/controllers/simple_flight/firmware/interfaces/CommonStructs.hpp +++ b/AirLib/include/controllers/simple_flight/firmware/interfaces/CommonStructs.hpp @@ -44,10 +44,18 @@ class Axis3 { }; typedef Axis3 Axis3r; -struct Axis4r { - TReal throttle = 0; - Axis3r axis3; +template +struct Axis4 { + T throttle = 0; + Axis3 axis3; + + Axis4(const T& throttle_val = T(), const T& x_val = T(), const T& y_val = T(), const T& z_val = T()) + : throttle(throttle_val), axis(x_val, y_val, z_val) + { + } }; +typedef Axis4 Axis4r; + enum class GoalModeType { AngleLevel,