76 lines
2.2 KiB
C++
76 lines
2.2 KiB
C++
#pragma once
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include "AP_HAL_QURT.h"
|
|
#include <AP_ESC_Telem/AP_ESC_Telem.h>
|
|
|
|
class QURT::RCOutput : public AP_HAL::RCOutput, AP_ESC_Telem_Backend
|
|
{
|
|
public:
|
|
void init() override;
|
|
void set_freq(uint32_t chmask, uint16_t freq_hz) override;
|
|
uint16_t get_freq(uint8_t ch) override;
|
|
void enable_ch(uint8_t ch) override;
|
|
void disable_ch(uint8_t ch) override;
|
|
void write(uint8_t ch, uint16_t period_us) override;
|
|
uint16_t read(uint8_t ch) override;
|
|
void read(uint16_t *period_us, uint8_t len) override;
|
|
void cork(void) override;
|
|
void push(void) override;
|
|
|
|
float get_voltage(void) const
|
|
{
|
|
return esc_voltage;
|
|
}
|
|
float get_current(void) const
|
|
{
|
|
return esc_current;
|
|
}
|
|
|
|
|
|
private:
|
|
const uint32_t baudrate = 2000000;
|
|
|
|
void send_receive(void);
|
|
void check_response(void);
|
|
void send_esc_packet(uint8_t type, uint8_t *data, uint16_t size);
|
|
|
|
struct PACKED esc_response_v2 {
|
|
uint8_t id_state; // bits 0:3 = state, bits 4:7 = ID
|
|
|
|
uint16_t rpm; // Current RPM of the motor
|
|
uint8_t cmd_counter; // Number of commands received by the ESC
|
|
uint8_t power; // Applied power [0..100]
|
|
|
|
uint16_t voltage; // Voltage measured by the ESC in mV
|
|
int16_t current; // Current measured by the ESC in 8mA resolution
|
|
int16_t temperature; // Temperature measured by the ESC in 0.01 degC resolution
|
|
};
|
|
|
|
struct PACKED esc_power_status {
|
|
uint8_t id; //ESC Id (could be used as system ID elsewhere)
|
|
uint16_t voltage; //Input voltage (Millivolts)
|
|
int16_t current; //Total Current (8mA resolution)
|
|
};
|
|
|
|
void handle_esc_feedback(const struct esc_response_v2 &pkt);
|
|
void handle_power_status(const struct esc_power_status &pkt);
|
|
|
|
// order is RL, FL, FR, RR, map to X frame
|
|
const uint8_t esc_map[4] {2, 0, 1, 3};
|
|
const uint8_t esc_map_rev[4] {1, 2, 0, 3};
|
|
|
|
int fd = -1;
|
|
uint16_t enable_mask;
|
|
static const uint8_t channel_count = 4;
|
|
uint16_t period[channel_count];
|
|
volatile bool need_write;
|
|
bool corked;
|
|
HAL_Semaphore mutex;
|
|
uint8_t last_fb_idx;
|
|
uint32_t last_fb_req_ms;
|
|
|
|
float esc_voltage;
|
|
float esc_current;
|
|
};
|