Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/SITL/SIM_FETtecOneWireESC.cpp

508 lines
16 KiB
C++
Raw Permalink Blame History

/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
Simulator for the FETtecOneWireESC
TODO:
- verify the assertion that DMA is required
- stop ignoring REQ_TYPE while in bootloader?
- correct visibility of members in simulation
- half-duplex will require the use of a thread as every time we call update() we expect to send out a configuration message
- tidy break vs return in AP_FETtec::handle_message
- determine if we should have a "REQ_OK" as well as an "OK"
- should rename simulated ESC "pwm" field to "value" or "fettec_value" or something
- periodically log _unknown_esc_message, _message_invalid_in_state_count, _period_too_short, _receive_buf_used to dataflash using a low prio thread.
- log type, version, subversion and sn to dataflash once.
Protocol:
- SET_FAST_COM_LENGTH could set a 32-bit bitmask that will be present rather than requring consecutive motors
- Use two magic bytes in the header instead of just one
- Use a 16bit CRC
- the reply request needs to repeat the data that it replies to, to make sure the reply can be clearly assigned to a request
- need to cope with reversals
- in the case that we don't have ESC telemetry, consider probing ESCs periodically with an "OK"-request while disarmed
*/
#include <AP_Math/AP_Math.h>
#include "SIM_FETtecOneWireESC.h"
#include "SITL.h"
#include <AP_HAL/utility/sparse-endian.h>
#include "SIM_Aircraft.h"
#include <stdio.h>
#include <errno.h>
using namespace SITL;
// table of user settable parameters
const AP_Param::GroupInfo FETtecOneWireESC::var_info[] = {
// @Param: ENA
// @DisplayName: FETtec OneWire ESC simulator enable/disable
// @Description: Allows you to enable (1) or disable (0) the FETtecOneWireESC simulator
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("ENA", 1, FETtecOneWireESC, _enabled, 0),
// @Param: PWOF
// @DisplayName: Power off FETtec ESC mask
// @Description: Allows you to turn power off to the simulated ESCs. Bits correspond to the ESC ID, *NOT* their servo channel.
// @User: Advanced
AP_GROUPINFO("POW", 2, FETtecOneWireESC, _powered_mask, 0xfff),
AP_GROUPEND
};
FETtecOneWireESC::FETtecOneWireESC() : SerialDevice::SerialDevice()
{
AP_Param::setup_object_defaults(this, var_info);
// initialise serial numbers and IDs
for (uint8_t n=0; n<ARRAY_SIZE(escs); n++) {
ESC &esc = escs[n];
esc.ofs = n; // so we can index for RPM, for example
esc.id = n+1; // really should parameterise this
for (uint8_t i=0; i<ARRAY_SIZE(esc.sn); i++) {
esc.sn[i] = n+1;
}
}
}
void FETtecOneWireESC::update_escs()
{
// process the power-off mask
for (auto &esc : escs) {
bool should_be_on = _powered_mask & (1U<<(esc.id-1));
switch (esc.state) {
case ESC::State::POWERED_OFF:
if (should_be_on) {
esc.state = ESC::State::IN_BOOTLOADER;
esc.pwm = 0;
esc.fast_com = {};
esc.telem_request = false;
}
break;
case ESC::State::IN_BOOTLOADER:
case ESC::State::RUNNING:
case ESC::State::RUNNING_START:
if (!should_be_on) {
esc.state = ESC::State::POWERED_OFF;
break;
}
}
}
for (auto &esc : escs) {
switch (esc.state) {
case ESC::State::POWERED_OFF:
case ESC::State::IN_BOOTLOADER:
case ESC::State::RUNNING:
continue;
case ESC::State::RUNNING_START:
esc.set_state(ESC::State::RUNNING);
send_response(PackedMessage<OK> {
esc.id,
OK{}
});
}
}
for (auto &esc : escs) {
if (esc.state != ESC::State::RUNNING) {
continue;
}
// FIXME: this may not be an entirely accurate model of the
// temperature profile of these ESCs.
esc.temperature += esc.pwm/100000;
esc.temperature *= 0.95;
}
}
void FETtecOneWireESC::update(const class Aircraft &aircraft)
{
if (!_enabled.get()) {
return;
}
update_escs();
update_input();
update_send(aircraft);
}
void FETtecOneWireESC::handle_config_message()
{
ESC &esc = escs[u.config_message_header.target_id-1];
simfet_debug("Config message type=%u esc=%u", (unsigned)u.config_message_header.request_type, (unsigned)u.config_message_header.target_id);
if ((ResponseFrameHeaderID)u.config_message_header.header != ResponseFrameHeaderID::MASTER) {
AP_HAL::panic("Unexpected header ID");
}
switch (esc.state) {
case ESC::State::POWERED_OFF:
return;
case ESC::State::IN_BOOTLOADER:
return bootloader_handle_config_message(esc);
case ESC::State::RUNNING_START:
return;
case ESC::State::RUNNING:
return running_handle_config_message(esc);
}
AP_HAL::panic("Unknown state");
}
template <typename T>
void FETtecOneWireESC::send_response(const T &r)
{
// simfet_debug("Sending response");
if (write_to_autopilot((char*)&r, sizeof(r)) != sizeof(r)) {
AP_HAL::panic("short write");
}
}
void FETtecOneWireESC::bootloader_handle_config_message(FETtecOneWireESC::ESC &esc)
{
switch ((ConfigMessageType)u.config_message_header.request_type) {
case ConfigMessageType::OK: {
PackedMessage<OK> msg {
esc.id,
OK{},
};
msg.header = (uint8_t)ResponseFrameHeaderID::BOOTLOADER;
msg.update_checksum();
send_response(msg);
return;
}
case ConfigMessageType::BL_PAGE_CORRECT: // BL only
case ConfigMessageType::NOT_OK:
break;
case ConfigMessageType::BL_START_FW: // BL only
esc.set_state(ESC::State::RUNNING_START);
// the main firmware sends an OK
return;
case ConfigMessageType::BL_PAGES_TO_FLASH: // BL only
break;
case ConfigMessageType::REQ_TYPE:
// ignore this for now
return;
case ConfigMessageType::REQ_SN:
case ConfigMessageType::REQ_SW_VER:
case ConfigMessageType::BEEP:
case ConfigMessageType::SET_FAST_COM_LENGTH:
case ConfigMessageType::SET_TLM_TYPE: //1 for alternative telemetry. ESC sends full telem per ESC: Temp, Volt, Current, ERPM, Consumption, CrcErrCount
case ConfigMessageType::SET_LED_TMP_COLOR:
break;
}
return;
AP_HAL::panic("Unhandled config message in bootloader (%u)",
(unsigned)u.config_message_header.request_type);
}
void FETtecOneWireESC::running_handle_config_message(FETtecOneWireESC::ESC &esc)
{
switch ((ConfigMessageType)u.config_message_header.request_type) {
case ConfigMessageType::OK:
return send_response(PackedMessage<OK> {
esc.id,
OK{}
});
case ConfigMessageType::BL_PAGE_CORRECT: // BL only
case ConfigMessageType::NOT_OK:
break;
case ConfigMessageType::BL_START_FW: // BL only
::fprintf(stderr, "received unexpected BL_START_FW message\n");
AP_HAL::panic("received unexpected BL_START_FW message");
return;
case ConfigMessageType::BL_PAGES_TO_FLASH: // BL only
break;
case ConfigMessageType::REQ_TYPE:
return send_response(PackedMessage<ESC_TYPE> {
esc.id,
ESC_TYPE{esc.type}
});
case ConfigMessageType::REQ_SN:
return send_response(PackedMessage<SN> {
esc.id,
SN{esc.sn, ARRAY_SIZE(esc.sn)}
});
case ConfigMessageType::REQ_SW_VER:
return send_response(PackedMessage<SW_VER> {
esc.id,
SW_VER{esc.sw_version, esc.sw_subversion}
});
case ConfigMessageType::BEEP:
break;
case ConfigMessageType::SET_FAST_COM_LENGTH:
esc.fast_com.length = u.packed_set_fast_com_length.msg.length;
esc.fast_com.byte_count = u.packed_set_fast_com_length.msg.byte_count;
esc.fast_com.min_esc_id = u.packed_set_fast_com_length.msg.min_esc_id;
esc.fast_com.id_count = u.packed_set_fast_com_length.msg.id_count;
return send_response(PackedMessage<OK> {
esc.id,
OK{}
});
case ConfigMessageType::SET_TLM_TYPE: //1 for alternative telemetry. ESC sends full telem per ESC: Temp, Volt, Current, ERPM, Consumption, CrcErrCount
return handle_config_message_set_tlm_type(esc);
case ConfigMessageType::SET_LED_TMP_COLOR:
break;
}
AP_HAL::panic("Unknown config message (%u)", (unsigned)u.config_message_header.request_type);
}
void FETtecOneWireESC::handle_config_message_set_tlm_type(ESC &esc)
{
const TLMType type = (TLMType)u.packed_set_tlm_type.msg.type;
switch (type) {
case TLMType::NORMAL:
case TLMType::ALTERNATIVE:
esc.telem_type = type;
send_response(PackedMessage<OK> {
esc.id,
OK{}
});
return;
}
AP_HAL::panic("unknown telem type=%u", (unsigned)type);
}
void FETtecOneWireESC::handle_fast_esc_data()
{
// decode first byte - see driver for details
const uint8_t telem_request = u.buffer[0] >> 4;
// offset into escs array for first esc involved in fast-throttle
// command:
const uint8_t esc0_ofs = fast_com.min_esc_id - 1;
// ::fprintf(stderr, "telem_request=%u\n", (unsigned)telem_request);
uint16_t esc0_pwm;
esc0_pwm = ((u.buffer[0] >> 3) & 0x1) << 10;
if ((u.buffer[0] & 0b111) != 0x1) {
AP_HAL::panic("expected fast-throttle command");
}
// decode second byte
esc0_pwm |= (u.buffer[1] >> 5) << 7;
if ((u.buffer[1] & 0b00011111) != 0x1f) {
AP_HAL::panic("Unexpected 5-bit target id");
}
// decode enough of third byte to complete pwm[0]
esc0_pwm |= u.buffer[2] >> 1;
if (escs[esc0_ofs].state == ESC::State::RUNNING) {
ESC &esc { escs[esc0_ofs] };
esc.pwm = esc0_pwm;
if (telem_request == esc.id) {
esc.telem_request = true;
}
simfet_debug("esc=%u out: %u", esc.id, (unsigned)esc.pwm);
}
// decode remainder of ESC values
// slides a window across the input buffer, extracting 11-bit ESC
// values. The top 11 bits in "window" are the ESC value.
uint8_t byte_ofs = 2;
uint32_t window = u.buffer[byte_ofs++]<<24;
window <<= 7;
uint8_t bits_free = 32-1;
for (uint8_t i=esc0_ofs+1; i<esc0_ofs+fast_com.id_count; i++) {
while (bits_free > 7) {
window |= u.buffer[byte_ofs++] << (bits_free-8);
bits_free -= 8;
}
ESC &esc { escs[i] };
if (esc.state == ESC::State::RUNNING) {
if (telem_request == esc.id) {
esc.telem_request = true;
}
esc.pwm = window >> 21;
simfet_debug("esc=%u out: %u", esc.id, (unsigned)esc.pwm);
}
window <<= 11;
bits_free += 11;
}
for (uint8_t i=0; i<ARRAY_SIZE(escs); i++) {
const ESC &esc { escs[i] };
if (esc.pwm == 0) {
continue;
}
// this will need to adjust for reversals. We should also set
// one of the simulated ESCs up to have a pair of motor wires
// crossed i.e. spin backwards. Maybe a mask for it
if (esc.pwm >= 1000 && esc.pwm <= 2000) {
continue;
}
AP_HAL::panic("transmitted value out of range (%u)", esc.pwm);
}
}
void FETtecOneWireESC::consume_bytes(uint8_t count)
{
if (count > buflen) {
AP_HAL::panic("Consuming more bytes than in buffer?");
}
if (buflen == count) {
buflen = 0;
return;
}
memmove(&u.buffer[0], &u.buffer[count], buflen - count);
buflen -= count;
}
void FETtecOneWireESC::update_input()
{
const ssize_t n = read_from_autopilot((char*)&u.buffer[buflen], ARRAY_SIZE(u.buffer) - buflen - 1);
if (n < 0) {
// TODO: do better here
if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
AP_HAL::panic("Failed to read from autopilot");
}
} else {
buflen += n;
}
// bool config_message_checksum_fail = false;
if (buflen > offsetof(ConfigMessageHeader, header) &&
u.config_message_header.header == 0x01 &&
buflen > offsetof(ConfigMessageHeader, frame_len) &&
buflen >= u.config_message_header.frame_len) {
const uint8_t calculated_checksum = crc8_dvb_update(0, u.buffer, u.config_message_header.frame_len-1);
const uint8_t received_checksum = u.buffer[u.config_message_header.frame_len-1];
if (calculated_checksum == received_checksum) {
handle_config_message();
// consume the message:
consume_bytes(u.config_message_header.frame_len);
return;
} else {
simfet_debug("Checksum mismatch");
abort();
// config_message_checksum_fail = true;
}
return; // 1 message/loop....
}
// no config message, so let's see if there's fast PWM input.
if (fast_com.id_count == 255) {
// see if any ESC has been configured:
for (uint8_t i=0; i<ARRAY_SIZE(escs); i++) {
if (escs[i].fast_com.id_count == 255) {
continue;
}
fast_com.id_count = escs[i].fast_com.id_count;
fast_com.min_esc_id = escs[i].fast_com.min_esc_id;
break;
}
}
if (fast_com.id_count == 255) {
// no ESC is configured. Ummm.
buflen = 0;
return;
}
const uint16_t total_bits_required = 4 + 1 + 7 + (fast_com.id_count*11);
const uint8_t bytes_required = (total_bits_required + 7) / 8 + 1;
if (buflen < bytes_required) {
return;
}
if (buflen == bytes_required) {
const uint8_t calculated_checksum = crc8_dvb_update(0, u.buffer, buflen-1);
if (u.buffer[buflen-1] != calculated_checksum) {
AP_HAL::panic("checksum failure");
}
handle_fast_esc_data();
consume_bytes(bytes_required);
return;
}
// debug("Read (%d) bytes from autopilot (%u)", (signed)n, config_message_checksum_fail);
if (n >= 0) {
abort();
}
buflen = 0;
}
void FETtecOneWireESC::update_sitl_input_pwm(struct sitl_input &input)
{
// overwrite the SITL input values passed through from
// sitl_model->update_model with those we're receiving serially
for (auto &esc : escs) {
if (esc.id > ARRAY_SIZE(input.servos)) {
// silently ignore; input.servos is 12-long, we are
// usually 16-long
continue;
}
input.servos[esc.id-1] = esc.pwm;
}
}
void FETtecOneWireESC::send_esc_telemetry(const Aircraft &aircraft)
{
for (auto &esc : escs) {
if (!esc.telem_request) {
continue;
}
esc.telem_request = false;
if (esc.state != ESC::State::RUNNING) {
continue;
}
if (esc.telem_type != TLMType::ALTERNATIVE) {
// no idea what "normal" looks like
abort();
}
const int8_t temp_cdeg = esc.temperature * 100;
const uint16_t voltage = aircraft.get_battery_voltage() * 100;
const uint16_t current = (6 + esc.id * 100);
// FIXME: the vehicle models should be supplying this RPM!
const uint16_t Kv = 1000;
const float p = (esc.pwm-1000)/1000.0;
int16_t rpm = aircraft.get_battery_voltage() * Kv * p;
const uint16_t consumption_mah = 0;
const uint16_t errcount = 17;
send_response(PackedMessage<ESCTelem> {
esc.id,
ESCTelem{temp_cdeg, voltage, current, rpm, consumption_mah, errcount}
});
}
}
void FETtecOneWireESC::update_send(const Aircraft &aircraft)
{
send_esc_telemetry(aircraft);
}
Reference in New Issue View Git Blame Copy Permalink