mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
AP_RCTelemetry: support CRSF parameter ping requests
process read and write parameter request frames add support for getting OSD parameters on read requests send text selections over CRSF chunker algorithm for CRSF text selections disable functions if OSD_PARAM_ENABLED is not set create separate parameter scheduler entry don't enable CRSF text selection on 1mb boards
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
7f3f135dc5
commit
06feecfaf2
@ -24,6 +24,8 @@
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Notify/AP_Notify.h>
|
||||
#include <AP_Common/AP_FWVersion.h>
|
||||
#include <AP_OSD/AP_OSD.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
@ -71,8 +73,9 @@ void AP_CRSF_Telem::setup_wfq_scheduler(void)
|
||||
|
||||
// CSRF telemetry rate is 150Hz (4ms) max, so these rates must fit
|
||||
add_scheduler_entry(50, 100); // heartbeat 10Hz
|
||||
add_scheduler_entry(200, 50); // parameters 20Hz (generally not active unless requested by the TX)
|
||||
add_scheduler_entry(50, 120); // Attitude and compass 8Hz
|
||||
add_scheduler_entry(200, 1000); // parameters 1Hz
|
||||
add_scheduler_entry(200, 1000); // VTX parameters 1Hz
|
||||
add_scheduler_entry(1300, 500); // battery 2Hz
|
||||
add_scheduler_entry(550, 280); // GPS 3Hz
|
||||
add_scheduler_entry(550, 500); // flight mode 2Hz
|
||||
@ -87,6 +90,8 @@ bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
|
||||
{
|
||||
switch (idx) {
|
||||
case PARAMETERS:
|
||||
return _request_pending > 0;
|
||||
case VTX_PARAMETERS:
|
||||
return AP::vtx().have_params_changed() ||_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending;
|
||||
default:
|
||||
return _enable_telemetry;
|
||||
@ -101,11 +106,14 @@ void AP_CRSF_Telem::process_packet(uint8_t idx)
|
||||
case HEARTBEAT: // HEARTBEAT
|
||||
calc_heartbeat();
|
||||
break;
|
||||
case PARAMETERS: // update parameter settings
|
||||
update_params();
|
||||
break;
|
||||
case ATTITUDE:
|
||||
calc_attitude();
|
||||
break;
|
||||
case PARAMETERS: // update various parameters
|
||||
update_params();
|
||||
case VTX_PARAMETERS: // update various VTX parameters
|
||||
update_vtx_params();
|
||||
break;
|
||||
case BATTERY: // BATTERY
|
||||
calc_battery();
|
||||
@ -139,6 +147,18 @@ bool AP_CRSF_Telem::_process_frame(AP_RCProtocol_CRSF::FrameType frame_type, voi
|
||||
process_vtx_telem_frame((VTXTelemetryFrame*)data);
|
||||
break;
|
||||
|
||||
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING:
|
||||
process_ping_frame((ParameterPingFrame*)data);
|
||||
break;
|
||||
|
||||
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ:
|
||||
process_param_read_frame((ParameterSettingsReadFrame*)data);
|
||||
break;
|
||||
|
||||
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_WRITE:
|
||||
process_param_write_frame((ParameterSettingsWriteFrame*)data);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@ -216,12 +236,51 @@ void AP_CRSF_Telem::process_vtx_telem_frame(VTXTelemetryFrame* vtx) {
|
||||
_vtx_power_change_pending = _vtx_freq_change_pending = _vtx_options_change_pending = false;
|
||||
}
|
||||
|
||||
// request for device info
|
||||
void AP_CRSF_Telem::process_ping_frame(ParameterPingFrame* ping)
|
||||
{
|
||||
debug("process_ping_frame: %d -> %d", ping->origin, ping->destination);
|
||||
if (ping->destination != 0 && ping->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
|
||||
return; // request was not for us
|
||||
}
|
||||
|
||||
_param_request.origin = ping->origin;
|
||||
_request_pending = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
|
||||
}
|
||||
|
||||
void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_frame)
|
||||
{
|
||||
//debug("process_param_read_frame: %d -> %d for %d[%d]", read_frame->origin, read_frame->destination,
|
||||
// read_frame->param_number, read_frame->param_chunk);
|
||||
if (read_frame->destination != 0 && read_frame->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
|
||||
return; // request was not for us
|
||||
}
|
||||
|
||||
_param_request = *read_frame;
|
||||
_request_pending = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ;
|
||||
}
|
||||
|
||||
// process any changed settings and schedule for transmission
|
||||
void AP_CRSF_Telem::update()
|
||||
{
|
||||
}
|
||||
|
||||
void AP_CRSF_Telem::update_params()
|
||||
{
|
||||
// handle general parameter requests
|
||||
switch (_request_pending) {
|
||||
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING:
|
||||
calc_device_info();
|
||||
break;
|
||||
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ:
|
||||
calc_parameter();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_CRSF_Telem::update_vtx_params()
|
||||
{
|
||||
AP_VideoTX& vtx = AP::vtx();
|
||||
|
||||
@ -406,6 +465,367 @@ void AP_CRSF_Telem::calc_flight_mode()
|
||||
}
|
||||
}
|
||||
|
||||
// return device information about ArduPilot
|
||||
void AP_CRSF_Telem::calc_device_info() {
|
||||
#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
|
||||
_telem.ext.info.destination = _param_request.origin;
|
||||
_telem.ext.info.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
|
||||
|
||||
const AP_FWVersion &fwver = AP::fwversion();
|
||||
// write out the name with version, max width is 60 - 18 = the meaning of life
|
||||
int32_t n = strlen(fwver.fw_string);
|
||||
strncpy((char*)_telem.ext.info.payload, fwver.fw_string, 41);
|
||||
n = MIN(n + 1, 42);
|
||||
|
||||
put_be32_ptr(&_telem.ext.info.payload[n], // serial number
|
||||
uint32_t(fwver.major) << 24 | uint32_t(fwver.minor) << 16 | uint32_t(fwver.patch) << 8 | uint32_t(fwver.fw_type));
|
||||
n += 4;
|
||||
put_be32_ptr(&_telem.ext.info.payload[n], // hardware id
|
||||
uint32_t(fwver.vehicle_type) << 24 | uint32_t(fwver.board_type) << 16 | uint32_t(fwver.board_subtype));
|
||||
n += 4;
|
||||
put_be32_ptr(&_telem.ext.info.payload[n], fwver.os_sw_version); // software id
|
||||
n += 4;
|
||||
#if OSD_PARAM_ENABLED
|
||||
_telem.ext.info.payload[n++] = AP_OSD_ParamScreen::NUM_PARAMS * AP_OSD_NUM_PARAM_SCREENS; // param count
|
||||
#else
|
||||
_telem.ext.info.payload[n++] = 0; // param count
|
||||
#endif
|
||||
_telem.ext.info.payload[n++] = 0; // param version
|
||||
|
||||
_telem_size = n + 2;
|
||||
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO;
|
||||
|
||||
_request_pending = 0;
|
||||
_telem_pending = true;
|
||||
#endif
|
||||
}
|
||||
|
||||
// return parameter information
|
||||
void AP_CRSF_Telem::calc_parameter() {
|
||||
#if OSD_PARAM_ENABLED
|
||||
AP_OSD* osd = AP::osd();
|
||||
|
||||
if (osd == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
_telem.ext.param_entry.header.destination = _param_request.origin;
|
||||
_telem.ext.param_entry.header.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
|
||||
|
||||
AP_OSD_ParamSetting* param = osd->get_setting((_param_request.param_num - 1) / AP_OSD_ParamScreen::NUM_PARAMS,
|
||||
(_param_request.param_num - 1) % AP_OSD_ParamScreen::NUM_PARAMS);
|
||||
|
||||
if (param == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
_telem.ext.param_entry.header.param_num = _param_request.param_num;
|
||||
#if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
|
||||
if (param->get_custom_metadata() != nullptr) {
|
||||
calc_text_selection(param, _param_request.param_chunk);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
size_t idx = 0;
|
||||
_telem.ext.param_entry.header.chunks_left = 0;
|
||||
_telem.ext.param_entry.payload[idx++] = 0; // parent folder
|
||||
idx++; // leave a gap for the type
|
||||
param->copy_name_camel_case((char*)&_telem.ext.param_entry.payload[idx], 17);
|
||||
idx += strnlen((char*)&_telem.ext.param_entry.payload[idx], 16) + 1;
|
||||
|
||||
switch (param->_param_type) {
|
||||
case AP_PARAM_INT8: {
|
||||
AP_Int8* p = (AP_Int8*)param->_param;
|
||||
_telem.ext.param_entry.payload[1] = ParameterType::INT8;
|
||||
_telem.ext.param_entry.payload[idx] = p->get(); // value
|
||||
_telem.ext.param_entry.payload[idx+1] = int8_t(param->_param_min); // min
|
||||
_telem.ext.param_entry.payload[idx+2] = int8_t(param->_param_max); // max
|
||||
_telem.ext.param_entry.payload[idx+3] = int8_t(0); // default
|
||||
idx += 4;
|
||||
break;
|
||||
}
|
||||
case AP_PARAM_INT16: {
|
||||
AP_Int16* p = (AP_Int16*)param->_param;
|
||||
_telem.ext.param_entry.payload[1] = ParameterType::INT16;
|
||||
put_be16_ptr(&_telem.ext.param_entry.payload[idx], p->get()); // value
|
||||
put_be16_ptr(&_telem.ext.param_entry.payload[idx+2], param->_param_min); // min
|
||||
put_be16_ptr(&_telem.ext.param_entry.payload[idx+4], param->_param_max); // max
|
||||
put_be16_ptr(&_telem.ext.param_entry.payload[idx+6], 0); // default
|
||||
idx += 8;
|
||||
break;
|
||||
}
|
||||
case AP_PARAM_INT32: {
|
||||
AP_Int32* p = (AP_Int32*)param->_param;
|
||||
_telem.ext.param_entry.payload[1] = ParameterType::FLOAT;
|
||||
#define FLOAT_ENCODE(f) (int32_t(roundf(f)))
|
||||
put_be32_ptr(&_telem.ext.param_entry.payload[idx], p->get()); // value
|
||||
put_be32_ptr(&_telem.ext.param_entry.payload[idx+4], FLOAT_ENCODE(param->_param_min)); // min
|
||||
put_be32_ptr(&_telem.ext.param_entry.payload[idx+8], FLOAT_ENCODE(param->_param_max)); // max
|
||||
put_be32_ptr(&_telem.ext.param_entry.payload[idx+12], FLOAT_ENCODE(0.0f)); // default
|
||||
#undef FLOAT_ENCODE
|
||||
_telem.ext.param_entry.payload[idx+16] = 0; // decimal point
|
||||
put_be32_ptr(&_telem.ext.param_entry.payload[idx+17], 1); // step size
|
||||
idx += 21;
|
||||
break;
|
||||
}
|
||||
case AP_PARAM_FLOAT: {
|
||||
AP_Float* p = (AP_Float*)param->_param;
|
||||
_telem.ext.param_entry.payload[1] = ParameterType::FLOAT;
|
||||
uint8_t digits = 0;
|
||||
const float incr = MAX(0.001f, param->_param_incr); // a bug in OpenTX prevents this going any smaller
|
||||
|
||||
for (float floatp = incr; floatp < 1.0f; floatp *= 10) {
|
||||
digits++;
|
||||
}
|
||||
const float mult = powf(10, digits);
|
||||
#define FLOAT_ENCODE(f) (int32_t(roundf(mult * f)))
|
||||
put_be32_ptr(&_telem.ext.param_entry.payload[idx], FLOAT_ENCODE(p->get())); // value
|
||||
put_be32_ptr(&_telem.ext.param_entry.payload[idx+4], FLOAT_ENCODE(param->_param_min)); // min
|
||||
put_be32_ptr(&_telem.ext.param_entry.payload[idx+8], FLOAT_ENCODE(param->_param_max)); // max
|
||||
put_be32_ptr(&_telem.ext.param_entry.payload[idx+12], FLOAT_ENCODE(0.0f)); // default
|
||||
_telem.ext.param_entry.payload[idx+16] = digits; // decimal point
|
||||
put_be32_ptr(&_telem.ext.param_entry.payload[idx+17], FLOAT_ENCODE(incr)); // step size
|
||||
#undef FLOAT_ENCODE
|
||||
//debug("Encoding param %f(%f -> %f, %f) as %d(%d) (%d -> %d, %d)", p->get(),
|
||||
// param->_param_min.get(), param->_param_max.get(), param->_param_incr.get(),
|
||||
// int(FLOAT_ENCODE(p->get())), digits, int(FLOAT_ENCODE(param->_param_min)),
|
||||
// int(FLOAT_ENCODE(param->_param_max)), int(FLOAT_ENCODE(param->_param_incr)));
|
||||
idx += 21;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
return;
|
||||
}
|
||||
_telem.ext.param_entry.payload[idx] = 0; // units
|
||||
|
||||
_telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + 1 + idx;
|
||||
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
|
||||
|
||||
_request_pending = 0;
|
||||
_telem_pending = true;
|
||||
#endif
|
||||
}
|
||||
|
||||
#if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
|
||||
// class that spits out a chunk of data from a larger stream of contiguous chunks
|
||||
// the caller describes which chunk it needs and provides this class with all of the data
|
||||
// data is not written until the start position is reached and after a whole chunk
|
||||
// is accumulated the rest of the data is skipped in order to determine how many chunks
|
||||
// are left to be sent
|
||||
class BufferChunker {
|
||||
public:
|
||||
BufferChunker(uint8_t* buf, uint16_t chunk_size, uint16_t start_chunk) :
|
||||
_buf(buf), _idx(0), _start_chunk(start_chunk), _chunk_size(chunk_size), _chunk(0), _bytes(0) {
|
||||
}
|
||||
|
||||
// accumulate a string, writing to the underlying buffer as required
|
||||
void put_string(const char* str, uint16_t str_len) {
|
||||
// skip over data we have already written or have yet to write
|
||||
if (_chunk != _start_chunk) {
|
||||
if (skip_bytes(str_len)) {
|
||||
// partial write
|
||||
strncpy((char*)_buf, &str[str_len - _idx], _idx);
|
||||
_bytes += _idx;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
uint16_t rem = remaining();
|
||||
if (rem > str_len) {
|
||||
strncpy((char*)&_buf[_idx], str, str_len);
|
||||
_idx += str_len;
|
||||
_bytes += str_len;
|
||||
} else {
|
||||
strncpy((char*)&_buf[_idx], str, rem);
|
||||
_chunk++;
|
||||
_idx += str_len;
|
||||
_bytes += rem;
|
||||
_idx %= _chunk_size;
|
||||
}
|
||||
}
|
||||
|
||||
// accumulate a byte, writing to the underlying buffer as required
|
||||
void put_byte(uint8_t b) {
|
||||
if (_chunk != _start_chunk) {
|
||||
if (skip_bytes(1)) {
|
||||
_buf[0] = b;
|
||||
_bytes++;
|
||||
}
|
||||
return;
|
||||
}
|
||||
if (remaining() > 0) {
|
||||
_buf[_idx++] = b;
|
||||
_bytes++;
|
||||
} else {
|
||||
_chunk++;
|
||||
_idx = 0;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t chunks_remaining() const { return _chunk - _start_chunk; }
|
||||
uint8_t bytes_written() const { return _bytes; }
|
||||
|
||||
private:
|
||||
uint16_t remaining() const { return _chunk_size - _bytes; }
|
||||
|
||||
// skip over the requested number of bytes
|
||||
// returns true if we overflow into a chunk that needs to be written
|
||||
bool skip_bytes(uint16_t len) {
|
||||
_idx += len;
|
||||
if (_idx >= _chunk_size) {
|
||||
_chunk++;
|
||||
_idx %= _chunk_size;
|
||||
// partial write
|
||||
if (_chunk == _start_chunk && _idx > 0) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t* _buf;
|
||||
uint16_t _idx;
|
||||
uint16_t _bytes;
|
||||
uint8_t _chunk;
|
||||
const uint16_t _start_chunk;
|
||||
const uint16_t _chunk_size;
|
||||
};
|
||||
|
||||
// provide information about a text selection, possibly over multiple chunks
|
||||
void AP_CRSF_Telem::calc_text_selection(AP_OSD_ParamSetting* param, uint8_t chunk)
|
||||
{
|
||||
const uint8_t CHUNK_SIZE = 56;
|
||||
const AP_OSD_ParamSetting::ParamMetadata* metadata = param->get_custom_metadata();
|
||||
|
||||
// chunk the output
|
||||
BufferChunker chunker(_telem.ext.param_entry.payload, CHUNK_SIZE, chunk);
|
||||
|
||||
chunker.put_byte(0); // parent folder
|
||||
chunker.put_byte(ParameterType::TEXT_SELECTION); // parameter type
|
||||
|
||||
char name[17];
|
||||
param->copy_name_camel_case(name, 17);
|
||||
chunker.put_string(name, strnlen(name, 16)); // parameter name
|
||||
chunker.put_byte(0); // trailing null
|
||||
|
||||
for (uint8_t i = 0; i < metadata->values_max; i++) {
|
||||
uint8_t len = strnlen(metadata->values[i], 16);
|
||||
if (len == 0) {
|
||||
chunker.put_string("---", 3);
|
||||
} else {
|
||||
chunker.put_string(metadata->values[i], len);
|
||||
}
|
||||
if (i == metadata->values_max - 1) {
|
||||
chunker.put_byte(0);
|
||||
} else {
|
||||
chunker.put_byte(';');
|
||||
}
|
||||
}
|
||||
|
||||
int32_t val = -1;
|
||||
switch (param->_param_type) {
|
||||
case AP_PARAM_INT8:
|
||||
val = ((AP_Int8*)param->_param)->get();
|
||||
break;
|
||||
case AP_PARAM_INT16:
|
||||
val = ((AP_Int16*)param->_param)->get();
|
||||
break;
|
||||
case AP_PARAM_INT32:
|
||||
val = ((AP_Int32*)param->_param)->get();
|
||||
break;
|
||||
default:
|
||||
return;
|
||||
}
|
||||
|
||||
// out of range values really confuse the TX
|
||||
val = constrain_int16(val, 0, metadata->values_max - 1);
|
||||
chunker.put_byte(val); // value
|
||||
chunker.put_byte(0); // min
|
||||
chunker.put_byte(metadata->values_max); // max
|
||||
chunker.put_byte(0); // default
|
||||
chunker.put_byte(0); // units
|
||||
|
||||
_telem.ext.param_entry.header.chunks_left = chunker.chunks_remaining();
|
||||
|
||||
_telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + chunker.bytes_written();
|
||||
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
|
||||
|
||||
_request_pending = 0;
|
||||
_telem_pending = true;
|
||||
}
|
||||
#endif
|
||||
|
||||
// write parameter information back into AP - assumes we already know the encoding for floats
|
||||
void AP_CRSF_Telem::process_param_write_frame(ParameterSettingsWriteFrame* write_frame)
|
||||
{
|
||||
debug("process_param_write_frame: %d -> %d", write_frame->origin, write_frame->destination);
|
||||
if (write_frame->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
|
||||
return; // request was not for us
|
||||
}
|
||||
#if OSD_PARAM_ENABLED
|
||||
AP_OSD* osd = AP::osd();
|
||||
|
||||
if (osd == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
AP_OSD_ParamSetting* param = osd->get_setting((write_frame->param_num - 1) / AP_OSD_ParamScreen::NUM_PARAMS,
|
||||
(write_frame->param_num - 1) % AP_OSD_ParamScreen::NUM_PARAMS);
|
||||
|
||||
if (param == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
#if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
|
||||
bool text_selection = param->get_custom_metadata() != nullptr;
|
||||
#else
|
||||
bool text_selection = false;
|
||||
#endif
|
||||
|
||||
switch (param->_param_type) {
|
||||
case AP_PARAM_INT8: {
|
||||
AP_Int8* p = (AP_Int8*)param->_param;
|
||||
p->set_and_save(write_frame->payload[0]);
|
||||
break;
|
||||
}
|
||||
case AP_PARAM_INT16: {
|
||||
AP_Int16* p = (AP_Int16*)param->_param;
|
||||
if (text_selection) {
|
||||
// if we have custom metadata then the parameter is a text selection
|
||||
p->set_and_save(write_frame->payload[0]);
|
||||
} else {
|
||||
p->set_and_save(be16toh_ptr(write_frame->payload));
|
||||
}
|
||||
break;
|
||||
}
|
||||
case AP_PARAM_INT32: {
|
||||
AP_Int32* p = (AP_Int32*)param->_param;
|
||||
if (text_selection) {
|
||||
// if we have custom metadata then the parameter is a text selection
|
||||
p->set_and_save(write_frame->payload[0]);
|
||||
} else {
|
||||
p->set_and_save(be32toh_ptr(write_frame->payload));
|
||||
}
|
||||
break;
|
||||
}
|
||||
case AP_PARAM_FLOAT: {
|
||||
AP_Float* p = (AP_Float*)param->_param;
|
||||
const int32_t val = be32toh_ptr(write_frame->payload);
|
||||
uint8_t digits = 0;
|
||||
const float incr = MAX(0.001f, param->_param_incr); // a bug in OpenTX prevents this going any smaller
|
||||
|
||||
for (float floatp = incr; floatp < 1.0f; floatp *= 10) {
|
||||
digits++;
|
||||
}
|
||||
p->set_and_save(float(val) / powf(10, digits));
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
fetch CRSF frame data
|
||||
*/
|
||||
|
||||
@ -15,11 +15,16 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
|
||||
#ifndef HAL_CRSF_TELEM_ENABLED
|
||||
#define HAL_CRSF_TELEM_ENABLED !HAL_MINIMIZE_FEATURES
|
||||
#endif
|
||||
|
||||
#ifndef HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
|
||||
#define HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED HAL_CRSF_TELEM_ENABLED && BOARD_FLASH_SIZE > 1024
|
||||
#endif
|
||||
|
||||
#if HAL_CRSF_TELEM_ENABLED
|
||||
|
||||
#include <AP_Notify/AP_Notify.h>
|
||||
@ -29,6 +34,8 @@
|
||||
#include "AP_RCTelemetry.h"
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
|
||||
class AP_OSD_ParamSetting;
|
||||
|
||||
class AP_CRSF_Telem : public AP_RCTelemetry {
|
||||
public:
|
||||
AP_CRSF_Telem();
|
||||
@ -114,6 +121,58 @@ public:
|
||||
uint8_t origin;
|
||||
} PACKED;
|
||||
|
||||
// CRSF_FRAMETYPE_PARAM_DEVICE_INFO
|
||||
struct ParameterDeviceInfoFrame {
|
||||
uint8_t destination;
|
||||
uint8_t origin;
|
||||
uint8_t payload[58]; // largest possible frame is 60
|
||||
} PACKED;
|
||||
|
||||
enum ParameterType : uint8_t
|
||||
{
|
||||
UINT8 = 0,
|
||||
INT8 = 1,
|
||||
UINT16 = 2,
|
||||
INT16 = 3,
|
||||
FLOAT = 8,
|
||||
TEXT_SELECTION = 9,
|
||||
STRING = 10,
|
||||
FOLDER = 11,
|
||||
INFO = 12,
|
||||
COMMAND = 13,
|
||||
OUT_OF_RANGE = 127
|
||||
};
|
||||
|
||||
// CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY
|
||||
struct ParameterSettingsEntryHeader {
|
||||
uint8_t destination;
|
||||
uint8_t origin;
|
||||
uint8_t param_num;
|
||||
uint8_t chunks_left;
|
||||
} PACKED;
|
||||
|
||||
// CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY
|
||||
struct ParameterSettingsEntry {
|
||||
ParameterSettingsEntryHeader header;
|
||||
uint8_t payload[56]; // largest possible frame is 60
|
||||
} PACKED;
|
||||
|
||||
// CRSF_FRAMETYPE_PARAMETER_READ
|
||||
struct ParameterSettingsReadFrame {
|
||||
uint8_t destination;
|
||||
uint8_t origin;
|
||||
uint8_t param_num;
|
||||
uint8_t param_chunk;
|
||||
} PACKED _param_request;
|
||||
|
||||
// CRSF_FRAMETYPE_PARAMETER_WRITE
|
||||
struct ParameterSettingsWriteFrame {
|
||||
uint8_t destination;
|
||||
uint8_t origin;
|
||||
uint8_t param_num;
|
||||
uint8_t payload[57]; // largest possible frame is 60
|
||||
} PACKED;
|
||||
|
||||
union BroadcastFrame {
|
||||
GPSFrame gps;
|
||||
HeartbeatFrame heartbeat;
|
||||
@ -126,6 +185,10 @@ public:
|
||||
union ExtendedFrame {
|
||||
CommandFrame command;
|
||||
ParameterPingFrame ping;
|
||||
ParameterDeviceInfoFrame info;
|
||||
ParameterSettingsEntry param_entry;
|
||||
ParameterSettingsReadFrame param_read;
|
||||
ParameterSettingsWriteFrame param_write;
|
||||
} PACKED;
|
||||
|
||||
union TelemetryPayload {
|
||||
@ -144,8 +207,9 @@ private:
|
||||
|
||||
enum SensorType {
|
||||
HEARTBEAT,
|
||||
ATTITUDE,
|
||||
PARAMETERS,
|
||||
ATTITUDE,
|
||||
VTX_PARAMETERS,
|
||||
BATTERY,
|
||||
GPS,
|
||||
FLIGHT_MODE,
|
||||
@ -163,10 +227,19 @@ private:
|
||||
void calc_gps();
|
||||
void calc_attitude();
|
||||
void calc_flight_mode();
|
||||
void calc_device_info();
|
||||
void calc_parameter();
|
||||
#if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
|
||||
void calc_text_selection( AP_OSD_ParamSetting* param, uint8_t chunk);
|
||||
#endif
|
||||
void update_params();
|
||||
void update_vtx_params();
|
||||
|
||||
void process_vtx_frame(VTXFrame* vtx);
|
||||
void process_vtx_telem_frame(VTXTelemetryFrame* vtx);
|
||||
void process_ping_frame(ParameterPingFrame* ping);
|
||||
void process_param_read_frame(ParameterSettingsReadFrame* read);
|
||||
void process_param_write_frame(ParameterSettingsWriteFrame* write);
|
||||
|
||||
// setup ready for passthrough operation
|
||||
void setup_wfq_scheduler(void) override;
|
||||
@ -181,6 +254,7 @@ private:
|
||||
|
||||
bool _telem_pending;
|
||||
bool _enable_telemetry;
|
||||
uint8_t _request_pending;
|
||||
|
||||
// vtx state
|
||||
bool _vtx_freq_update; // update using the frequency method or not
|
||||
|
||||
Loading…
Reference in New Issue
Block a user