|
|
|
@ -29,11 +29,11 @@
@@ -29,11 +29,11 @@
|
|
|
|
|
struct AP_Module::hook_list *AP_Module::hooks[NUM_HOOKS]; |
|
|
|
|
|
|
|
|
|
const char *AP_Module::hook_names[AP_Module::NUM_HOOKS] = { |
|
|
|
|
"hook_setup_start", |
|
|
|
|
"hook_setup_complete", |
|
|
|
|
"hook_AHRS_update", |
|
|
|
|
"hook_gyro_sample", |
|
|
|
|
"hook_accel_sample", |
|
|
|
|
"ap_hook_setup_start", |
|
|
|
|
"ap_hook_setup_complete", |
|
|
|
|
"ap_hook_AHRS_update", |
|
|
|
|
"ap_hook_gyro_sample", |
|
|
|
|
"ap_hook_accel_sample", |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
@ -104,7 +104,7 @@ void AP_Module::call_hook_setup_start(void)
@@ -104,7 +104,7 @@ void AP_Module::call_hook_setup_start(void)
|
|
|
|
|
{ |
|
|
|
|
uint64_t now = AP_HAL::micros64(); |
|
|
|
|
for (const struct hook_list *h=hooks[HOOK_SETUP_START]; h; h=h->next) { |
|
|
|
|
hook_setup_start_fn_t fn = reinterpret_cast<hook_setup_start_fn_t>(h->symbol); |
|
|
|
|
ap_hook_setup_start_fn_t fn = reinterpret_cast<ap_hook_setup_start_fn_t>(h->symbol); |
|
|
|
|
fn(now); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -116,7 +116,7 @@ void AP_Module::call_hook_setup_complete(void)
@@ -116,7 +116,7 @@ void AP_Module::call_hook_setup_complete(void)
|
|
|
|
|
{ |
|
|
|
|
uint64_t now = AP_HAL::micros64(); |
|
|
|
|
for (const struct hook_list *h=hooks[HOOK_SETUP_COMPLETE]; h; h=h->next) { |
|
|
|
|
hook_setup_complete_fn_t fn = reinterpret_cast<hook_setup_complete_fn_t>(h->symbol); |
|
|
|
|
ap_hook_setup_complete_fn_t fn = reinterpret_cast<ap_hook_setup_complete_fn_t>(h->symbol); |
|
|
|
|
fn(now); |
|
|
|
|
}
|
|
|
|
|
} |
|
|
|
@ -190,7 +190,7 @@ void AP_Module::call_hook_AHRS_update(const AP_AHRS_NavEKF &ahrs)
@@ -190,7 +190,7 @@ void AP_Module::call_hook_AHRS_update(const AP_AHRS_NavEKF &ahrs)
|
|
|
|
|
state.accel_ef[2] = accel_ef[0]; |
|
|
|
|
|
|
|
|
|
for (const struct hook_list *h=hooks[HOOK_AHRS_UPDATE]; h; h=h->next) { |
|
|
|
|
hook_AHRS_update_fn_t fn = reinterpret_cast<hook_AHRS_update_fn_t>(h->symbol); |
|
|
|
|
ap_hook_AHRS_update_fn_t fn = reinterpret_cast<ap_hook_AHRS_update_fn_t>(h->symbol); |
|
|
|
|
fn(&state); |
|
|
|
|
}
|
|
|
|
|
} |
|
|
|
@ -219,7 +219,7 @@ void AP_Module::call_hook_gyro_sample(uint8_t instance, float dt, const Vector3f
@@ -219,7 +219,7 @@ void AP_Module::call_hook_gyro_sample(uint8_t instance, float dt, const Vector3f
|
|
|
|
|
state.gyro[2] = gyro[2]; |
|
|
|
|
|
|
|
|
|
for (const struct hook_list *h=hooks[HOOK_GYRO_SAMPLE]; h; h=h->next) { |
|
|
|
|
hook_gyro_sample_fn_t fn = reinterpret_cast<hook_gyro_sample_fn_t>(h->symbol); |
|
|
|
|
ap_hook_gyro_sample_fn_t fn = reinterpret_cast<ap_hook_gyro_sample_fn_t>(h->symbol); |
|
|
|
|
fn(&state); |
|
|
|
|
}
|
|
|
|
|
} |
|
|
|
@ -247,7 +247,7 @@ void AP_Module::call_hook_accel_sample(uint8_t instance, float dt, const Vector3
@@ -247,7 +247,7 @@ void AP_Module::call_hook_accel_sample(uint8_t instance, float dt, const Vector3
|
|
|
|
|
state.accel[2] = accel[2]; |
|
|
|
|
|
|
|
|
|
for (const struct hook_list *h=hooks[HOOK_ACCEL_SAMPLE]; h; h=h->next) { |
|
|
|
|
hook_accel_sample_fn_t fn = reinterpret_cast<hook_accel_sample_fn_t>(h->symbol); |
|
|
|
|
ap_hook_accel_sample_fn_t fn = reinterpret_cast<ap_hook_accel_sample_fn_t>(h->symbol); |
|
|
|
|
fn(&state); |
|
|
|
|
}
|
|
|
|
|
} |
|
|
|
|