/* read update GPS position - 10Hz update */ void Plane::update_GPS_10Hz(void) { static uint32_t last_gps_msg_ms; if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) { last_gps_msg_ms = gps.last_message_time_ms(); if (ground_start_count > 1) { ground_start_count--; } else if (ground_start_count == 1) { // We countdown N number of good GPS fixes // so that the altitude is more accurate // ------------------------------------- if (current_loc.lat == 0) { ground_start_count = 5; } else { init_home(); // set system clock for log timestamps uint64_t gps_timestamp = gps.time_epoch_usec(); hal.util->set_system_clock(gps_timestamp); // update signing timestamp GCS_MAVLINK::update_signing_timestamp(gps_timestamp); if (g.compass_enabled) { // Set compass declination automatically const Location &loc = gps.location(); compass.set_initial_location(loc.lat, loc.lng); } ground_start_count = 0; } } // see if we've breached the geo-fence geofence_check(false); #if CAMERA == ENABLED if (camera.update_location(current_loc, plane.ahrs ) == true) { do_take_picture(); } #endif if (!hal.util->get_soft_armed()) { update_home(); // reset the landing altitude correction auto_state.land_alt_offset = 0; } // update wind estimate ahrs.estimate_wind(); } else if (gps.status() < AP_GPS::GPS_OK_FIX_3D && ground_start_count != 0) { // lost 3D fix, start again ground_start_count = 5; } calc_gndspeed_undershoot(); }
/* read update GPS position - 10Hz update */ void Plane::update_GPS_10Hz(void) { // get position from AHRS have_position = ahrs.get_position(current_loc); static uint32_t last_gps_msg_ms; if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) { last_gps_msg_ms = gps.last_message_time_ms(); if (ground_start_count > 1) { ground_start_count--; } else if (ground_start_count == 1) { // We countdown N number of good GPS fixes // so that the altitude is more accurate // ------------------------------------- if (current_loc.lat == 0) { ground_start_count = 5; } else { init_home(); // set system clock for log timestamps hal.util->set_system_clock(gps.time_epoch_usec()); if (g.compass_enabled) { // Set compass declination automatically const Location &loc = gps.location(); compass.set_initial_location(loc.lat, loc.lng); } ground_start_count = 0; } } // see if we've breached the geo-fence geofence_check(false); #if CAMERA == ENABLED if (camera.update_location(current_loc) == true) { do_take_picture(); } #endif if (!hal.util->get_soft_armed()) { update_home(); } // update wind estimate ahrs.estimate_wind(); } calc_gndspeed_undershoot(); }
void Rover::update_GPS_10Hz(void) { have_position = ahrs.get_position(current_loc); if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) { last_gps_msg_ms = gps.last_message_time_ms(); if (ground_start_count > 1){ ground_start_count--; } else if (ground_start_count == 1) { // We countdown N number of good GPS fixes // so that the altitude is more accurate // ------------------------------------- if (current_loc.lat == 0) { ground_start_count = 20; } else { init_home(); // set system clock for log timestamps uint64_t gps_timestamp = gps.time_epoch_usec(); hal.util->set_system_clock(gps_timestamp); // update signing timestamp GCS_MAVLINK::update_signing_timestamp(gps_timestamp); if (g.compass_enabled) { // Set compass declination automatically compass.set_initial_location(gps.location().lat, gps.location().lng); } ground_start_count = 0; } } // get ground speed estimate from AHRS ground_speed = ahrs.groundspeed(); #if CAMERA == ENABLED if (camera.update_location(current_loc, rover.ahrs) == true) { do_take_picture(); } #endif if (!hal.util->get_soft_armed()) { update_home(); } } }
void Rover::update_GPS_10Hz(void) { have_position = ahrs.get_position(current_loc); if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) { last_gps_msg_ms = gps.last_message_time_ms(); if (ground_start_count > 1){ ground_start_count--; } else if (ground_start_count == 1) { // We countdown N number of good GPS fixes // so that the altitude is more accurate // ------------------------------------- if (current_loc.lat == 0) { ground_start_count = 20; } else { init_home(); // set system clock for log timestamps hal.util->set_system_clock(gps.time_epoch_usec()); if (g.compass_enabled) { // Set compass declination automatically compass.set_initial_location(gps.location().lat, gps.location().lng); } ground_start_count = 0; } } Vector3f velocity; if (ahrs.get_velocity_NED(velocity)) { ground_speed = pythagorous2(velocity.x, velocity.y); } else { ground_speed = gps.ground_speed(); } #if CAMERA == ENABLED if (camera.update_location(current_loc, rover.ahrs) == true) { do_take_picture(); } #endif if (!hal.util->get_soft_armed()) { update_home(); } } }
// called at 50hz void Copter::update_GPS(void) { static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message bool gps_updated = false; gps.update(); // log after every gps message for (uint8_t i=0; i<gps.num_sensors(); i++) { if (gps.last_message_time_ms(i) != last_gps_reading[i]) { last_gps_reading[i] = gps.last_message_time_ms(i); // log GPS message if (should_log(MASK_LOG_GPS)) { DataFlash.Log_Write_GPS(gps, i, current_loc.alt); } gps_updated = true; } } if (gps_updated) { // set system time if necessary set_system_time_from_GPS(); //Added by sdb to rewrite PID parameters when switch from RTK-> GPS & Baro if (gps.status() < AP_GPS::GPS_OK_FIX_3D){ g.p_alt_hold.kP(1.0f); g.p_pos_xy.kP(1.0f);} if (gps.status() == AP_GPS::GPS_OK_FIX_3D_RTK){ g.p_alt_hold.kP(1.4f); g.p_pos_xy.kP(2.6f);} // checks to initialise home and take location based pictures if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { #if CAMERA == ENABLED if (camera.update_location(current_loc) == true) { do_take_picture(); } #endif } } }
// called at 50hz void Copter::update_GPS(void) { static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message bool gps_updated = false; gps.update(); // log after every gps message for (uint8_t i=0; i<gps.num_sensors(); i++) { if (gps.last_message_time_ms(i) != last_gps_reading[i]) { last_gps_reading[i] = gps.last_message_time_ms(i); // log GPS message if (should_log(MASK_LOG_GPS)) { DataFlash.Log_Write_GPS(gps, i, current_loc.alt); } gps_updated = true; } } if (gps_updated) { // set system time if necessary set_system_time_from_GPS(); // checks to initialise home and take location based pictures if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { #if CAMERA == ENABLED if (camera.update_location(current_loc, copter.ahrs) == true) { do_take_picture(); } #endif } } }
// do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) { switch(ch_function) { case AUXSW_FLIP: // flip if switch is on, positive throttle and we're actually flying if (ch_flag == AUX_SWITCH_HIGH) { set_mode(FLIP, MODE_REASON_TX_COMMAND); } break; case AUXSW_SIMPLE_MODE: // low = simple mode off, middle or high position turns simple mode on set_simple_mode(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE); break; case AUXSW_SUPERSIMPLE_MODE: // low = simple mode off, middle = simple mode, high = super simple mode set_simple_mode(ch_flag); break; case AUXSW_RTL: if (ch_flag == AUX_SWITCH_HIGH) { // engage RTL (if not possible we remain in current flight mode) set_mode(RTL, MODE_REASON_TX_COMMAND); } else { // return to flight mode switch's flight mode if we are currently in RTL if (control_mode == RTL) { reset_control_switch(); } } break; case AUXSW_SAVE_TRIM: if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->get_control_in() == 0)) { save_trim(); } break; case AUXSW_SAVE_WP: // save waypoint when switch is brought high if (ch_flag == AUX_SWITCH_HIGH) { // do not allow saving new waypoints while we're in auto or disarmed if (control_mode == AUTO || !motors.armed()) { return; } // do not allow saving the first waypoint with zero throttle if ((mission.num_commands() == 0) && (channel_throttle->get_control_in() == 0)) { return; } // create new mission command AP_Mission::Mission_Command cmd = {}; // if the mission is empty save a takeoff command if (mission.num_commands() == 0) { // set our location ID to 16, MAV_CMD_NAV_WAYPOINT cmd.id = MAV_CMD_NAV_TAKEOFF; cmd.content.location.options = 0; cmd.p1 = 0; cmd.content.location.lat = 0; cmd.content.location.lng = 0; cmd.content.location.alt = MAX(current_loc.alt,100); // use the current altitude for the target alt for takeoff. // only altitude will matter to the AP mission script for takeoff. if (mission.add_cmd(cmd)) { // log event Log_Write_Event(DATA_SAVEWP_ADD_WP); } } // set new waypoint to current location cmd.content.location = current_loc; // if throttle is above zero, create waypoint command if (channel_throttle->get_control_in() > 0) { cmd.id = MAV_CMD_NAV_WAYPOINT; } else { // with zero throttle, create LAND command cmd.id = MAV_CMD_NAV_LAND; } // save command if (mission.add_cmd(cmd)) { // log event Log_Write_Event(DATA_SAVEWP_ADD_WP); } } break; case AUXSW_CAMERA_TRIGGER: #if CAMERA == ENABLED if (ch_flag == AUX_SWITCH_HIGH) { do_take_picture(); } #endif break; case AUXSW_RANGEFINDER: // enable or disable the rangefinder #if RANGEFINDER_ENABLED == ENABLED if ((ch_flag == AUX_SWITCH_HIGH) && (rangefinder.num_sensors() >= 1)) { rangefinder_state.enabled = true; } else { rangefinder_state.enabled = false; } #endif break; case AUXSW_FENCE: #if AC_FENCE == ENABLED // enable or disable the fence if (ch_flag == AUX_SWITCH_HIGH) { fence.enable(true); Log_Write_Event(DATA_FENCE_ENABLE); } else { fence.enable(false); Log_Write_Event(DATA_FENCE_DISABLE); } #endif break; case AUXSW_ACRO_TRAINER: switch(ch_flag) { case AUX_SWITCH_LOW: g.acro_trainer = ACRO_TRAINER_DISABLED; Log_Write_Event(DATA_ACRO_TRAINER_DISABLED); break; case AUX_SWITCH_MIDDLE: g.acro_trainer = ACRO_TRAINER_LEVELING; Log_Write_Event(DATA_ACRO_TRAINER_LEVELING); break; case AUX_SWITCH_HIGH: g.acro_trainer = ACRO_TRAINER_LIMITED; Log_Write_Event(DATA_ACRO_TRAINER_LIMITED); break; } break; case AUXSW_GRIPPER: #if GRIPPER_ENABLED == ENABLED switch(ch_flag) { case AUX_SWITCH_LOW: g2.gripper.release(); Log_Write_Event(DATA_GRIPPER_RELEASE); break; case AUX_SWITCH_HIGH: g2.gripper.grab(); Log_Write_Event(DATA_GRIPPER_GRAB); break; } #endif break; case AUXSW_SPRAYER: #if SPRAYER == ENABLED sprayer.run(ch_flag == AUX_SWITCH_HIGH); // if we are disarmed the pilot must want to test the pump sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors.armed()); #endif break; case AUXSW_AUTO: if (ch_flag == AUX_SWITCH_HIGH) { set_mode(AUTO, MODE_REASON_TX_COMMAND); } else { // return to flight mode switch's flight mode if we are currently in AUTO if (control_mode == AUTO) { reset_control_switch(); } } break; case AUXSW_AUTOTUNE: #if AUTOTUNE_ENABLED == ENABLED // turn on auto tuner switch(ch_flag) { case AUX_SWITCH_LOW: case AUX_SWITCH_MIDDLE: // restore flight mode based on flight mode switch position if (control_mode == AUTOTUNE) { reset_control_switch(); } break; case AUX_SWITCH_HIGH: // start an autotuning session set_mode(AUTOTUNE, MODE_REASON_TX_COMMAND); break; } #endif break; case AUXSW_LAND: if (ch_flag == AUX_SWITCH_HIGH) { set_mode(LAND, MODE_REASON_TX_COMMAND); } else { // return to flight mode switch's flight mode if we are currently in LAND if (control_mode == LAND) { reset_control_switch(); } } break; case AUXSW_PARACHUTE_ENABLE: #if PARACHUTE == ENABLED // Parachute enable/disable parachute.enabled(ch_flag == AUX_SWITCH_HIGH); #endif break; case AUXSW_PARACHUTE_RELEASE: #if PARACHUTE == ENABLED if (ch_flag == AUX_SWITCH_HIGH) { parachute_manual_release(); } #endif break; case AUXSW_PARACHUTE_3POS: #if PARACHUTE == ENABLED // Parachute disable, enable, release with 3 position switch switch (ch_flag) { case AUX_SWITCH_LOW: parachute.enabled(false); Log_Write_Event(DATA_PARACHUTE_DISABLED); break; case AUX_SWITCH_MIDDLE: parachute.enabled(true); Log_Write_Event(DATA_PARACHUTE_ENABLED); break; case AUX_SWITCH_HIGH: parachute.enabled(true); parachute_manual_release(); break; } #endif break; case AUXSW_MISSION_RESET: if (ch_flag == AUX_SWITCH_HIGH) { mission.reset(); } break; case AUXSW_ATTCON_FEEDFWD: // enable or disable feed forward attitude_control.bf_feedforward(ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_ATTCON_ACCEL_LIM: // enable or disable accel limiting by restoring defaults attitude_control.accel_limiting(ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_RETRACT_MOUNT: #if MOUNT == ENABLE switch (ch_flag) { case AUX_SWITCH_HIGH: camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT); break; case AUX_SWITCH_LOW: camera_mount.set_mode_to_default(); break; } #endif break; case AUXSW_RELAY: ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_RELAY2: ServoRelayEvents.do_set_relay(1, ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_RELAY3: ServoRelayEvents.do_set_relay(2, ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_RELAY4: ServoRelayEvents.do_set_relay(3, ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_LANDING_GEAR: switch (ch_flag) { case AUX_SWITCH_LOW: landinggear.set_cmd_mode(LandingGear_Deploy); break; case AUX_SWITCH_MIDDLE: landinggear.set_cmd_mode(LandingGear_Auto); break; case AUX_SWITCH_HIGH: landinggear.set_cmd_mode(LandingGear_Retract); break; } break; case AUXSW_LOST_COPTER_SOUND: switch (ch_flag) { case AUX_SWITCH_HIGH: AP_Notify::flags.vehicle_lost = true; break; case AUX_SWITCH_LOW: AP_Notify::flags.vehicle_lost = false; break; } break; case AUXSW_MOTOR_ESTOP: // Turn on Emergency Stop logic when channel is high set_motor_emergency_stop(ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_MOTOR_INTERLOCK: // Turn on when above LOW, because channel will also be used for speed // control signal in tradheli ap.motor_interlock_switch = (ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE); break; case AUXSW_BRAKE: // brake flight mode if (ch_flag == AUX_SWITCH_HIGH) { set_mode(BRAKE, MODE_REASON_TX_COMMAND); } else { // return to flight mode switch's flight mode if we are currently in BRAKE if (control_mode == BRAKE) { reset_control_switch(); } } break; case AUXSW_THROW: // throw flight mode if (ch_flag == AUX_SWITCH_HIGH) { set_mode(THROW, MODE_REASON_TX_COMMAND); } else { // return to flight mode switch's flight mode if we are currently in throw mode if (control_mode == THROW) { reset_control_switch(); } } break; case AUXSW_AVOID_ADSB: // enable or disable AP_Avoidance if (ch_flag == AUX_SWITCH_HIGH) { avoidance_adsb.enable(); Log_Write_Event(DATA_AVOIDANCE_ADSB_ENABLE); } else { avoidance_adsb.disable(); Log_Write_Event(DATA_AVOIDANCE_ADSB_DISABLE); } break; case AUXSW_PRECISION_LOITER: #if PRECISION_LANDING == ENABLED switch (ch_flag) { case AUX_SWITCH_HIGH: set_precision_loiter_enabled(true); break; case AUX_SWITCH_LOW: set_precision_loiter_enabled(false); break; } #endif break; case AUXSW_AVOID_PROXIMITY: #if PROXIMITY_ENABLED == ENABLED switch (ch_flag) { case AUX_SWITCH_HIGH: avoid.proximity_avoidance_enable(true); Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_ENABLE); break; case AUX_SWITCH_LOW: avoid.proximity_avoidance_enable(false); Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_DISABLE); break; } #endif break; } }