5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 15:38:29 -04:00

AP_Periph: use native_millis for use in SITL

This commit is contained in:
bugobliterator 2020-09-13 00:27:27 +05:30 committed by Andrew Tridgell
parent 09a0d8d0c0
commit ed3683fce3
3 changed files with 30 additions and 30 deletions

View File

@ -157,7 +157,7 @@ void AP_Periph_FW::init()
msp_init(hal.uartD);
#endif
start_ms = AP_HAL::millis();
start_ms = AP_HAL::native_millis();
}
#if defined(HAL_PERIPH_NEOPIXEL_COUNT) && HAL_PERIPH_NEOPIXEL_COUNT == 8
@ -170,7 +170,7 @@ static void update_rainbow()
if (rainbow_done) {
return;
}
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
if (now-start_ms > 1500) {
rainbow_done = true;
hal.rcout->set_serial_led_rgb_data(HAL_PERIPH_NEOPIXEL_CHAN, -1, 0, 0, 0);
@ -217,7 +217,7 @@ static void update_rainbow()
void AP_Periph_FW::update()
{
static uint32_t last_led_ms;
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
if (now - last_led_ms > 1000) {
last_led_ms = now;
#ifdef HAL_GPIO_PIN_LED

View File

@ -132,7 +132,7 @@ static void handle_get_node_info(CanardInstance* ins,
uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE] {};
uavcan_protocol_GetNodeInfoResponse pkt {};
node_status.uptime_sec = AP_HAL::millis() / 1000U;
node_status.uptime_sec = AP_HAL::native_millis() / 1000U;
pkt.status = node_status;
pkt.software_version.major = AP::fwversion().major;
@ -384,7 +384,7 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr
{
// Rule C - updating the randomized time interval
send_next_node_id_allocation_request_at_ms =
AP_HAL::millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
AP_HAL::native_millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);
if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID)
@ -462,7 +462,7 @@ static void handle_beep_command(CanardInstance* ins, CanardRxTransfer* transfer)
}
fix_float16(req.frequency);
fix_float16(req.duration);
buzzer_start_ms = AP_HAL::millis();
buzzer_start_ms = AP_HAL::native_millis();
buzzer_len_ms = req.duration*1000;
float volume = constrain_float(periph.g.buzz_volume/100.0, 0, 1);
hal.util->toneAlarm_set_buzzer_tone(req.frequency, volume, uint32_t(req.duration*1000));
@ -474,7 +474,7 @@ static void handle_beep_command(CanardInstance* ins, CanardRxTransfer* transfer)
static void can_buzzer_update(void)
{
if (buzzer_start_ms != 0) {
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
if (now - buzzer_start_ms > buzzer_len_ms) {
hal.util->toneAlarm_set_buzzer_tone(0, 0, 0);
buzzer_start_ms = 0;
@ -585,7 +585,7 @@ static void can_safety_LED_update(void)
palWriteLine(HAL_GPIO_PIN_SAFE_LED, SAFE_LED_ON);
break;
case ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_ON: {
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
if (now - last_update_ms > 100) {
last_update_ms = now;
static uint8_t led_counter;
@ -614,7 +614,7 @@ static void can_safety_button_update(void)
{
static uint32_t last_update_ms;
static uint8_t counter;
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
// send at 10Hz when pressed
if (palReadLine(HAL_GPIO_PIN_SAFE_BUTTON) != HAL_SAFE_BUTTON_ON) {
counter = 0;
@ -801,8 +801,8 @@ static void processTx(void)
txmsg.dlc = txf->data_len;
memcpy(txmsg.data, txf->data, 8);
txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF);
// push message with 1s timeout
if (can_iface.send(txmsg, AP_HAL::micros64() + 1000000, 0) > 0) {
// push message with 1s timeout
if (can_iface.send(txmsg, AP_HAL::native_micros64() + 1000000, 0) > 0) {
canardPopTxQueue(&canard);
fail_count = 0;
} else {
@ -876,7 +876,7 @@ static void process1HzTasks(uint64_t timestamp_usec)
*/
{
uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE];
node_status.uptime_sec = AP_HAL::millis() / 1000U;
node_status.uptime_sec = AP_HAL::native_millis() / 1000U;
node_status.vendor_specific_status_code = hal.util->available_memory();
@ -919,12 +919,12 @@ static void process1HzTasks(uint64_t timestamp_usec)
#if 0
// test code for watchdog reset
if (AP_HAL::millis() > 15000) {
if (AP_HAL::native_millis() > 15000) {
while (true) ;
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
if (AP_HAL::millis() > 30000) {
if (AP_HAL::native_millis() > 30000) {
// use RTC to mark that we have been running fine for
// 30s. This is used along with watchdog resets to ensure the
// user has a chance to load a fixed firmware
@ -941,7 +941,7 @@ static void can_wait_node_id(void)
uint8_t node_id_allocation_transfer_id = 0;
const uint32_t led_pattern = 0xAAAA;
uint8_t led_idx = 0;
uint32_t last_led_change = AP_HAL::millis();
uint32_t last_led_change = AP_HAL::native_millis();
const uint32_t led_change_period = 50;
while (canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID)
@ -949,18 +949,18 @@ static void can_wait_node_id(void)
printf("Waiting for dynamic node ID allocation... (pool %u)\n", pool_peak_percent());
stm32_watchdog_pat();
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
send_next_node_id_allocation_request_at_ms =
now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);
while (((now=AP_HAL::millis()) < send_next_node_id_allocation_request_at_ms) &&
while (((now=AP_HAL::native_millis()) < send_next_node_id_allocation_request_at_ms) &&
(canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID))
{
processTx();
processRx();
canardCleanupStaleTransfers(&canard, AP_HAL::micros64());
canardCleanupStaleTransfers(&canard, AP_HAL::native_micros64());
stm32_watchdog_pat();
if (now - last_led_change > led_change_period) {
@ -1034,7 +1034,7 @@ void AP_Periph_FW::can_start()
{
node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION;
node_status.uptime_sec = AP_HAL::millis() / 1000U;
node_status.uptime_sec = AP_HAL::native_millis() / 1000U;
if (g.can_node >= 0 && g.can_node < 128) {
PreferredNodeID = g.can_node;
@ -1082,7 +1082,7 @@ void AP_Periph_FW::pwm_irq_handler(uint8_t pin, bool pin_state, uint32_t timesta
void AP_Periph_FW::pwm_hardpoint_update()
{
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
// send at 10Hz
void *save = hal.scheduler->disable_interrupts_save();
uint16_t value = pwm_hardpoint.highest_pwm;
@ -1146,10 +1146,10 @@ void AP_Periph_FW::hwesc_telem_update()
void AP_Periph_FW::can_update()
{
static uint32_t last_1Hz_ms;
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
if (now - last_1Hz_ms >= 1000) {
last_1Hz_ms = now;
process1HzTasks(AP_HAL::micros64());
process1HzTasks(AP_HAL::native_micros64());
}
can_mag_update();
can_gps_update();
@ -1192,7 +1192,7 @@ void AP_Periph_FW::can_mag_update(void)
#if CAN_PROBE_CONTINUOUS
if (compass.get_count() == 0) {
static uint32_t last_probe_ms;
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
if (now - last_probe_ms >= 1000) {
last_probe_ms = now;
compass.init();
@ -1250,7 +1250,7 @@ void AP_Periph_FW::can_gps_update(void)
const Location &loc = gps.location();
const Vector3f &vel = gps.velocity();
pkt.timestamp.usec = AP_HAL::micros64();
pkt.timestamp.usec = AP_HAL::native_micros64();
pkt.gnss_timestamp.usec = gps.time_epoch_usec();
if (pkt.gnss_timestamp.usec == 0) {
pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_NONE;
@ -1333,7 +1333,7 @@ void AP_Periph_FW::can_gps_update(void)
const Location &loc = gps.location();
const Vector3f &vel = gps.velocity();
pkt.timestamp.usec = AP_HAL::micros64();
pkt.timestamp.usec = AP_HAL::native_micros64();
pkt.gnss_timestamp.usec = gps.time_epoch_usec();
if (pkt.gnss_timestamp.usec == 0) {
pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_NONE;
@ -1515,7 +1515,7 @@ void AP_Periph_FW::can_airspeed_update(void)
}
#if CAN_PROBE_CONTINUOUS
if (!airspeed.healthy()) {
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
static uint32_t last_probe_ms;
if (now - last_probe_ms >= 1000) {
last_probe_ms = now;
@ -1523,7 +1523,7 @@ void AP_Periph_FW::can_airspeed_update(void)
}
}
#endif
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
if (now - last_airspeed_update_ms < 50) {
// max 20Hz data
return;
@ -1579,7 +1579,7 @@ void AP_Periph_FW::can_rangefinder_update(void)
}
#if CAN_PROBE_CONTINUOUS
if (rangefinder.num_sensors() == 0) {
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
static uint32_t last_probe_ms;
if (now - last_probe_ms >= 1000) {
last_probe_ms = now;
@ -1587,7 +1587,7 @@ void AP_Periph_FW::can_rangefinder_update(void)
}
}
#endif
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
static uint32_t last_update_ms;
if (now - last_update_ms < 20) {
// max 50Hz data

View File

@ -39,7 +39,7 @@ bool HWESC_Telem::update()
}
// we expect at least 50ms idle between frames
uint32_t now = AP_HAL::millis();
uint32_t now = AP_HAL::native_millis();
bool frame_gap = (now - last_read_ms) > 10;
last_read_ms = now;