// log_picture - log picture taken and send feedback to GCS
void Rover::log_picture()
{
gcs_send_message(MSG_CAMERA_FEEDBACK);
if (should_log(MASK_LOG_CAMERA)) {
DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
}
}
void Plane::one_second_loop()
{
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// make it possible to change control channel ordering at runtime
set_control_channels();
// make it possible to change orientation at runtime
ahrs.set_orientation();
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
update_aux();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
#if AP_TERRAIN_AVAILABLE
if (should_log(MASK_LOG_GPS)) {
terrain.log_terrain_data(DataFlash);
}
#endif
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
}
// fifty_hz_logging_loop
// should be run at 50hz
void Copter::fifty_hz_logging_loop()
{
#if HIL_MODE != HIL_MODE_DISABLED
// HIL for a copter needs very fast update of the servo values
gcs_send_message(MSG_RADIO_OUT);
#endif
#if HIL_MODE == HIL_MODE_DISABLED
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
Log_Write_Rate();
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
}
}
// log IMU data if we're not already logging at the higher rate
if (should_log(MASK_LOG_IMU) && !(should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW))) {
DataFlash.Log_Write_IMU(ins);
}
#endif
}
// twentyfive_hz_logging_loop
// should be run at 25hz
void Sub::twentyfive_hz_logging()
{
#if HIL_MODE != HIL_MODE_DISABLED
// HIL for a copter needs very fast update of the servo values
gcs_send_message(MSG_RADIO_OUT);
#endif
#if HIL_MODE == HIL_MODE_DISABLED
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
}
}
// log IMU data if we're not already logging at the higher rate
if (should_log(MASK_LOG_IMU) && should_log(MASK_LOG_IMU_RAW)) {
DataFlash.Log_Write_IMU(ins);
}
#endif
}
// log_picture - log picture taken and send feedback to GCS
void Plane::log_picture()
{
#if CAMERA == ENABLED
gcs_send_message(MSG_CAMERA_FEEDBACK);
if (should_log(MASK_LOG_CAMERA)) {
DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
}
#endif
}
void Plane::one_second_loop()
{
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// make it possible to change control channel ordering at runtime
set_control_channels();
#if HAVE_PX4_MIXER
if (!hal.util->get_soft_armed() && (last_mixer_crc == -1)) {
// if disarmed try to configure the mixer
setup_failsafe_mixing();
}
#endif // CONFIG_HAL_BOARD
// make it possible to change orientation at runtime
ahrs.set_orientation();
adsb.set_stall_speed_cm(aparm.airspeed_min);
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
update_aux();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
#if AP_TERRAIN_AVAILABLE
if (should_log(MASK_LOG_GPS)) {
terrain.log_terrain_data(DataFlash);
}
#endif
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
// update home position if soft armed and gps position has
// changed. Update every 5s at most
if (!hal.util->get_soft_armed() &&
gps.last_message_time_ms() - last_home_update_ms > 5000 &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_home_update_ms = gps.last_message_time_ms();
update_home();
// reset the landing altitude correction
auto_state.land_alt_offset = 0;
}
// update error mask of sensors and subsystems. The mask uses the
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
// indicates that the sensor or subsystem is present but not
// functioning correctly
update_sensor_status_flags();
}
/*
* allocate and fill the geofence state structure
*/
void Plane::geofence_load(void)
{
uint8_t i;
if (geofence_state == nullptr) {
uint16_t boundary_size = sizeof(Vector2l) * max_fencepoints();
if (hal.util->available_memory() < 100 + boundary_size + sizeof(struct GeofenceState)) {
// too risky to enable as we could run out of stack
goto failed;
}
geofence_state = (struct GeofenceState *)calloc(1, sizeof(struct GeofenceState));
if (geofence_state == nullptr) {
// not much we can do here except disable it
goto failed;
}
geofence_state->boundary = (Vector2l *)calloc(1, boundary_size);
if (geofence_state->boundary == nullptr) {
free(geofence_state);
geofence_state = nullptr;
goto failed;
}
geofence_state->old_switch_position = 254;
}
if (g.fence_total <= 0) {
g.fence_total.set(0);
return;
}
for (i=0; i<g.fence_total; i++) {
geofence_state->boundary[i] = get_fence_point_with_index(i);
}
geofence_state->num_points = i;
if (!Polygon_complete(&geofence_state->boundary[1], geofence_state->num_points-1)) {
// first point and last point must be the same
goto failed;
}
if (Polygon_outside(geofence_state->boundary[0], &geofence_state->boundary[1], geofence_state->num_points-1)) {
// return point needs to be inside the fence
goto failed;
}
geofence_state->boundary_uptodate = true;
geofence_state->fence_triggered = false;
gcs_send_text(MAV_SEVERITY_INFO,"Geofence loaded");
gcs_send_message(MSG_FENCE_STATUS);
return;
failed:
g.fence_action.set(FENCE_ACTION_NONE);
gcs_send_text(MAV_SEVERITY_WARNING,"Geofence setup error");
}
/*
once a second events
*/
void Rover::one_second_loop(void)
{
if (should_log(MASK_LOG_CURRENT)) {
Log_Write_Current();
}
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// allow orientation change at runtime to aid config
ahrs.set_orientation();
set_control_channels();
// cope with changes to aux functions
update_aux();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
// cope with changes to mavlink system ID
mavlink_system.sysid = g.sysid_this_mav;
static uint8_t counter;
counter++;
// write perf data every 20s
if (counter % 10 == 0) {
if (scheduler.debug() != 0) {
hal.console->printf("G_Dt_max=%u\n", G_Dt_max);
}
if (should_log(MASK_LOG_PM)) {
Log_Write_Performance();
}
G_Dt_max = 0;
resetPerfData();
}
// save compass offsets once a minute
if (counter >= 60) {
if (g.compass_enabled) {
compass.save_offsets();
}
counter = 0;
}
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
// update error mask of sensors and subsystems. The mask uses the
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
// indicates that the sensor or subsystem is present but not
// functioning correctly
update_sensor_status_flags();
}
/*
update camera trigger - 50Hz
*/
void Rover::update_trigger(void)
{
#if CAMERA == ENABLED
camera.trigger_pic_cleanup();
if (camera.check_trigger_pin()) {
gcs_send_message(MSG_CAMERA_FEEDBACK);
if (should_log(MASK_LOG_CAMERA)) {
DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
}
}
#endif
}
// log_picture - log picture taken and send feedback to GCS
void Copter::log_picture()
{
if (!camera.using_feedback_pin()) {
gcs_send_message(MSG_CAMERA_FEEDBACK);
if (should_log(MASK_LOG_CAMERA)) {
DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
}
} else {
if (should_log(MASK_LOG_CAMERA)) {
DataFlash.Log_Write_Trigger(ahrs, gps, current_loc);
}
}
}
/*
once a second events
*/
void Rover::one_second_loop(void)
{
if (should_log(MASK_LOG_CURRENT))
Log_Write_Current();
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// allow orientation change at runtime to aid config
ahrs.set_orientation();
set_control_channels();
// cope with changes to aux functions
update_aux();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
// cope with changes to mavlink system ID
mavlink_system.sysid = g.sysid_this_mav;
static uint8_t counter;
counter++;
// write perf data every 20s
if (counter % 10 == 0) {
if (scheduler.debug() != 0) {
hal.console->printf("G_Dt_max=%lu\n", (unsigned long)G_Dt_max);
}
if (should_log(MASK_LOG_PM))
Log_Write_Performance();
G_Dt_max = 0;
resetPerfData();
}
// save compass offsets once a minute
if (counter >= 60) {
if (g.compass_enabled) {
compass.save_offsets();
}
counter = 0;
}
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
}
void Plane::one_second_loop()
{
if (should_log(MASK_LOG_CURRENT))
Log_Write_Current();
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// make it possible to change control channel ordering at runtime
set_control_channels();
// make it possible to change orientation at runtime
ahrs.set_orientation();
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
update_aux();
// determine if we are flying or not
determine_is_flying();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
#if AP_TERRAIN_AVAILABLE
if (should_log(MASK_LOG_GPS)) {
terrain.log_terrain_data(DataFlash);
}
#endif
// piggyback the status log entry on the MODE log entry flag
if (should_log(MASK_LOG_MODE)) {
Log_Write_Status();
}
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
}
void Plane::one_second_loop()
{
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// make it possible to change control channel ordering at runtime
set_control_channels();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
if (!hal.util->get_soft_armed() && (last_mixer_crc == -1)) {
// if disarmed try to configure the mixer
setup_failsafe_mixing();
}
#endif // CONFIG_HAL_BOARD
// make it possible to change orientation at runtime
ahrs.set_orientation();
adsb.set_stall_speed_cm(airspeed.get_airspeed_min());
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
update_aux();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
#if AP_TERRAIN_AVAILABLE
if (should_log(MASK_LOG_GPS)) {
terrain.log_terrain_data(DataFlash);
}
#endif
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
}
void Tracker::one_second_loop()
{
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// make it possible to change orientation at runtime
ahrs.set_orientation();
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
// updated armed/disarmed status LEDs
update_armed_disarmed();
one_second_counter++;
if (one_second_counter >= 60) {
if(g.compass_enabled) {
compass.save_offsets();
}
one_second_counter = 0;
}
}
/**
retry any deferred messages
*/
void Tracker::gcs_retry_deferred(void)
{
gcs_send_message(MSG_RETRY_DEFERRED);
}
/*
* check if we have breached the geo-fence
*/
void Plane::geofence_check(bool altitude_check_only)
{
if (!geofence_enabled()) {
// switch back to the chosen control mode if still in
// GUIDED to the return point
if (geofence_state != nullptr &&
(g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS || g.fence_action == FENCE_ACTION_RTL) &&
(control_mode == GUIDED || control_mode == AVOID_ADSB) &&
geofence_present() &&
geofence_state->boundary_uptodate &&
geofence_state->old_switch_position == oldSwitchPosition &&
guided_WP_loc.lat == geofence_state->guided_lat &&
guided_WP_loc.lng == geofence_state->guided_lng) {
geofence_state->old_switch_position = 254;
set_mode(get_previous_mode(), MODE_REASON_GCS_COMMAND);
}
return;
}
/* allocate the geo-fence state if need be */
if (geofence_state == nullptr || !geofence_state->boundary_uptodate) {
geofence_load();
if (!geofence_enabled()) {
// may have been disabled by load
return;
}
}
bool outside = false;
uint8_t breach_type = FENCE_BREACH_NONE;
struct Location loc;
// Never trigger a fence breach in the final stage of landing
if (landing.is_expecting_impact()) {
return;
}
if (geofence_state->floor_enabled && geofence_check_minalt()) {
outside = true;
breach_type = FENCE_BREACH_MINALT;
} else if (geofence_check_maxalt()) {
outside = true;
breach_type = FENCE_BREACH_MAXALT;
} else if (!altitude_check_only && ahrs.get_position(loc)) {
Vector2l location;
location.x = loc.lat;
location.y = loc.lng;
outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
if (outside) {
breach_type = FENCE_BREACH_BOUNDARY;
}
}
if (!outside) {
if (geofence_state->fence_triggered && !altitude_check_only) {
// we have moved back inside the fence
geofence_state->fence_triggered = false;
gcs_send_text(MAV_SEVERITY_INFO,"Geofence OK");
#if FENCE_TRIGGERED_PIN > 0
hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->write(FENCE_TRIGGERED_PIN, 0);
#endif
gcs_send_message(MSG_FENCE_STATUS);
}
// we're inside, all is good with the world
return;
}
// we are outside the fence
if (geofence_state->fence_triggered &&
(control_mode == GUIDED || control_mode == AVOID_ADSB || control_mode == RTL || g.fence_action == FENCE_ACTION_REPORT)) {
// we have already triggered, don't trigger again until the
// user disables/re-enables using the fence channel switch
return;
}
// we are outside, and have not previously triggered.
geofence_state->fence_triggered = true;
geofence_state->breach_count++;
geofence_state->breach_time = millis();
geofence_state->breach_type = breach_type;
#if FENCE_TRIGGERED_PIN > 0
hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->write(FENCE_TRIGGERED_PIN, 1);
#endif
gcs_send_text(MAV_SEVERITY_NOTICE,"Geofence triggered");
gcs_send_message(MSG_FENCE_STATUS);
// see what action the user wants
switch (g.fence_action) {
case FENCE_ACTION_REPORT:
break;
case FENCE_ACTION_GUIDED:
case FENCE_ACTION_GUIDED_THR_PASS:
case FENCE_ACTION_RTL:
// make sure we don't auto trim the surfaces on this mode change
int8_t saved_auto_trim = g.auto_trim;
g.auto_trim.set(0);
if (g.fence_action == FENCE_ACTION_RTL) {
set_mode(RTL, MODE_REASON_FENCE_BREACH);
} else {
set_mode(GUIDED, MODE_REASON_FENCE_BREACH);
}
g.auto_trim.set(saved_auto_trim);
if (g.fence_ret_rally != 0 || g.fence_action == FENCE_ACTION_RTL) { //return to a rally point
guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
} else { //return to fence return point, not a rally point
if (g.fence_retalt > 0) {
//fly to the return point using fence_retalt
guided_WP_loc.alt = home.alt + 100.0f*g.fence_retalt;
} else if (g.fence_minalt >= g.fence_maxalt) {
// invalid min/max, use RTL_altitude
guided_WP_loc.alt = home.alt + g.RTL_altitude_cm;
} else {
// fly to the return point, with an altitude half way between
// min and max
guided_WP_loc.alt = home.alt + 100.0f*(g.fence_minalt + g.fence_maxalt)/2;
}
guided_WP_loc.options = 0;
guided_WP_loc.lat = geofence_state->boundary[0].x;
guided_WP_loc.lng = geofence_state->boundary[0].y;
}
geofence_state->guided_lat = guided_WP_loc.lat;
geofence_state->guided_lng = guided_WP_loc.lng;
geofence_state->old_switch_position = oldSwitchPosition;
if (g.fence_action != FENCE_ACTION_RTL) { //not needed for RTL mode
setup_terrain_target_alt(guided_WP_loc);
set_guided_WP();
}
if (g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) {
guided_throttle_passthru = true;
}
break;
}
}
/**
retry any deferred messages
*/
void Tracker::gcs_retry_deferred(void)
{
gcs_send_message(MSG_RETRY_DEFERRED);
gcs().service_statustext();
}