Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: instantiate optflow on Pixhawk

Browse Source 
Run sensor reads from scheduler
This commit is contained in:
Randy Mackay 2014-07-12 11:33:49 +09:00
parent 021485fffc
commit 717e63f47a
1 changed files with 23 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ArduCopter/ArduCopter.pde
View File

@ -330,7 +330,11 @@ AP_Mission mission(ahrs, &start_command, &verify_command, &exit_mission);
// Optical flow sensor // Optical flow sensor
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
#if OPTFLOW == ENABLED #if OPTFLOW == ENABLED
static AP_OpticalFlow_ADNS3080 optflow; #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
static AP_OpticalFlow_ADNS3080 optflow(ahrs);
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
static AP_OpticalFlow_PX4 optflow(ahrs);
#endif
#endif #endif
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -783,6 +787,9 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ rc_loop, 4, 10 }, { rc_loop, 4, 10 },
{ throttle_loop, 8, 45 }, { throttle_loop, 8, 45 },
{ update_GPS, 8, 90 }, { update_GPS, 8, 90 },
#if OPTFLOW == ENABLED
{ update_optical_flow, 8, 20 },
#endif
{ update_batt_compass, 40, 72 }, { update_batt_compass, 40, 72 },
{ read_aux_switches, 40, 5 }, { read_aux_switches, 40, 5 },
{ arm_motors_check, 40, 1 }, { arm_motors_check, 40, 1 },
@ -854,6 +861,9 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ rc_loop, 1, 100 }, { rc_loop, 1, 100 },
{ throttle_loop, 2, 450 }, { throttle_loop, 2, 450 },
{ update_GPS, 2, 900 }, { update_GPS, 2, 900 },
#if OPTFLOW == ENABLED
{ update_optical_flow, 2, 100 },
#endif
{ update_batt_compass, 10, 720 }, { update_batt_compass, 10, 720 },
{ read_aux_switches, 10, 50 }, { read_aux_switches, 10, 50 },
{ arm_motors_check, 10, 10 }, { arm_motors_check, 10, 10 },
@ -1014,15 +1024,6 @@ static void fast_loop()
// run the attitude controllers // run the attitude controllers
update_flight_mode(); update_flight_mode();
// optical flow
// --------------------
#if OPTFLOW == ENABLED
if(g.optflow_enabled) {
update_optical_flow();
}
#endif // OPTFLOW == ENABLED
} }
// rc_loops - reads user input from transmitter/receiver // rc_loops - reads user input from transmitter/receiver
@ -1207,20 +1208,20 @@ static void one_hz_loop()
static void update_optical_flow(void) static void update_optical_flow(void)
{ {
static uint32_t last_of_update = 0; static uint32_t last_of_update = 0;
static uint8_t of_log_counter = 0;
// if new data has arrived, process it // exit immediately if not enabled
if( optflow.last_update != last_of_update ) { if (!optflow.enabled()) {
last_of_update = optflow.last_update; return;
optflow.update_position(ahrs.roll, ahrs.pitch, ahrs.sin_yaw(), ahrs.cos_yaw(), current_loc.alt); // updates internal lon and lat with estimation based on optical flow }
// write to log at 5hz // read from sensor
of_log_counter++; optflow.update();
if( of_log_counter >= 4 ) {
of_log_counter = 0; // write to log if new data has arrived
if (g.log_bitmask & MASK_LOG_OPTFLOW) { if (optflow.last_update() != last_of_update) {
Log_Write_Optflow(); last_of_update = optflow.last_update();
} if (g.log_bitmask & MASK_LOG_OPTFLOW) {
Log_Write_Optflow();
} }
} }
} }