dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

UAVCAN: allow to arm single ESCs

This commit is contained in:
Holger Steinhaus 2014-08-14 15:43:27 +02:00
parent 1cab08cc2a
commit 58f36714f8
3 changed files with 30 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/modules/uavcan/actuators/esc.cpp
View File

@ -40,6 +40,9 @@
#include "esc.hpp"
#include <systemlib/err.h>
#define MOTOR_BIT(x) (1<<(x))
UavcanEscController::UavcanEscController(uavcan::INode &node) :
_node(node),
_uavcan_pub_raw_cmd(node),
@ -95,9 +98,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
if (_armed) {
for (unsigned i = 0; i < num_outputs; i++) {
for (unsigned i = 0; i < num_outputs; i++) {
if (_armed_mask & MOTOR_BIT(i)) {
float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
if (scaled < 1.0F) {
scaled = 1.0F; // Since we're armed, we don't want to stop it completely
@ -112,6 +114,9 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
}
else {
msg.cmd.push_back(static_cast<unsigned>(0));
}
}
/*
@ -121,9 +126,20 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
(void)_uavcan_pub_raw_cmd.broadcast(msg);
}
void UavcanEscController::arm_esc(bool arm)
void UavcanEscController::arm_all_escs(bool arm)
{
_armed = arm;
if (arm)
_armed_mask = -1;
else
_armed_mask = 0;
}
void UavcanEscController::arm_single_esc(int num, bool arm)
{
if (arm)
_armed_mask = MOTOR_BIT(num);
else
_armed_mask = 0;
}
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)

9
src/modules/uavcan/actuators/esc.hpp
View File

@ -61,7 +61,8 @@ public:
void update_outputs(float *outputs, unsigned num_outputs);
void arm_esc(bool arm);
void arm_all_escs(bool arm);
void arm_single_esc(int num, bool arm);
private:
/**
@ -98,6 +99,12 @@ private:
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer;
/*
* ESC states
*/
uint32_t _armed_mask = 0;
uavcan::equipment::esc::Status _states[MAX_ESCS];
/*
* Perf counters
*/

2
src/modules/uavcan/uavcan_main.cpp
View File

@ -429,7 +429,7 @@ int
UavcanNode::arm_actuators(bool arm)
{
_is_armed = arm;
_esc_controller.arm_esc(arm);
_esc_controller.arm_all_escs(arm);
return OK;
}