mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 02:13:35 +08:00
Collateral fixes
This commit is contained in:
@@ -7,6 +7,7 @@
|
||||
#include <cassert>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <new>
|
||||
#include <uavcan/stdint.hpp>
|
||||
#include <uavcan/util/templates.hpp>
|
||||
#include <uavcan/impl_constants.hpp>
|
||||
|
||||
@@ -5,6 +5,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdlib>
|
||||
#include <new>
|
||||
#include <uavcan/error.hpp>
|
||||
#include <uavcan/impl_constants.hpp>
|
||||
#include <uavcan/util/templates.hpp>
|
||||
|
||||
@@ -5,6 +5,7 @@
|
||||
#include <uavcan/transport/transfer_buffer.hpp>
|
||||
#include <cassert>
|
||||
#include <cstdlib>
|
||||
#include <new>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
|
||||
#include <uavcan_lpc11c24/can.hpp>
|
||||
#include <uavcan_lpc11c24/clock.hpp>
|
||||
#include <uavcan/util/templates.hpp>
|
||||
#include <chip.h>
|
||||
#include "internal.hpp"
|
||||
|
||||
@@ -254,7 +255,7 @@ uavcan::int16_t CanDriver::send(const uavcan::CanFrame& frame, uavcan::Monotonic
|
||||
msgobj.mode_id |= CAN_MSGOBJ_RTR;
|
||||
}
|
||||
msgobj.dlc = frame.dlc;
|
||||
std::copy(frame.data, frame.data + frame.dlc, msgobj.data);
|
||||
uavcan::copy(frame.data, frame.data + frame.dlc, msgobj.data);
|
||||
|
||||
/*
|
||||
* Transmission
|
||||
@@ -387,7 +388,7 @@ void canRxCallback(uint8_t msg_obj_num)
|
||||
|
||||
// Payload
|
||||
frame.dlc = msg_obj.dlc;
|
||||
std::copy(msg_obj.data, msg_obj.data + msg_obj.dlc, frame.data);
|
||||
uavcan::copy(msg_obj.data, msg_obj.data + msg_obj.dlc, frame.data);
|
||||
|
||||
uavcan_lpc11c24::rx_queue.push(frame, uavcan_lpc11c24::last_irq_utc_timestamp);
|
||||
uavcan_lpc11c24::had_activity = true;
|
||||
|
||||
@@ -3,6 +3,7 @@
|
||||
*/
|
||||
|
||||
#include <uavcan_lpc11c24/clock.hpp>
|
||||
#include <uavcan/util/templates.hpp>
|
||||
#include <chip.h>
|
||||
#include "internal.hpp"
|
||||
|
||||
@@ -113,8 +114,10 @@ void adjustUtc(uavcan::UtcDuration adjustment)
|
||||
utc_correction_usec_per_overflow_x16 += adjustment.isPositive() ? 1 : -1; // I
|
||||
utc_correction_usec_per_overflow_x16 += (adj_delta > 0) ? 1 : -1; // P
|
||||
|
||||
utc_correction_usec_per_overflow_x16 = std::max(utc_correction_usec_per_overflow_x16, -MaxUtcSpeedCorrectionX16);
|
||||
utc_correction_usec_per_overflow_x16 = std::min(utc_correction_usec_per_overflow_x16, MaxUtcSpeedCorrectionX16);
|
||||
utc_correction_usec_per_overflow_x16 =
|
||||
uavcan::max(utc_correction_usec_per_overflow_x16, -MaxUtcSpeedCorrectionX16);
|
||||
utc_correction_usec_per_overflow_x16 =
|
||||
uavcan::min(utc_correction_usec_per_overflow_x16, MaxUtcSpeedCorrectionX16);
|
||||
|
||||
if (adjustment.getAbs().toMSec() > 9 || !utc_set)
|
||||
{
|
||||
|
||||
@@ -180,16 +180,18 @@ static void updateRatePID(uavcan::UtcDuration adjustment)
|
||||
else
|
||||
{
|
||||
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
|
||||
utc_rel_rate_error_integral = std::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral = std::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
}
|
||||
|
||||
/*
|
||||
* Rate controller
|
||||
*/
|
||||
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = std::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = std::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
|
||||
utc_correction_nsec_per_overflow = (USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user