/*
update async airspeed calibration
*/
void AP_Airspeed::update_calibration(float raw_pressure)
{
// consider calibration complete when we have at least 15 samples
// over at least 1 second
if (AP_HAL::millis() - _cal.start_ms >= 1000 &&
_cal.read_count > 15) {
if (_cal.count == 0) {
gcs().send_text(MAV_SEVERITY_INFO, "Airspeed sensor unhealthy");
} else {
gcs().send_text(MAV_SEVERITY_INFO, "Airspeed sensor calibrated");
_offset.set_and_save(_cal.sum / _cal.count);
}
_cal.start_ms = 0;
return;
}
// we discard the first 5 samples
if (_healthy && _cal.read_count > 5) {
_cal.sum += raw_pressure;
_cal.count++;
}
_cal.read_count++;
}
void AP_Arming::check_failed(const enum AP_Arming::ArmingChecks check, bool report, const char *fmt, ...) const
{
if (!report) {
return;
}
char taggedfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
hal.util->snprintf((char*)taggedfmt, sizeof(taggedfmt)-1, "PreArm: %s", fmt);
MAV_SEVERITY severity = check_severity(check);
va_list arg_list;
va_start(arg_list, fmt);
gcs().send_textv(severity, taggedfmt, arg_list);
va_end(arg_list);
}
void AP_ADSB::handle_transceiver_report(const mavlink_channel_t chan, const mavlink_message_t* msg)
{
mavlink_uavionix_adsb_transceiver_health_report_t packet {};
mavlink_msg_uavionix_adsb_transceiver_health_report_decode(msg, &packet);
if (out_state.chan != chan) {
gcs().send_text(MAV_SEVERITY_DEBUG, "ADSB: Found transceiver on channel %d", chan);
}
out_state.chan_last_ms = AP_HAL::millis();
out_state.chan = chan;
out_state.status = (UAVIONIX_ADSB_RF_HEALTH)packet.rfHealth;
}
// these are basically the same checks as in AP_Arming:
bool Mode::enter_gps_checks() const
{
const AP_GPS &gps = AP::gps();
if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Bad GPS Position");
return false;
}
//GPS update rate acceptable
if (!gps.is_healthy()) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS is not healthy");
}
// check GPSs are within 50m of each other and that blending is healthy
float distance_m;
if (!gps.all_consistent(distance_m)) {
gcs().send_text(MAV_SEVERITY_CRITICAL,
"GPS positions differ by %4.1fm",
(double)distance_m);
return false;
}
if (!gps.blend_health_check()) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS blending unhealthy");
return false;
}
// check AHRS and GPS are within 10m of each other
const Location gps_loc = gps.location();
Location ahrs_loc;
if (ahrs.get_position(ahrs_loc)) {
float distance = location_3d_diff_NED(gps_loc, ahrs_loc).length();
if (distance > MODE_AHRS_GPS_ERROR_MAX) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS and AHRS differ by %4.1fm", (double)distance);
return false;
}
}
return true;
}
// do_nav_delay - Delay the next navigation command
void Copter::ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
{
nav_delay_time_start = millis();
if (cmd.content.nav_delay.seconds > 0) {
// relative delay
nav_delay_time_max = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds
} else {
// absolute delay to utc time
nav_delay_time_max = hal.util->get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0);
}
gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec",(unsigned int)(nav_delay_time_max/1000));
}
// check to see if current command goal has been achieved
// called by mission library in mission.update()
bool Sub::verify_command_callback(const AP_Mission::Mission_Command& cmd)
{
if (control_mode == AUTO) {
bool cmd_complete = verify_command(cmd);
// send message to GCS
if (cmd_complete) {
gcs().send_mission_item_reached_message(cmd.index);
}
return cmd_complete;
}
return false;
}
MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration_baro()
{
if (sub.motors.armed()) {
gcs().send_text(MAV_SEVERITY_INFO, "Disarm before calibration.");
return MAV_RESULT_FAILED;
}
if (!sub.control_check_barometer()) {
return MAV_RESULT_FAILED;
}
AP::baro().calibrate(true);
return MAV_RESULT_ACCEPTED;
}
/**
* Set Simple mode
*
* @param [in] b 0:false or disabled, 1:true or SIMPLE, 2:SUPERSIMPLE
*/
void Copter::set_simple_mode(uint8_t b)
{
if (ap.simple_mode != b) {
switch (b) {
case 0:
Log_Write_Event(DATA_SET_SIMPLE_OFF);
gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode off");
break;
case 1:
Log_Write_Event(DATA_SET_SIMPLE_ON);
gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode on");
break;
case 2:
default:
// initialise super simple heading
update_super_simple_bearing(true);
Log_Write_Event(DATA_SET_SUPERSIMPLE_ON);
gcs().send_text(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on");
break;
}
ap.simple_mode = b;
}
}
bool AP_Landing_Deepstall::override_servos(void)
{
if (stage != DEEPSTALL_STAGE_LAND) {
return false;
}
SRV_Channel* elevator = SRV_Channels::get_channel_for(SRV_Channel::k_elevator);
if (elevator == nullptr) {
// deepstalls are impossible without these channels, abort the process
gcs().send_text(MAV_SEVERITY_CRITICAL, "Deepstall: Unable to find the elevator channels");
request_go_around();
return false;
}
// calculate the progress on slewing the elevator
float slew_progress = 1.0f;
if (slew_speed > 0) {
slew_progress = (AP_HAL::millis() - stall_entry_time) / (100.0f * slew_speed);
}
// mix the elevator to the correct value
elevator->set_output_pwm(linear_interpolate(initial_elevator_pwm, elevator_pwm,
slew_progress, 0.0f, 1.0f));
// use the current airspeed to dictate the travel limits
float airspeed;
landing.ahrs.airspeed_estimate(&airspeed);
// only allow the deepstall steering controller to run below the handoff airspeed
if (slew_progress >= 1.0f || airspeed <= handoff_airspeed) {
// run the steering conntroller
float pid = update_steering();
float travel_limit = constrain_float((handoff_airspeed - airspeed) /
(handoff_airspeed - handoff_lower_limit_airspeed) *
0.5f + 0.5f,
0.5f, 1.0f);
float output = constrain_float(pid, -travel_limit, travel_limit);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, output*4500*aileron_scalar);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, output*4500);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); // this will normally be managed as part of landing,
// but termination needs to set throttle control here
}
// hand off rudder control to deepstall controlled
return true;
}
bool AP_Arming::barometer_checks(bool report)
{
if ((checks_to_perform & ARMING_CHECK_ALL) ||
(checks_to_perform & ARMING_CHECK_BARO)) {
if (!AP::baro().all_healthy()) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Barometer not healthy");
}
return false;
}
}
return true;
}
// Set the NED origin to be used until the next filter reset
void NavEKF3_core::setOrigin()
{
// assume origin at current GPS location (no averaging)
EKF_origin = _ahrs->get_gps().location();
// if flying, correct for height change from takeoff so that the origin is at field elevation
if (inFlight) {
EKF_origin.alt += (int32_t)(100.0f * stateStruct.position.z);
}
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
// define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
validOrigin = true;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u Origin set to GPS",(unsigned)imu_index);
}
/*
update home location from GPS
this is called as long as we have 3D lock and the arming switch is
not pushed
*/
void Rover::update_home()
{
if (home_is_set == HOME_SET_NOT_LOCKED) {
Location loc;
if (ahrs.get_position(loc)) {
if (get_distance(loc, ahrs.get_home()) > DISTANCE_HOME_MAX) {
ahrs.set_home(loc);
Log_Write_Home_And_Origin();
gcs().send_home(gps.location());
}
}
}
barometer.update_calibration();
}
// terrain failsafe action
void Copter::failsafe_terrain_on_event()
{
failsafe.terrain = true;
gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing");
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_FAILSAFE_OCCURRED);
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else if (control_mode == RTL) {
mode_rtl.restart_without_terrain();
} else {
set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE);
}
}
// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
bool Copter::ModeAuto::verify_command_callback(const AP_Mission::Mission_Command& cmd)
{
if (_copter.flightmode == &_copter.mode_auto) {
bool cmd_complete = verify_command(cmd);
// send message to GCS
if (cmd_complete) {
gcs().send_mission_item_reached_message(cmd.index);
}
return cmd_complete;
}
return false;
}
// update GCS based termination
// returns true if AFS is in the desired termination state
bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate) {
if (!_enable) {
gcs().send_text(MAV_SEVERITY_INFO, "AFS not enabled, can't terminate the vehicle");
return false;
}
_terminate.set_and_notify(should_terminate ? 1 : 0);
// evaluate if we will crash or not, as AFS must be enabled, and TERM_ACTION has to be correct
bool is_terminating = should_crash_vehicle();
if(should_terminate == is_terminating) {
if (is_terminating) {
gcs().send_text(MAV_SEVERITY_INFO, "Terminating due to GCS request");
} else {
gcs().send_text(MAV_SEVERITY_INFO, "Aborting termination due to GCS request");
}
return true;
} else if (should_terminate && _terminate_action != TERMINATE_ACTION_TERMINATE) {
gcs().send_text(MAV_SEVERITY_INFO, "Unable to terminate, termination is not configured");
}
return false;
}
/*
If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
only called from AP_Landing, when the landing library is ready to disarm
*/
void Plane::disarm_if_autoland_complete()
{
if (landing.get_disarm_delay() > 0 &&
!is_flying() &&
arming.arming_required() != AP_Arming::NO &&
arming.is_armed()) {
/* we have auto disarm enabled. See if enough time has passed */
if (millis() - auto_state.last_flying_ms >= landing.get_disarm_delay()*1000UL) {
if (disarm_motors()) {
gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed");
}
}
}
}
// parameter_check - check if helicopter specific parameters are sensible
bool AP_MotorsHeli::parameter_check(bool display_msg) const
{
// returns false if _rsc_setpoint is not higher than _rsc_critical as this would not allow rotor_runup_complete to ever return true
if (_rsc_critical >= _rsc_setpoint) {
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_CRITICAL too large");
}
return false;
}
// returns false if RSC Mode is not set to a valid control mode
if (_rsc_mode <= (int8_t)ROTOR_CONTROL_MODE_DISABLED || _rsc_mode > (int8_t)ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) {
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_MODE invalid");
}
return false;
}
// returns false if RSC Runup Time is less than Ramp time as this could cause undesired behaviour of rotor speed estimate
if (_rsc_runup_time <= _rsc_ramp_time){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RUNUP_TIME too small");
}
return false;
}
// returns false if idle output is higher than critical rotor speed as this could block runup_complete from going false
if ( _rsc_idle_output >= _rsc_critical){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_RSC_IDLE too large");
}
return false;
}
// all other cases parameters are OK
return true;
}
// init_disarm_motors - disarm motors
void Copter::init_disarm_motors()
{
// return immediately if we are already disarmed
if (!motors->armed()) {
return;
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors");
#endif
// save compass offsets learned by the EKF if enabled
if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) {
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
Vector3f magOffsets;
if (ahrs.getMagOffsets(i, magOffsets)) {
compass.set_and_save_offsets(i, magOffsets);
}
}
}
#if AUTOTUNE_ENABLED == ENABLED
// save auto tuned parameters
mode_autotune.save_tuning_gains();
#endif
// we are not in the air
set_land_complete(true);
set_land_complete_maybe(true);
// log disarm to the dataflash
Log_Write_Event(DATA_DISARMED);
// send disarm command to motors
motors->armed(false);
#if MODE_AUTO_ENABLED == ENABLED
// reset the mission
mission.reset();
#endif
DataFlash_Class::instance()->set_vehicle_armed(false);
// disable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(false);
hal.util->set_soft_armed(false);
ap.in_arming_delay = false;
}
bool AP_Arming::board_voltage_checks(bool report)
{
#if HAL_HAVE_BOARD_VOLTAGE
// check board voltage
if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) {
if(((hal.analogin->board_voltage() < AP_ARMING_BOARD_VOLTAGE_MIN) || (hal.analogin->board_voltage() > AP_ARMING_BOARD_VOLTAGE_MAX))) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check board voltage");
}
return false;
}
}
#endif
return true;
}
/*
setup a callback for a feedback pin. When on PX4 with the right FMU
mode we can use the microsecond timer.
*/
void AP_Camera::setup_feedback_callback(void)
{
if (_feedback_pin <= 0 || _timer_installed) {
// invalid or already installed
return;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
/*
special case for pin 53 on PX4. We can use the fast timer support
*/
if (_feedback_pin == 53) {
int fd = open("/dev/px4fmu", 0);
if (fd != -1) {
if (ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_MODE_3PWM1CAP) != 0) {
gcs().send_text(MAV_SEVERITY_WARNING, "Camera: unable to setup 3PWM1CAP");
close(fd);
goto failed;
}
if (up_input_capture_set(3, _feedback_polarity==1?Rising:Falling, 0, capture_callback, this) != 0) {
gcs().send_text(MAV_SEVERITY_WARNING, "Camera: unable to setup timer capture");
close(fd);
goto failed;
}
close(fd);
_timer_installed = true;
gcs().send_text(MAV_SEVERITY_WARNING, "Camera: setup fast trigger capture");
}
}
failed:
#endif // CONFIG_HAL_BOARD
if (!_timer_installed) {
// install a 1kHz timer to check feedback pin
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Camera::feedback_pin_timer, void));
}
void Sub::perf_update(void)
{
if (should_log(MASK_LOG_PM)) {
Log_Write_Performance();
}
if (scheduler.debug()) {
gcs().send_text(MAV_SEVERITY_WARNING, "PERF: %u/%u %lu %lu",
(unsigned)perf_info_get_num_long_running(),
(unsigned)perf_info_get_num_loops(),
(unsigned long)perf_info_get_max_time(),
(unsigned long)perf_info_get_min_time());
}
perf_info_reset();
pmTest1 = 0;
}
// parachute_release - trigger the release of the parachute, disarm the motors and notify the user
void Copter::parachute_release()
{
// send message to gcs and dataflash
gcs().send_text(MAV_SEVERITY_INFO,"Parachute: Released");
Log_Write_Event(DATA_PARACHUTE_RELEASED);
// disarm motors
init_disarm_motors();
// release parachute
parachute.release();
// deploy landing gear
landinggear.set_position(AP_LandingGear::LandingGear_Deploy);
}
/*
handle DO_ENGINE_CONTROL messages via MAVLink or mission
*/
bool AP_ICEngine::engine_control(float start_control, float cold_start, float height_delay)
{
if (start_control <= 0) {
state = ICE_OFF;
return true;
}
if (start_chan != 0) {
// get starter control channel
if (hal.rcin->read(start_chan-1) <= 1300) {
gcs().send_text(MAV_SEVERITY_INFO, "Engine: start control disabled");
return false;
}
}
if (height_delay > 0) {
height_pending = true;
initial_height = 0;
height_required = height_delay;
state = ICE_START_HEIGHT_DELAY;
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff height set to %.1fm", (double)height_delay);
return true;
}
state = ICE_STARTING;
return true;
}
// This interface assumes that it's being called by the
// vm thread. It collects the heap assuming that the
// heap lock is already held and that we are executing in
// the context of the vm thread.
void ParallelScavengeHeap::collect_as_vm_thread(GCCause::Cause cause) {
assert(Thread::current()->is_VM_thread(), "Precondition#1");
assert(Heap_lock->is_locked(), "Precondition#2");
GCCauseSetter gcs(this, cause);
switch (cause) {
case GCCause::_heap_inspection:
case GCCause::_heap_dump: {
HandleMark hm;
invoke_full_gc(false);
break;
}
default: // XXX FIX ME
ShouldNotReachHere();
}
}
// returns true if checks pass, false if they fail. report should be true to send text messages to GCS
bool AP_MotorsUGV::pre_arm_check(bool report) const
{
// check if both regular and skid steering functions have been defined
if (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) &&
SRV_Channels::function_assigned(SRV_Channel::k_throttleRight) &&
SRV_Channels::function_assigned(SRV_Channel::k_throttle) &&
SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: regular AND skid steering configured");
}
return false;
}
// check if only one of skid-steering output has been configured
if (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) != SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: check skid steering config");
}
return false;
}
// check if only one of throttle or steering outputs has been configured
if (SRV_Channels::function_assigned(SRV_Channel::k_throttle) != SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: check steering and throttle config");
}
return false;
}
// check if only one of the omni rover outputs has been configured
if ((SRV_Channels::function_assigned(SRV_Channel::k_motor1)) != (SRV_Channels::function_assigned(SRV_Channel::k_motor2)) ||
(SRV_Channels::function_assigned(SRV_Channel::k_motor1)) != (SRV_Channels::function_assigned(SRV_Channel::k_motor3)) ||
(SRV_Channels::function_assigned(SRV_Channel::k_motor2)) != (SRV_Channels::function_assigned(SRV_Channel::k_motor3))) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: check motor 1, motor2 and motor3 config");
}
}
return true;
}
float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const float height, const bool print)
{
bool reverse = false;
float course = radians(target_heading_deg);
// a forward speed of 0 will result in a divide by 0
float forward_speed_ms = MAX(forward_speed, 0.1f);
Vector2f wind_vec(wind.x, wind.y); // work with the 2D component of wind
float wind_length = MAX(wind_vec.length(), 0.05f); // always assume a slight wind to avoid divide by 0
Vector2f course_vec(cosf(course), sinf(course));
float offset = course - atan2f(-wind.y, -wind.x);
// estimator for how far the aircraft will travel while entering the stall
float stall_distance = slope_a * wind_length * cosf(offset) + slope_b;
float theta = acosf(constrain_float((wind_vec * course_vec) / wind_length, -1.0f, 1.0f));
if ((course_vec % wind_vec) > 0) {
reverse = true;
theta *= -1;
}
float cross_component = sinf(theta) * wind_length;
float estimated_crab_angle = asinf(constrain_float(cross_component / forward_speed_ms, -1.0f, 1.0f));
if (reverse) {
estimated_crab_angle *= -1;
}
float estimated_forward = cosf(estimated_crab_angle) * forward_speed_ms + cosf(theta) * wind_length;
if (is_positive(down_speed)) {
predicted_travel_distance = (estimated_forward * height / down_speed) + stall_distance;
} else {
// if we don't have a sane downward speed in a deepstall (IE not zero, and not
// an ascent) then just provide the stall_distance as a reasonable approximation
predicted_travel_distance = stall_distance;
}
if(print) {
// allow printing the travel distances on the final entry as its used for tuning
gcs().send_text(MAV_SEVERITY_INFO, "Deepstall: Entry: %0.1f (m) Travel: %0.1f (m)",
(double)stall_distance, (double)predicted_travel_distance);
}
return predicted_travel_distance;
}
bool AP_Arming::hardware_safety_check(bool report)
{
if ((checks_to_perform & ARMING_CHECK_ALL) ||
(checks_to_perform & ARMING_CHECK_SWITCH)) {
// check if safety switch has been pushed
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Hardware safety switch");
}
return false;
}
}
return true;
}
/*
set the flight stage
*/
void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs)
{
if (fs == flight_stage) {
return;
}
landing.handle_flight_stage_change(fs == AP_Vehicle::FixedWing::FLIGHT_LAND);
if (fs == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm",
auto_state.takeoff_altitude_rel_cm/100);
}
flight_stage = fs;
Log_Write_Status();
}
bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
{
switch (cmd.id) {
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlimited(cmd);
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time(cmd);
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
case MAV_CMD_NAV_SET_YAW_SPEED:
return verify_nav_set_yaw_speed();
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_REPEAT_SERVO:
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CONTROL_VIDEO:
case MAV_CMD_DO_DIGICAM_CONFIGURE:
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_SET_ROI:
case MAV_CMD_DO_SET_REVERSE:
case MAV_CMD_DO_FENCE_ENABLE:
return true;
default:
// error message
gcs().send_text(MAV_SEVERITY_WARNING, "Skipping invalid cmd #%i", cmd.id);
// return true if we do not recognize the command so that we move on to the next command
return true;
}
}
void Tracker::send_heartbeat(mavlink_channel_t chan)
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
// work out the base_mode. This value is not very useful
// for APM, but we calculate it as best we can so a generic
// MAVLink enabled ground station can work out something about
// what the MAV is up to. The actual bit values are highly
// ambiguous for most of the APM flight modes. In practice, you
// only get useful information from the custom_mode, which maps to
// the APM flight mode and has a well defined meaning in the
// ArduPlane documentation
switch (control_mode) {
case MANUAL:
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case STOP:
break;
case SCAN:
case SERVO_TEST:
case AUTO:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED |
MAV_MODE_FLAG_STABILIZE_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
// APM does in any mode, as that is defined as "system finds its own goal
// positions", which APM does not currently do
break;
case INITIALISING:
system_status = MAV_STATE_CALIBRATING;
break;
}
// we are armed if safety switch is not disarmed
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(MAV_TYPE_ANTENNA_TRACKER,
base_mode,
custom_mode,
system_status);
}
