zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Periph: use native_millis for use in SITL

c415-sdk
bugobliterator 4 years ago committed by Andrew Tridgell
parent
09a0d8d0c0
commit
ed3683fce3
3 changed files with 30 additions and 30 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      Tools/AP_Periph/AP_Periph.cpp
  2. 52
      Tools/AP_Periph/can.cpp
  3. 2
      Tools/AP_Periph/hwing_esc.cpp

6
Tools/AP_Periph/AP_Periph.cpp
Unescape View File

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

52
Tools/AP_Periph/can.cpp
Unescape View File

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

2
Tools/AP_Periph/hwing_esc.cpp
Unescape View File

@ -39,7 +39,7 @@ bool HWESC_Telem::update()
} }
// we expect at least 50ms idle between frames // 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; bool frame_gap = (now - last_read_ms) > 10;
last_read_ms = now; last_read_ms = now;

Write Preview
Loading…
Cancel
Save