Copter: stop using global ap variable as bitmask

This commit is contained in:
Peter Barker 2024-10-09 11:14:19 +11:00 committed by Randy Mackay
parent 2ba7516b0f
commit e4859599cf
3 changed files with 55 additions and 37 deletions

View File

@ -688,12 +688,29 @@ void Copter::three_hz_loop()
low_alt_avoidance();
}
// ap_value calculates a 32-bit bitmask representing various pieces of
// state about the Copter. It replaces a global variable which was
// used to track this state.
uint32_t Copter::ap_value() const
{
uint32_t ret = 0;
const bool *b = (const bool *)≈
for (uint8_t i=0; i<sizeof(ap); i++) {
if (b[i]) {
ret |= 1U<<i;
}
}
return ret;
}
// one_hz_loop - runs at 1Hz
void Copter::one_hz_loop()
{
#if HAL_LOGGING_ENABLED
if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(LogDataID::AP_STATE, ap.value);
Log_Write_Data(LogDataID::AP_STATE, ap_value());
}
#endif

View File

@ -347,46 +347,48 @@ private:
# include USERHOOK_VARIABLES
#endif
// Documentation of GLobals:
typedef union {
struct {
uint8_t unused1 : 1; // 0
uint8_t unused_was_simple_mode : 2; // 1,2
uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully
uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised
uint8_t logging_started : 1; // 6 // true if logging has started
uint8_t land_complete : 1; // 7 // true if we have detected a landing
uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio
uint8_t usb_connected_unused : 1; // 9 // UNUSED
uint8_t rc_receiver_present_unused : 1; // 10 // UNUSED
uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration
uint8_t motor_test : 1; // 12 // true if we are currently performing the motors test
uint8_t initialised : 1; // 13 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes
uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete)
uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
uint8_t system_time_set_unused : 1; // 16 // true if the system time has been set from the GPS
uint8_t gps_glitching : 1; // 17 // true if GPS glitching is affecting navigation accuracy
uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use
uint8_t land_repo_active : 1; // 21 // true if the pilot is overriding the landing position
uint8_t motor_interlock_switch : 1; // 22 // true if pilot is requesting motor interlock enable
uint8_t in_arming_delay : 1; // 23 // true while we are armed but waiting to spin motors
uint8_t initialised_params : 1; // 24 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done
uint8_t unused3 : 1; // 25 // was compass_init_location; true when the compass's initial location has been set
uint8_t unused2 : 1; // 26 // aux switch rc_override is allowed
uint8_t armed_with_airmode_switch : 1; // 27 // we armed using a arming switch
uint8_t prec_land_active : 1; // 28 // true if precland is active
};
uint32_t value;
} ap_t;
// ap_value calculates a 32-bit bitmask representing various pieces of
// state about the Copter. It replaces a global variable which was
// used to track this state.
uint32_t ap_value() const;
ap_t ap;
// These variables are essentially global variables. These should
// be removed over time. It is critical that the offsets of these
// variables remain unchanged - the logging is dependent on this
// ordering!
struct PACKED {
bool unused1; // 0
bool unused_was_simple_mode_byte1; // 1
bool unused_was_simple_mode_byte2; // 2
bool pre_arm_rc_check; // 3 true if rc input pre-arm checks have been completed successfully
bool pre_arm_check; // 4 true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
bool auto_armed; // 5 stops auto missions from beginning until throttle is raised
bool unused_log_started; // 6
bool land_complete; // 7 true if we have detected a landing
bool new_radio_frame; // 8 Set true if we have new PWM data to act on from the Radio
bool unused_usb_connected; // 9
bool unused_receiver_present; // 10
bool compass_mot; // 11 true if we are currently performing compassmot calibration
bool motor_test; // 12 true if we are currently performing the motors test
bool initialised; // 13 true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes
bool land_complete_maybe; // 14 true if we may have landed (less strict version of land_complete)
bool throttle_zero; // 15 true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
bool system_time_set_unused; // 16 true if the system time has been set from the GPS
bool gps_glitching; // 17 true if GPS glitching is affecting navigation accuracy
bool using_interlock; // 18 aux switch motor interlock function is in use
bool land_repo_active; // 19 true if the pilot is overriding the landing position
bool motor_interlock_switch; // 20 true if pilot is requesting motor interlock enable
bool in_arming_delay; // 21 true while we are armed but waiting to spin motors
bool initialised_params; // 22 true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done
bool unused_compass_init_location; // 23
bool unused2_aux_switch_rc_override_allowed; // 24
bool armed_with_airmode_switch; // 25 we armed using a arming switch
bool prec_land_active; // 26 true if precland is active
} ap;
AirMode air_mode; // air mode is 0 = not-configured ; 1 = disabled; 2 = enabled;
bool force_flying; // force flying is enabled when true;
static_assert(sizeof(uint32_t) == sizeof(ap), "ap_t must be uint32_t");
// This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO,
Mode *flightmode;

View File

@ -347,7 +347,6 @@ void Copter::update_auto_armed()
*/
bool Copter::should_log(uint32_t mask)
{
ap.logging_started = logger.logging_started();
return logger.should_log(mask);
}
#endif