Collateral fixes

This commit is contained in:
Pavel Kirienko
2014-05-05 16:15:22 +04:00
parent 17ac1f7f6b
commit 8c8732d051
6 changed files with 17 additions and 8 deletions
@@ -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);