mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 01:48:29 -04:00
AP_HAL_PX4: implement HAL::run()
This commit is contained in:
parent
3fe1d86c80
commit
0dab23c4df
libraries/AP_HAL_PX4
@ -6,7 +6,8 @@
|
||||
#define AP_HAL_MAIN() \
|
||||
extern "C" __EXPORT int SKETCH_MAIN(int argc, char * const argv[]); \
|
||||
int SKETCH_MAIN(int argc, char * const argv[]) { \
|
||||
hal.init(argc, argv); \
|
||||
AP_HAL::HAL::FunCallbacks callbacks(setup, loop); \
|
||||
hal.run(argc, argv, &callbacks); \
|
||||
return OK; \
|
||||
}
|
||||
|
||||
|
@ -114,12 +114,10 @@ static void loop_overtime(void *)
|
||||
px4_ran_overtime = true;
|
||||
}
|
||||
|
||||
static AP_HAL::HAL::Callbacks* g_callbacks;
|
||||
|
||||
static int main_loop(int argc, char **argv)
|
||||
{
|
||||
extern void setup(void);
|
||||
extern void loop(void);
|
||||
|
||||
|
||||
hal.uartA->begin(115200);
|
||||
hal.uartB->begin(38400);
|
||||
hal.uartC->begin(57600);
|
||||
@ -140,7 +138,7 @@ static int main_loop(int argc, char **argv)
|
||||
|
||||
schedulerInstance.hal_initialized();
|
||||
|
||||
setup();
|
||||
g_callbacks->setup();
|
||||
hal.scheduler->system_initialized();
|
||||
|
||||
perf_counter_t perf_loop = perf_alloc(PC_ELAPSED, "APM_loop");
|
||||
@ -165,7 +163,7 @@ static int main_loop(int argc, char **argv)
|
||||
*/
|
||||
hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, NULL);
|
||||
|
||||
loop();
|
||||
g_callbacks->loop();
|
||||
|
||||
if (px4_ran_overtime) {
|
||||
/*
|
||||
@ -307,6 +305,13 @@ void HAL_PX4::init(int argc, char * const argv[]) const
|
||||
exit(1);
|
||||
}
|
||||
|
||||
void HAL_PX4::run(int argc, char * const argv[], Callbacks* callbacks) const
|
||||
{
|
||||
assert(callbacks);
|
||||
g_callbacks = callbacks;
|
||||
init(argc, argv);
|
||||
}
|
||||
|
||||
const AP_HAL::HAL& AP_HAL::get_HAL() {
|
||||
static const HAL_PX4 hal_px4;
|
||||
return hal_px4;
|
||||
|
@ -15,6 +15,7 @@ class HAL_PX4 : public AP_HAL::HAL {
|
||||
public:
|
||||
HAL_PX4();
|
||||
void init(int argc, char * const argv[]) const;
|
||||
void run(int argc, char* const argv[], Callbacks* callbacks) const override;
|
||||
};
|
||||
|
||||
void hal_px4_set_priority(uint8_t priority);
|
||||
|
Loading…
Reference in New Issue
Block a user