Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: added local millis() and micros() to reduce code size a bit

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-14 08:05:32 +10:00
parent 150af130ee
commit b92c2409e4
17 changed files with 67 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/ArduPlane.cpp
View File

@ -104,7 +104,7 @@ void Plane::loop()
// wait for an INS sample
ins.wait_for_sample();
uint32_t timer = hal.scheduler->micros();
uint32_t timer = micros();
delta_us_fast_loop = timer - fast_loopTimer_us;
G_Dt = delta_us_fast_loop * 1.0e-6f;
@ -128,7 +128,7 @@ void Plane::loop()
// in multiples of the main loop tick. So if they don't run on
// the first call to the scheduler they won't run on a later
// call until scheduler.tick() is called again
uint32_t remaining = (timer + 20000) - hal.scheduler->micros();
uint32_t remaining = (timer + 20000) - micros();
if (remaining > 19500) {
remaining = 19500;
}
@ -869,7 +869,7 @@ void Plane::determine_is_flying(void)
been flying for. This helps for crash detection and auto-disarm
*/
if (is_flying()) {
auto_state.last_flying_ms = hal.scheduler->millis();
auto_state.last_flying_ms = millis();
}
}

22
ArduPlane/GCS_Mavlink.cpp
View File

@ -93,7 +93,7 @@ void Plane::send_attitude(mavlink_channel_t chan)
Vector3f omega = ahrs.get_gyro();
mavlink_msg_attitude_send(
chan,
hal.scheduler->millis(),
millis(),
ahrs.roll,
ahrs.pitch - radians(g.pitch_trim_cd*0.01f),
ahrs.yaw,
@ -307,7 +307,7 @@ void Plane::send_location(mavlink_channel_t chan)
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
fix_time_ms = gps.last_fix_time_ms();
} else {
fix_time_ms = hal.scheduler->millis();
fix_time_ms = millis();
}
const Vector3f &vel = gps.velocity();
mavlink_msg_global_position_int_send(
@ -345,7 +345,7 @@ void Plane::send_servo_out(mavlink_channel_t chan)
// HIL maintainers
mavlink_msg_rc_channels_scaled_send(
chan,
hal.scheduler->millis(),
millis(),
0, // port 0
10000 * channel_roll->norm_output() * (channel_roll->get_reverse()?-1:1),
10000 * channel_pitch->norm_output() * (channel_pitch->get_reverse()?-1:1),
@ -363,7 +363,7 @@ void Plane::send_radio_out(mavlink_channel_t chan)
if (g.hil_mode==1 && !g.hil_servos) {
mavlink_msg_servo_output_raw_send(
chan,
hal.scheduler->micros(),
micros(),
0, // port
RC_Channel::rc_channel(0)->radio_out,
RC_Channel::rc_channel(1)->radio_out,
@ -377,7 +377,7 @@ void Plane::send_radio_out(mavlink_channel_t chan)
}
mavlink_msg_servo_output_raw_send(
chan,
hal.scheduler->micros(),
micros(),
0, // port
hal.rcout->read(0),
hal.rcout->read(1),
@ -483,7 +483,7 @@ void Plane::send_statustext(mavlink_channel_t chan)
// are we still delaying telemetry to try to avoid Xbee bricking?
bool Plane::telemetry_delayed(mavlink_channel_t chan)
{
uint32_t tnow = hal.scheduler->millis() >> 10;
uint32_t tnow = millis() >> 10;
if (tnow > (uint32_t)g.telem_delay) {
return false;
}
@ -520,7 +520,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
switch (id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
plane.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = hal.scheduler->millis();
plane.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = plane.millis();
plane.send_heartbeat(chan);
return true;
@ -1470,12 +1470,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
v[7] = packet.chan8_raw;
if (hal.rcin->set_overrides(v, 8)) {
plane.failsafe.last_valid_rc_ms = hal.scheduler->millis();
plane.failsafe.last_valid_rc_ms = plane.millis();
}
// a RC override message is consiered to be a 'heartbeat' from
// the ground station for failsafe purposes
plane.failsafe.last_heartbeat_ms = hal.scheduler->millis();
plane.failsafe.last_heartbeat_ms = plane.millis();
break;
}
@ -1484,7 +1484,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// We keep track of the last time we received a heartbeat from
// our GCS for failsafe purposes
if (msg->sysid != plane.g.sysid_my_gcs) break;
plane.failsafe.last_heartbeat_ms = hal.scheduler->millis();
plane.failsafe.last_heartbeat_ms = plane.millis();
break;
}
@ -1632,7 +1632,7 @@ void Plane::mavlink_delay_cb()
in_mavlink_delay = true;
uint32_t tnow = hal.scheduler->millis();
uint32_t tnow = millis();
if (tnow - last_1hz > 1000) {
last_1hz = tnow;
gcs_send_message(MSG_HEARTBEAT);

14
ArduPlane/Log.cpp
View File

@ -201,7 +201,7 @@ void Plane::Log_Write_Performance()
{
struct log_Performance pkt = {
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
loop_time : hal.scheduler->millis() - perf_mon_timer,
loop_time : millis() - perf_mon_timer,
main_loop_count : mainLoop_count,
g_dt_max : G_Dt_max,
gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000),
@ -270,7 +270,7 @@ void Plane::Log_Write_Control_Tuning()
Vector3f accel = ins.get_accel();
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
time_ms : hal.scheduler->millis(),
time_ms : millis(),
nav_roll_cd : (int16_t)nav_roll_cd,
roll : (int16_t)ahrs.roll_sensor,
nav_pitch_cd : (int16_t)nav_pitch_cd,
@ -306,7 +306,7 @@ void Plane::Log_Write_Nav_Tuning()
{
struct log_Nav_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
time_ms : hal.scheduler->millis(),
time_ms : millis(),
yaw : (uint16_t)ahrs.yaw_sensor,
wp_distance : auto_state.wp_distance,
target_bearing_cd : (int16_t)nav_controller->target_bearing_cd(),
@ -332,7 +332,7 @@ void Plane::Log_Write_Status()
{
struct log_Status pkt = {
LOG_PACKET_HEADER_INIT(LOG_STATUS_MSG)
,timestamp : hal.scheduler->millis()
,timestamp : millis()
,is_flying : is_flying()
,is_flying_probability : isFlyingProbability
,armed : hal.util->get_soft_armed()
@ -359,7 +359,7 @@ void Plane::Log_Write_Sonar()
{
struct log_Sonar pkt = {
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
timestamp : hal.scheduler->millis(),
timestamp : millis(),
distance : (float)rangefinder.distance_cm(),
voltage : rangefinder.voltage_mv()*0.001f,
baro_alt : barometer.get_altitude(),
@ -393,7 +393,7 @@ void Plane::Log_Write_Optflow()
const Vector2f &bodyRate = optflow.bodyRate();
struct log_Optflow pkt = {
LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG),
time_ms : hal.scheduler->millis(),
time_ms : millis(),
surface_quality : optflow.quality(),
flow_x : flowRate.x,
flow_y : flowRate.y,
@ -422,7 +422,7 @@ void Plane::Log_Write_Current()
void Plane::Log_Arm_Disarm() {
struct log_Arm_Disarm pkt = {
LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG),
time_ms : hal.scheduler->millis(),
time_ms : millis(),
arm_state : arming.is_armed(),
arm_checks : arming.get_enabled_checks()
};

4
ArduPlane/Parameters.cpp
View File

@ -1243,10 +1243,10 @@ void Plane::load_parameters(void)
g.format_version.set_and_save(Parameters::k_format_version);
cliSerial->println_P(PSTR("done."));
} else {
uint32_t before = hal.scheduler->micros();
uint32_t before = micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
AP_Param::convert_old_parameters(&conversion_table[0], sizeof(conversion_table)/sizeof(conversion_table[0]));
cliSerial->printf_P(PSTR("load_all took %luus\n"), hal.scheduler->micros() - before);
cliSerial->printf_P(PSTR("load_all took %luus\n"), micros() - before);
}
}

2
ArduPlane/Plane.h
View File

@ -886,6 +886,8 @@ private:
void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
void run_cli(AP_HAL::UARTDriver *port);
void log_init();
uint32_t millis() const;
uint32_t micros() const;
public:
void mavlink_delay_cb();

6
ArduPlane/altitude.cpp
View File

@ -525,7 +525,7 @@ float Plane::lookahead_adjustment(void)
*/
float Plane::rangefinder_correction(void)
{
if (hal.scheduler->millis() - rangefinder_state.last_correction_time_ms > 5000) {
if (millis() - rangefinder_state.last_correction_time_ms > 5000) {
// we haven't had any rangefinder data for 5s - don't use it
return 0;
}
@ -583,11 +583,11 @@ void Plane::rangefinder_height_update(void)
// remember the last correction. Use a low pass filter unless
// the old data is more than 5 seconds old
if (hal.scheduler->millis() - rangefinder_state.last_correction_time_ms > 5000) {
if (millis() - rangefinder_state.last_correction_time_ms > 5000) {
rangefinder_state.correction = correction;
} else {
rangefinder_state.correction = 0.8f*rangefinder_state.correction + 0.2f*correction;
}
rangefinder_state.last_correction_time_ms = hal.scheduler->millis();
rangefinder_state.last_correction_time_ms = millis();
}
}

12
ArduPlane/commands_logic.cpp
View File

@ -394,10 +394,10 @@ bool Plane::verify_takeoff()
if (auto_state.takeoff_speed_time_ms == 0 &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
gps.ground_speed() > min_gps_speed) {
auto_state.takeoff_speed_time_ms = hal.scheduler->millis();
auto_state.takeoff_speed_time_ms = millis();
}
if (auto_state.takeoff_speed_time_ms != 0 &&
hal.scheduler->millis() - auto_state.takeoff_speed_time_ms >= 2000) {
millis() - auto_state.takeoff_speed_time_ms >= 2000) {
// once we reach sufficient speed for good GPS course
// estimation we save our current GPS ground course
// corrected for summed yaw to set the take off
@ -509,9 +509,9 @@ bool Plane::verify_loiter_time()
if (loiter.start_time_ms == 0) {
if (nav_controller->reached_loiter_target()) {
// we've reached the target, start the timer
loiter.start_time_ms = hal.scheduler->millis();
loiter.start_time_ms = millis();
}
} else if ((hal.scheduler->millis() - loiter.start_time_ms) > loiter.time_max_ms) {
} else if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) {
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
return true;
}
@ -631,7 +631,7 @@ bool Plane::verify_continue_and_change_alt()
void Plane::do_wait_delay(const AP_Mission::Mission_Command& cmd)
{
condition_start = hal.scheduler->millis();
condition_start = millis();
condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
}
@ -663,7 +663,7 @@ void Plane::do_within_distance(const AP_Mission::Mission_Command& cmd)
bool Plane::verify_wait_delay()
{
if ((unsigned)(hal.scheduler->millis() - condition_start) > (unsigned)condition_value) {
if ((unsigned)(millis() - condition_start) > (unsigned)condition_value) {
condition_value = 0;
return true;
}

2
ArduPlane/control_modes.cpp
View File

@ -17,7 +17,7 @@ void Plane::read_control_switch()
return;
}
if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 100) {
if (millis() - failsafe.last_valid_rc_ms > 100) {
// only use signals that are less than 0.1s old.
return;
}

2
ArduPlane/events.cpp
View File

@ -6,7 +6,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
{
// This is how to handle a short loss of control signal failsafe.
failsafe.state = fstype;
failsafe.ch3_timer_ms = hal.scheduler->millis();
failsafe.ch3_timer_ms = millis();
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, "));
switch(control_mode)
{

2
ArduPlane/failsafe.cpp
View File

@ -21,7 +21,7 @@ void Plane::failsafe_check(void)
static uint16_t last_mainLoop_count;
static uint32_t last_timestamp;
static bool in_failsafe;
uint32_t tnow = hal.scheduler->micros();
uint32_t tnow = micros();
if (mainLoop_count != last_mainLoop_count) {
// the main loop is running, all is OK

2
ArduPlane/geofence.cpp
View File

@ -356,7 +356,7 @@ void Plane::geofence_check(bool altitude_check_only)
// we are outside, and have not previously triggered.
geofence_state->fence_triggered = true;
geofence_state->breach_count++;
geofence_state->breach_time = hal.scheduler->millis();
geofence_state->breach_time = millis();
geofence_state->breach_type = breach_type;
#if FENCE_TRIGGERED_PIN > 0

4
ArduPlane/landing.cpp
View File

@ -58,7 +58,7 @@ bool Plane::verify_land()
(fabsf(auto_state.land_sink_rate) < 0.2f && !is_flying())) {
if (!auto_state.land_complete) {
if (!is_flying() && (hal.scheduler->millis()-auto_state.last_flying_ms) > 3000) {
if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) {
gcs_send_text_fmt(PSTR("Flare crash detected: speed=%.1f"), (double)gps.ground_speed());
} else {
gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"),
@ -112,7 +112,7 @@ void Plane::disarm_if_autoland_complete()
arming.arming_required() != AP_Arming::NO &&
arming.is_armed()) {
/* we have auto disarm enabled. See if enough time has passed */
if (hal.scheduler->millis() - auto_state.last_flying_ms >= g.land_disarm_delay*1000UL) {
if (millis() - auto_state.last_flying_ms >= g.land_disarm_delay*1000UL) {
if (disarm_motors()) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Auto-Disarmed"));
}

4
ArduPlane/navigation.cpp
View File

@ -144,10 +144,10 @@ void Plane::update_cruise()
gps.ground_speed() >= 3 &&
cruise_state.lock_timer_ms == 0) {
// user wants to lock the heading - start the timer
cruise_state.lock_timer_ms = hal.scheduler->millis();
cruise_state.lock_timer_ms = millis();
}
if (cruise_state.lock_timer_ms != 0 &&
(hal.scheduler->millis() - cruise_state.lock_timer_ms) > 500) {
(millis() - cruise_state.lock_timer_ms) > 500) {
// lock the heading after 0.5 seconds of zero heading input
// from user
cruise_state.locked_heading = true;

8
ArduPlane/radio.cpp
View File

@ -110,7 +110,7 @@ void Plane::rudder_arm_check()
// full right rudder starts arming counter
if (channel_rudder->control_in > 4000) {
uint32_t now = hal.scheduler->millis();
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
@ -133,7 +133,7 @@ void Plane::read_radio()
return;
}
failsafe.last_valid_rc_ms = hal.scheduler->millis();
failsafe.last_valid_rc_ms = millis();
elevon.ch1_temp = channel_roll->read();
elevon.ch2_temp = channel_pitch->read();
@ -190,7 +190,7 @@ void Plane::read_radio()
void Plane::control_failsafe(uint16_t pwm)
{
if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 1000 || rc_failsafe_active()) {
if (millis() - failsafe.last_valid_rc_ms > 1000 || rc_failsafe_active()) {
// we do not have valid RC input. Set all primary channel
// control inputs to the trim value and throttle to min
channel_roll->radio_in = channel_roll->radio_trim;
@ -312,7 +312,7 @@ bool Plane::rc_failsafe_active(void)
if (!g.throttle_fs_enabled) {
return false;
}
if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 1000) {
if (millis() - failsafe.last_valid_rc_ms > 1000) {
// we haven't had a valid RC frame for 1 seconds
return true;
}

19
ArduPlane/system.cpp
View File

@ -289,7 +289,7 @@ void Plane::startup_ground(void)
// reset last heartbeat time, so we don't trigger failsafe on slow
// startup
failsafe.last_heartbeat_ms = hal.scheduler->millis();
failsafe.last_heartbeat_ms = millis();
// we don't want writes to the serial port to cause us to pause
// mid-flight, so set the serial ports non-blocking once we are
@ -472,7 +472,7 @@ void Plane::exit_mode(enum FlightMode mode)
void Plane::check_long_failsafe()
{
uint32_t tnow = hal.scheduler->millis();
uint32_t tnow = millis();
// only act on changes
// -------------------
if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS) {
@ -575,7 +575,7 @@ void Plane::resetPerfData(void)
mainLoop_count = 0;
G_Dt_max = 0;
G_Dt_min = 0;
perf_mon_timer = hal.scheduler->millis();
perf_mon_timer = millis();
}
@ -769,3 +769,16 @@ bool Plane::disarm_motors(void)
return true;
}
/*
having local millis() and micros() reduces code size a bit on AVR
*/
uint32_t Plane::millis(void) const
{
return hal.scheduler->millis();
}
uint32_t Plane::micros(void) const
{
return hal.scheduler->micros();
}

2
ArduPlane/takeoff.cpp
View File

@ -14,7 +14,7 @@
bool Plane::auto_takeoff_check(void)
{
// this is a more advanced check that relies on TECS
uint32_t now = hal.scheduler->millis();
uint32_t now = millis();
static bool launchTimerStarted;
static uint32_t last_tkoff_arm_time;
static uint32_t last_check_ms;

8
ArduPlane/test.cpp
View File

@ -384,8 +384,8 @@ int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv)
while(1) {
hal.scheduler->delay(20);
if (hal.scheduler->micros() - fast_loopTimer_us > 19000UL) {
fast_loopTimer_us = hal.scheduler->micros();
if (micros() - fast_loopTimer_us > 19000UL) {
fast_loopTimer_us = micros();
// INS
// ---
@ -446,8 +446,8 @@ int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv)
while(1) {
hal.scheduler->delay(20);
if (hal.scheduler->micros() - fast_loopTimer_us > 19000UL) {
fast_loopTimer_us = hal.scheduler->micros();
if (micros() - fast_loopTimer_us > 19000UL) {
fast_loopTimer_us = micros();
// INS
// ---