/*
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;
}
}
