mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_PiccoloCAN: SendECU throttle commands over CAN
This commit is contained in:
parent
1cff0125ae
commit
19f135b1b6
@ -45,6 +45,10 @@
|
||||
#include <AP_PiccoloCAN/piccolo_protocol/ServoProtocol.h>
|
||||
#include <AP_PiccoloCAN/piccolo_protocol/ServoPackets.h>
|
||||
|
||||
// Protocol files for the ECU
|
||||
#include <AP_PiccoloCAN/piccolo_protocol/ECUProtocol.h>
|
||||
#include <AP_PiccoloCAN/piccolo_protocol/ECUPackets.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#if HAL_CANMANAGER_ENABLED
|
||||
@ -85,7 +89,22 @@ const AP_Param::GroupInfo AP_PiccoloCAN::var_info[] = {
|
||||
// @User: Advanced
|
||||
// @Range: 1 500
|
||||
AP_GROUPINFO("SRV_RT", 4, AP_PiccoloCAN, _srv_hz, PICCOLO_MSG_RATE_HZ_DEFAULT),
|
||||
#if HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
// @Param: ECU_ID
|
||||
// @DisplayName: ECU Node ID
|
||||
// @Description: Node ID to send ECU throttle messages to. Set to zero to disable ECU throttle messages. Set to 255 to broadcast to all ECUs.
|
||||
// @Range: 0 255
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ECU_ID", 5, AP_PiccoloCAN, _ecu_id, PICCOLO_CAN_ECU_ID_DEFAULT),
|
||||
|
||||
// @Param: ECU_RT
|
||||
// @DisplayName: ECU command output rate
|
||||
// @Description: Output rate of ECU command messages
|
||||
// @Units: Hz
|
||||
// @User: Advanced
|
||||
// @Range: 1 500
|
||||
AP_GROUPINFO("ECU_RT", 6, AP_PiccoloCAN, _ecu_hz, PICCOLO_MSG_RATE_HZ_DEFAULT),
|
||||
#endif
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
@ -163,6 +182,9 @@ void AP_PiccoloCAN::loop()
|
||||
|
||||
uint16_t esc_tx_counter = 0;
|
||||
uint16_t servo_tx_counter = 0;
|
||||
#if HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
uint16_t ecu_tx_counter = 0;
|
||||
#endif
|
||||
|
||||
// CAN Frame ID components
|
||||
uint8_t frame_id_group; // Piccolo message group
|
||||
@ -185,7 +207,11 @@ void AP_PiccoloCAN::loop()
|
||||
_srv_hz.set(constrain_int16(_srv_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX));
|
||||
|
||||
uint16_t servoCmdRateMs = 1000 / _srv_hz;
|
||||
#if HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
_ecu_hz = constrain_int16(_ecu_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX);
|
||||
|
||||
uint16_t ecuCmdRateMs = 1000 / _ecu_hz;
|
||||
#endif
|
||||
uint64_t timeout = AP_HAL::micros64() + 250ULL;
|
||||
|
||||
// 1ms loop delay
|
||||
@ -203,6 +229,14 @@ void AP_PiccoloCAN::loop()
|
||||
send_servo_messages();
|
||||
}
|
||||
|
||||
#if HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
// Transmit ecu throttle commands at regular intervals
|
||||
if (ecu_tx_counter++ > ecuCmdRateMs) {
|
||||
ecu_tx_counter = 0;
|
||||
send_ecu_messages();
|
||||
}
|
||||
#endif
|
||||
|
||||
// Look for any message responses on the CAN bus
|
||||
while (read_frame(rxFrame, timeout)) {
|
||||
|
||||
@ -330,6 +364,13 @@ void AP_PiccoloCAN::update()
|
||||
}
|
||||
}
|
||||
|
||||
#if HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
if (_ecu_id != 0) {
|
||||
_ecu_info.command = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
|
||||
_ecu_info.newCommand = true;
|
||||
}
|
||||
#endif // HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
|
||||
AP_Logger *logger = AP_Logger::get_singleton();
|
||||
|
||||
// Push received telemetry data into the logging system
|
||||
@ -746,6 +787,27 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame)
|
||||
}
|
||||
|
||||
#if HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
void AP_PiccoloCAN::send_ecu_messages(void)
|
||||
{
|
||||
AP_HAL::CANFrame txFrame {};
|
||||
|
||||
const uint64_t timeout = AP_HAL::micros64() + 1000ULL;
|
||||
|
||||
// No ECU node id set, don't send anything
|
||||
if (_ecu_id == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (_ecu_info.newCommand) {
|
||||
encodeECU_ThrottleCommandPacket(&txFrame, _ecu_info.command);
|
||||
txFrame.id |= (uint8_t) _ecu_id;
|
||||
|
||||
_ecu_info.newCommand = false;
|
||||
|
||||
write_frame(txFrame, timeout);
|
||||
}
|
||||
}
|
||||
|
||||
bool AP_PiccoloCAN::handle_ecu_message(AP_HAL::CANFrame &frame)
|
||||
{
|
||||
// Get the ecu instance
|
||||
@ -755,7 +817,7 @@ bool AP_PiccoloCAN::handle_ecu_message(AP_HAL::CANFrame &frame)
|
||||
}
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
#endif // HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
|
||||
/**
|
||||
* Check if a given servo channel is "active" (has been configured for Piccolo control output)
|
||||
|
@ -12,7 +12,7 @@
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* Author: Oliver Walters
|
||||
* Author: Oliver Walters / Currawong Engineering Pty Ltd
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
@ -47,6 +47,8 @@
|
||||
#define PICCOLO_MSG_RATE_HZ_MAX 500
|
||||
#define PICCOLO_MSG_RATE_HZ_DEFAULT 50
|
||||
|
||||
#define PICCOLO_CAN_ECU_ID_DEFAULT 0
|
||||
|
||||
class AP_PiccoloCAN : public AP_CANDriver, public AP_ESC_Telem_Backend
|
||||
{
|
||||
public:
|
||||
@ -134,6 +136,8 @@ private:
|
||||
bool handle_servo_message(AP_HAL::CANFrame &frame);
|
||||
|
||||
#if HAL_EFI_CURRAWONG_ECU_ENABLED
|
||||
void send_ecu_messages(void);
|
||||
|
||||
// interpret an ECU message received over CAN
|
||||
bool handle_ecu_message(AP_HAL::CANFrame &frame);
|
||||
#endif
|
||||
@ -207,6 +211,11 @@ private:
|
||||
|
||||
} _esc_info[PICCOLO_CAN_MAX_NUM_ESC];
|
||||
|
||||
struct CurrawongECU_Info_t {
|
||||
float command;
|
||||
bool newCommand;
|
||||
} _ecu_info;
|
||||
|
||||
// Piccolo CAN parameters
|
||||
AP_Int32 _esc_bm; //! ESC selection bitmask
|
||||
AP_Int16 _esc_hz; //! ESC update rate (Hz)
|
||||
@ -214,7 +223,8 @@ private:
|
||||
AP_Int32 _srv_bm; //! Servo selection bitmask
|
||||
AP_Int16 _srv_hz; //! Servo update rate (Hz)
|
||||
|
||||
AP_Int8 _ecu_en; //! ECU Enable
|
||||
AP_Int16 _ecu_id; //! ECU Node ID
|
||||
AP_Int16 _ecu_hz; //! ECU update rate (Hz)
|
||||
|
||||
HAL_Semaphore _telem_sem;
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user