#if MOUNT == ENABLED
camera_mount.update_mount_type(); #endif
#if MOUNT2 == ENABLED
camera_mount2.update_mount_type(); #endif
check_usb_mux(); }
// called at 100hz but data from sensor only arrives at 20 Hz #if OPTFLOW == ENABLED
static void update_optical_flow(void) {
static uint32_t last_of_update = 0; static uint8_t of_log_counter = 0;
// if new data has arrived, process it
if( optflow.last_update != last_of_update ) { last_of_update = optflow.last_update;
optflow.update_position(ahrs.roll, ahrs.pitch, sin_yaw, cos_yaw, current_loc.alt); // updates internal lon and lat with estimation based on optical flow
// write to log at 5hz of_log_counter++;
if( of_log_counter >= 4 ) { of_log_counter = 0;
if (g.log_bitmask & MASK_LOG_OPTFLOW) { Log_Write_Optflow(); } } } }
#endif // OPTFLOW == ENABLED
// called at 50hz
static void update_GPS(void) {
// A counter that is used to grab at least 10 reads before commiting the Home location static uint8_t ground_start_count = 10;
g_gps->update();
if (g_gps->new_data && last_gps_time != g_gps->last_fix_time && g_gps->status() >= GPS::GPS_OK_FIX_2D) { // clear new data flag g_gps->new_data = false;
// save GPS time so we don't get duplicate reads last_gps_time = g_gps->last_fix_time;
// log location if we have at least a 2D fix
if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) { DataFlash.Log_Write_GPS(g_gps, current_loc.alt); }
// for performance monitoring gps_fix_count++;
// run glitch protection and update AP_Notify gps_glitch.check_position();
if (AP_Notify::flags.gps_glitching != gps_glitch.glitching()) {
if (gps_glitch.glitching()) {
Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_GPS_GLITCH); }else{
Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_ERROR_RESOLVED); }
AP_Notify::flags.gps_glitching = gps_glitch.glitching(); }
// check if we can initialise home yet if (!ap.home_is_set) {
// if we have a 3d lock and valid location
if(g_gps->status() >= GPS::GPS_OK_FIX_3D && g_gps->latitude != 0) { if( ground_start_count > 0 ) { ground_start_count--; }else{
// after 10 successful reads store home location
// ap.home_is_set will be true so this will only happen once ground_start_count = 0; init_home();
// set system clock for log timestamps
hal.util->set_system_clock(g_gps->time_epoch_usec());
if (g.compass_enabled) {
// Set compass declination automatically
compass.set_initial_location(g_gps->latitude, g_gps->longitude); } } }else{
// start again if we lose 3d lock ground_start_count = 10; } }
#if CAMERA == ENABLED
if (camera.update_location(current_loc) == true) { do_take_picture(); }
#endif }
// check for loss of gps failsafe_gps_check(); }
// set_yaw_mode - update yaw mode and initialise any variables required bool set_yaw_mode(uint8_t new_yaw_mode) {
// boolean to ensure proper initialisation of throttle modes bool yaw_initialised = false;
// return immediately if no change if( new_yaw_mode == yaw_mode ) { return true; }
switch( new_yaw_mode ) { case YAW_HOLD:
yaw_initialised = true; break; case YAW_ACRO:
yaw_initialised = true;
acro_yaw_rate = 0; break;
case YAW_LOOK_AT_NEXT_WP: if( ap.home_is_set ) {
yaw_initialised = true; }
break;
case YAW_LOOK_AT_LOCATION: if( ap.home_is_set ) {
// update bearing - assumes yaw_look_at_WP has been intialised before set_yaw_mode was called yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP); yaw_initialised = true; }
break;
case YAW_CIRCLE:
if( ap.home_is_set ) {
// set yaw to point to center of circle yaw_look_at_WP = circle_center;
// initialise bearing to current heading yaw_look_at_WP_bearing = ahrs.yaw_sensor; yaw_initialised = true; }
break;
case YAW_LOOK_AT_HEADING: yaw_initialised = true; break;
case YAW_LOOK_AT_HOME: if( ap.home_is_set ) {
yaw_initialised = true; }
break;
case YAW_LOOK_AHEAD:
if( ap.home_is_set ) {
yaw_initialised = true; }
break; case YAW_TOY:
yaw_initialised = true; break;
case YAW_RESETTOARMEDYAW:
nav_yaw = ahrs.yaw_sensor; // store current yaw so we can start rotating back to correct one yaw_initialised = true; break; }
// if initialisation has been successful update the yaw mode if( yaw_initialised ) {
yaw_mode = new_yaw_mode; }
// return success or failure return yaw_initialised; }
// update_yaw_mode - run high level yaw controllers // 100hz update rate
void update_yaw_mode(void) {
switch(yaw_mode) {
case YAW_HOLD:
// if we are landed reset yaw target to current heading
if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor; }
// heading hold at heading held in nav_yaw but allow input from pilot get_yaw_rate_stabilized_ef(g.rc_4.control_in); break;
case YAW_ACRO:
// pilot controlled yaw using rate controller get_yaw_rate_stabilized_bf(g.rc_4.control_in); break;
case YAW_LOOK_AT_NEXT_WP:
// if we are landed reset yaw target to current heading if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor; }else{
// point towards next waypoint (no pilot input accepted)
// we don't use wp_bearing because we don't want the copter to turn too much during flight nav_yaw = get_yaw_slew(nav_yaw, original_wp_bearing, AUTO_YAW_SLEW_RATE); }
get_stabilize_yaw(nav_yaw);
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration if( g.rc_4.control_in != 0 ) { set_yaw_mode(YAW_HOLD); }
break;
case YAW_LOOK_AT_LOCATION:
// if we are landed reset yaw target to current heading if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor; }
// point towards a location held in yaw_look_at_WP get_look_at_yaw();
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration if( g.rc_4.control_in != 0 ) { set_yaw_mode(YAW_HOLD); }
break;
case YAW_CIRCLE:
// if we are landed reset yaw target to current heading if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor; }
// points toward the center of the circle or does a panorama get_circle_yaw();
// if there is any pilot input, switch to YAW_HOLD mode for the next iteration if( g.rc_4.control_in != 0 ) { set_yaw_mode(YAW_HOLD); }
break;
case YAW_LOOK_AT_HOME:
// if we are landed reset yaw target to current heading if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor; }else{