mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 14:47:44 +08:00
dshot: breakout driver header file, use static_casts, add const specifiers (#15908)
* Break out dshot.h from dshot.cpp and rename files to reflect the class name. Move DShot class specific #defines from drv_pwm_output.h to dshot.h. Replace c-style casts with static_casts, standardize method naming, single line comment styles, and formatting. Rename update_dshot_out_state() to enable_dshot_outputs().
This commit is contained in:
Mark Sauder
GitHub
@@ -293,14 +293,6 @@ struct pwm_output_rc_config {
|
||||
#define PWM_RATE_LOWER_LIMIT 1u
|
||||
#define PWM_RATE_UPPER_LIMIT 10000u
|
||||
|
||||
/** Dshot PWM frequency */
|
||||
#define DSHOT1200 1200000u //Hz
|
||||
#define DSHOT600 600000u //Hz
|
||||
#define DSHOT300 300000u //Hz
|
||||
#define DSHOT150 150000u //Hz
|
||||
|
||||
#define DSHOT_MAX_THROTTLE 1999
|
||||
|
||||
typedef enum {
|
||||
DShot_cmd_motor_stop = 0,
|
||||
DShot_cmd_beacon1,
|
||||
@@ -315,23 +307,23 @@ typedef enum {
|
||||
DShot_cmd_3d_mode_on,
|
||||
DShot_cmd_settings_request, // Currently not implemented
|
||||
DShot_cmd_save_settings,
|
||||
DShot_cmd_spin_direction_normal = 20,
|
||||
DShot_cmd_spin_direction_normal = 20,
|
||||
DShot_cmd_spin_direction_reversed = 21,
|
||||
DShot_cmd_led0_on, // BLHeli32 only
|
||||
DShot_cmd_led1_on, // BLHeli32 only
|
||||
DShot_cmd_led2_on, // BLHeli32 only
|
||||
DShot_cmd_led3_on, // BLHeli32 only
|
||||
DShot_cmd_led0_off, // BLHeli32 only
|
||||
DShot_cmd_led1_off, // BLHeli32 only
|
||||
DShot_cmd_led2_off, // BLHeli32 only
|
||||
DShot_cmd_led4_off, // BLHeli32 only
|
||||
DShot_cmd_audio_stream_mode_on_off = 30, // KISS audio Stream mode on/off
|
||||
DShot_cmd_silent_mode_on_off = 31, // KISS silent Mode on/off
|
||||
DShot_cmd_signal_line_telemeetry_disable = 32,
|
||||
DShot_cmd_led0_on, // BLHeli32 only
|
||||
DShot_cmd_led1_on, // BLHeli32 only
|
||||
DShot_cmd_led2_on, // BLHeli32 only
|
||||
DShot_cmd_led3_on, // BLHeli32 only
|
||||
DShot_cmd_led0_off, // BLHeli32 only
|
||||
DShot_cmd_led1_off, // BLHeli32 only
|
||||
DShot_cmd_led2_off, // BLHeli32 only
|
||||
DShot_cmd_led4_off, // BLHeli32 only
|
||||
DShot_cmd_audio_stream_mode_on_off = 30, // KISS audio Stream mode on/off
|
||||
DShot_cmd_silent_mode_on_off = 31, // KISS silent Mode on/off
|
||||
DShot_cmd_signal_line_telemeetry_disable = 32,
|
||||
DShot_cmd_signal_line_continuous_erpm_telemetry = 33,
|
||||
DShot_cmd_MAX = 47,
|
||||
DShot_cmd_MIN_throttle = 48
|
||||
// >47 are throttle values
|
||||
DShot_cmd_MAX = 47, // >47 are throttle values
|
||||
DShot_cmd_MIN_throttle = 48,
|
||||
DShot_cmd_MAX_throttle = 2047
|
||||
} dshot_command_t;
|
||||
|
||||
|
||||
@@ -424,11 +416,11 @@ __EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel);
|
||||
/**
|
||||
* Intialise the Dshot outputs using the specified configuration.
|
||||
*
|
||||
* @param channel_mask Bitmask of channels (LSB = channel 0) to enable.
|
||||
* This allows some of the channels to remain configured
|
||||
* as GPIOs or as another function.
|
||||
* @param dshot_pwm_freq is frequency of DSHOT signal. Usually DSHOT1200, DSHOT600, DSHOT300 or DSHOT150
|
||||
* @return OK on success.
|
||||
* @param channel_mask Bitmask of channels (LSB = channel 0) to enable.
|
||||
* This allows some of the channels to remain configured
|
||||
* as GPIOs or as another function.
|
||||
* @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200
|
||||
* @return OK on success.
|
||||
*/
|
||||
__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq);
|
||||
|
||||
@@ -436,8 +428,8 @@ __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq
|
||||
* Set the current dshot throttle value for a channel (motor).
|
||||
*
|
||||
* @param channel The channel to set.
|
||||
* @param throttle The output dshot throttle value in [0, 1999 = DSHOT_MAX_THROTTLE].
|
||||
* @param telemetry If true, request telemetry from that motor
|
||||
* @param throttle The output dshot throttle value in [0 = DSHOT_DISARM_VALUE, 1 = DSHOT_MIN_THROTTLE, 1999 = DSHOT_MAX_THROTTLE].
|
||||
* @param telemetry If true, request telemetry from that motor
|
||||
*/
|
||||
__EXPORT extern void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry);
|
||||
|
||||
@@ -446,7 +438,7 @@ __EXPORT extern void up_dshot_motor_data_set(unsigned channel, uint16_t throttle
|
||||
*
|
||||
* @param channel The channel to set.
|
||||
* @param command dshot_command_t
|
||||
* @param telemetry If true, request telemetry from that motor
|
||||
* @param telemetry If true, request telemetry from that motor
|
||||
*/
|
||||
__EXPORT extern void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry);
|
||||
|
||||
|
||||
@@ -35,8 +35,8 @@ px4_add_module(
|
||||
MAIN dshot
|
||||
STACK_MAIN 1200
|
||||
SRCS
|
||||
dshot.cpp
|
||||
telemetry.cpp
|
||||
DShot.cpp
|
||||
DShotTelemetry.cpp
|
||||
DEPENDS
|
||||
arch_io_pins
|
||||
arch_dshot
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,233 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019-2020 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_input_capture.h>
|
||||
#include <drivers/drv_mixer.h>
|
||||
#include <lib/mixer_module/mixer_module.hpp>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
|
||||
#include "DShotTelemetry.h"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
#if !defined(BOARD_HAS_PWM)
|
||||
# error "board_config.h needs to define BOARD_HAS_PWM"
|
||||
#endif
|
||||
|
||||
/** Dshot PWM frequency, Hz */
|
||||
static constexpr unsigned int DSHOT150 = 150000u;
|
||||
static constexpr unsigned int DSHOT300 = 300000u;
|
||||
static constexpr unsigned int DSHOT600 = 600000u;
|
||||
static constexpr unsigned int DSHOT1200 = 1200000u;
|
||||
|
||||
static constexpr int DSHOT_DISARM_VALUE = 0;
|
||||
static constexpr int DSHOT_MIN_THROTTLE = 1;
|
||||
static constexpr int DSHOT_MAX_THROTTLE = 1999;
|
||||
|
||||
class DShot : public cdev::CDev, public ModuleBase<DShot>, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
DShot();
|
||||
virtual ~DShot();
|
||||
|
||||
enum Mode {
|
||||
MODE_NONE = 0,
|
||||
MODE_1PWM,
|
||||
MODE_2PWM,
|
||||
MODE_2PWM2CAP,
|
||||
MODE_3PWM,
|
||||
MODE_3PWM1CAP,
|
||||
MODE_4PWM,
|
||||
MODE_4PWM1CAP,
|
||||
MODE_4PWM2CAP,
|
||||
MODE_5PWM,
|
||||
MODE_5PWM1CAP,
|
||||
MODE_6PWM,
|
||||
MODE_8PWM,
|
||||
MODE_14PWM,
|
||||
MODE_4CAP,
|
||||
MODE_5CAP,
|
||||
MODE_6CAP,
|
||||
};
|
||||
|
||||
/** Mode given via CLI */
|
||||
enum PortMode {
|
||||
PORT_MODE_UNSET = 0,
|
||||
PORT_FULL_GPIO,
|
||||
PORT_FULL_PWM,
|
||||
PORT_PWM8,
|
||||
PORT_PWM6,
|
||||
PORT_PWM5,
|
||||
PORT_PWM4,
|
||||
PORT_PWM3,
|
||||
PORT_PWM2,
|
||||
PORT_PWM1,
|
||||
PORT_PWM3CAP1,
|
||||
PORT_PWM4CAP1,
|
||||
PORT_PWM4CAP2,
|
||||
PORT_PWM5CAP1,
|
||||
PORT_PWM2CAP2,
|
||||
PORT_CAPTURE,
|
||||
};
|
||||
|
||||
static void capture_trampoline(void *context, const uint32_t channel_index, const hrt_abstime edge_time,
|
||||
const uint32_t edge_state, const uint32_t overflow);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
Mode get_mode() { return _mode; }
|
||||
|
||||
virtual int init();
|
||||
|
||||
virtual int ioctl(file *filp, int cmd, unsigned long arg);
|
||||
|
||||
/** change the mode of the running module */
|
||||
static int module_new_mode(const PortMode new_mode);
|
||||
|
||||
void mixerChanged() override;
|
||||
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
void retrieve_and_print_esc_info_thread_safe(const int motor_index);
|
||||
|
||||
/**
|
||||
* Send a dshot command to one or all motors
|
||||
* This is expected to be called from another thread.
|
||||
* @param num_repetitions number of times to repeat, set at least to 1
|
||||
* @param motor_index index or -1 for all
|
||||
* @return 0 on success, <0 error otherwise
|
||||
*/
|
||||
int send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index);
|
||||
|
||||
int set_mode(const Mode new_mode);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
bool telemetry_enabled() const { return _telemetry != nullptr; }
|
||||
|
||||
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated) override;
|
||||
|
||||
private:
|
||||
|
||||
/** Disallow copy construction and move assignment. */
|
||||
DShot(const DShot &) = delete;
|
||||
DShot operator=(const DShot &) = delete;
|
||||
|
||||
enum class DShotConfig {
|
||||
Disabled = 0,
|
||||
DShot150 = 150,
|
||||
DShot300 = 300,
|
||||
DShot600 = 600,
|
||||
DShot1200 = 1200,
|
||||
};
|
||||
|
||||
struct Command {
|
||||
dshot_command_t command{};
|
||||
int num_repetitions{0};
|
||||
uint8_t motor_mask{0xff};
|
||||
bool valid() const { return num_repetitions > 0; }
|
||||
void clear() { num_repetitions = 0; }
|
||||
};
|
||||
|
||||
struct Telemetry {
|
||||
DShotTelemetry handler{};
|
||||
uORB::PublicationData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
|
||||
int last_motor_index{-1};
|
||||
};
|
||||
|
||||
void capture_callback(const uint32_t channel_index, const hrt_abstime edge_time,
|
||||
const uint32_t edge_state, const uint32_t overflow);
|
||||
|
||||
int capture_ioctl(file *filp, const int cmd, const unsigned long arg);
|
||||
|
||||
void enable_dshot_outputs(const bool enabled);
|
||||
|
||||
void init_telemetry(const char *device);
|
||||
|
||||
void handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data);
|
||||
|
||||
int pwm_ioctl(file *filp, const int cmd, const unsigned long arg);
|
||||
|
||||
int request_esc_info();
|
||||
|
||||
void Run() override;
|
||||
|
||||
void update_params();
|
||||
|
||||
void update_telemetry_num_motors();
|
||||
|
||||
MixingOutput _mixing_output{DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
|
||||
Telemetry *_telemetry{nullptr};
|
||||
|
||||
static char _telemetry_device[20];
|
||||
static px4::atomic_bool _request_telemetry_init;
|
||||
|
||||
px4::atomic<Command *> _new_command{nullptr};
|
||||
|
||||
px4::atomic<DShotTelemetry::OutputBuffer *> _request_esc_info{nullptr};
|
||||
|
||||
bool _outputs_initialized{false};
|
||||
bool _outputs_on{false};
|
||||
bool _waiting_for_esc_info{false};
|
||||
|
||||
unsigned _num_outputs{0};
|
||||
uint32_t _output_mask{0};
|
||||
|
||||
int _class_instance{-1};
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
|
||||
Command _current_command{};
|
||||
|
||||
Mode _mode{MODE_NONE};
|
||||
|
||||
uORB::Subscription _param_sub{ORB_ID(parameter_update)};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::DSHOT_CONFIG>) _param_dshot_config,
|
||||
(ParamFloat<px4::params::DSHOT_MIN>) _param_dshot_min,
|
||||
(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count
|
||||
)
|
||||
};
|
||||
@@ -31,7 +31,7 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "telemetry.h"
|
||||
#include "DShotTelemetry.h"
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
Reference in New Issue
Block a user