mirror of https://github.com/ArduPilot/ardupilot
Plane: update severities
This commit is contained in:
parent
dfc35cca8b
commit
583c087eca
|
@ -625,7 +625,7 @@ void Plane::update_flight_mode(void)
|
|||
if (tdrag_mode && !auto_state.fbwa_tdrag_takeoff_mode) {
|
||||
if (auto_state.highest_airspeed < g.takeoff_tdrag_speed1) {
|
||||
auto_state.fbwa_tdrag_takeoff_mode = true;
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("FBWA tdrag mode\n"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("FBWA tdrag mode\n"));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -769,15 +769,15 @@ void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs)
|
|||
#if GEOFENCE_ENABLED == ENABLED
|
||||
if (g.fence_autoenable == 1) {
|
||||
if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Disable fence failed (autodisable)"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Disable fence failed (autodisable)"));
|
||||
} else {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence disabled (autodisable)"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Fence disabled (autodisable)"));
|
||||
}
|
||||
} else if (g.fence_autoenable == 2) {
|
||||
if (! geofence_set_floor_enabled(false)) {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Disable fence floor failed (autodisable)"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Disable fence floor failed (autodisable)"));
|
||||
} else {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence floor disabled (auto disable)"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Fence floor disabled (auto disable)"));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -1025,7 +1025,7 @@ void Plane::set_servos(void)
|
|||
void Plane::demo_servos(uint8_t i)
|
||||
{
|
||||
while(i > 0) {
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("Demo Servos!"));
|
||||
demoing_servos = true;
|
||||
servo_write(1, 1400);
|
||||
hal.scheduler->delay(400);
|
||||
|
|
|
@ -1102,7 +1102,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
uint8_t result = MAV_RESULT_UNSUPPORTED;
|
||||
|
||||
// do command
|
||||
send_text_P(SEVERITY_LOW,PSTR("command received: "));
|
||||
send_text_P(MAV_SEVERITY_WARNING,PSTR("command received: "));
|
||||
|
||||
switch(packet.command) {
|
||||
|
||||
|
@ -1196,7 +1196,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
}
|
||||
}
|
||||
else {
|
||||
send_text_P(SEVERITY_LOW, PSTR("Unsupported preflight calibration"));
|
||||
send_text_P(MAV_SEVERITY_WARNING, PSTR("Unsupported preflight calibration"));
|
||||
}
|
||||
plane.in_calibration = false;
|
||||
break;
|
||||
|
@ -1311,9 +1311,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
plane.auto_state.commanded_go_around = true;
|
||||
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
plane.gcs_send_text_P(SEVERITY_HIGH,PSTR("Go around command accepted."));
|
||||
plane.gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Go around command accepted."));
|
||||
} else {
|
||||
plane.gcs_send_text_P(SEVERITY_HIGH,PSTR("Rejected go around command."));
|
||||
plane.gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Rejected go around command."));
|
||||
}
|
||||
break;
|
||||
|
||||
|
@ -1337,7 +1337,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
if (! plane.geofence_set_floor_enabled(false)) {
|
||||
result = MAV_RESULT_FAILED;
|
||||
} else {
|
||||
plane.gcs_send_text_P(SEVERITY_HIGH,PSTR("Fence floor disabled."));
|
||||
plane.gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Fence floor disabled."));
|
||||
}
|
||||
break;
|
||||
default:
|
||||
|
@ -1432,10 +1432,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
|
||||
{
|
||||
// mark the firmware version in the tlog
|
||||
send_text_P(SEVERITY_LOW, PSTR(FIRMWARE_STRING));
|
||||
send_text_P(MAV_SEVERITY_WARNING, PSTR(FIRMWARE_STRING));
|
||||
|
||||
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
|
||||
send_text_P(SEVERITY_LOW, PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION));
|
||||
send_text_P(MAV_SEVERITY_WARNING, PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION));
|
||||
#endif
|
||||
handle_param_request_list(msg);
|
||||
break;
|
||||
|
@ -1494,9 +1494,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
mavlink_fence_point_t packet;
|
||||
mavlink_msg_fence_point_decode(msg, &packet);
|
||||
if (plane.g.fence_action != FENCE_ACTION_NONE) {
|
||||
send_text_P(SEVERITY_LOW,PSTR("fencing must be disabled"));
|
||||
send_text_P(MAV_SEVERITY_WARNING,PSTR("fencing must be disabled"));
|
||||
} else if (packet.count != plane.g.fence_total) {
|
||||
send_text_P(SEVERITY_LOW,PSTR("bad fence point"));
|
||||
send_text_P(MAV_SEVERITY_WARNING,PSTR("bad fence point"));
|
||||
} else {
|
||||
Vector2l point;
|
||||
point.x = packet.lat*1.0e7f;
|
||||
|
@ -1511,7 +1511,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
mavlink_fence_fetch_point_t packet;
|
||||
mavlink_msg_fence_fetch_point_decode(msg, &packet);
|
||||
if (packet.idx >= plane.g.fence_total) {
|
||||
send_text_P(SEVERITY_LOW,PSTR("bad fence point"));
|
||||
send_text_P(MAV_SEVERITY_WARNING,PSTR("bad fence point"));
|
||||
} else {
|
||||
Vector2l point = plane.get_fence_point_with_index(packet.idx);
|
||||
mavlink_msg_fence_point_send_buf(msg, chan, msg->sysid, msg->compid, packet.idx, plane.g.fence_total,
|
||||
|
@ -1528,12 +1528,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
|
||||
if (packet.idx >= plane.rally.get_rally_total() ||
|
||||
packet.idx >= plane.rally.get_rally_max()) {
|
||||
send_text_P(SEVERITY_LOW,PSTR("bad rally point message ID"));
|
||||
send_text_P(MAV_SEVERITY_WARNING,PSTR("bad rally point message ID"));
|
||||
break;
|
||||
}
|
||||
|
||||
if (packet.count != plane.rally.get_rally_total()) {
|
||||
send_text_P(SEVERITY_LOW,PSTR("bad rally point message count"));
|
||||
send_text_P(MAV_SEVERITY_WARNING,PSTR("bad rally point message count"));
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -1553,12 +1553,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
mavlink_rally_fetch_point_t packet;
|
||||
mavlink_msg_rally_fetch_point_decode(msg, &packet);
|
||||
if (packet.idx > plane.rally.get_rally_total()) {
|
||||
send_text_P(SEVERITY_LOW, PSTR("bad rally point index"));
|
||||
send_text_P(MAV_SEVERITY_WARNING, PSTR("bad rally point index"));
|
||||
break;
|
||||
}
|
||||
RallyLocation rally_point;
|
||||
if (!plane.rally.get_rally_point_with_index(packet.idx, rally_point)) {
|
||||
send_text_P(SEVERITY_LOW, PSTR("failed to set rally point"));
|
||||
send_text_P(MAV_SEVERITY_WARNING, PSTR("failed to set rally point"));
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -1782,7 +1782,7 @@ void Plane::mavlink_delay_cb()
|
|||
}
|
||||
if (tnow - last_5s > 5000) {
|
||||
last_5s = tnow;
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Initialising APM..."));
|
||||
}
|
||||
check_usb_mux();
|
||||
|
||||
|
@ -1842,7 +1842,7 @@ void Plane::gcs_update(void)
|
|||
}
|
||||
}
|
||||
|
||||
void Plane::gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
|
||||
void Plane::gcs_send_text_P(MAV_SEVERITY severity, const prog_char_t *str)
|
||||
{
|
||||
for (uint8_t i=0; i<num_gcs; i++) {
|
||||
if (gcs[i].initialised) {
|
||||
|
@ -1862,7 +1862,7 @@ void Plane::gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
|
|||
void Plane::gcs_send_text_fmt(const prog_char_t *fmt, ...)
|
||||
{
|
||||
va_list arg_list;
|
||||
gcs[0].pending_status.severity = (uint8_t)SEVERITY_LOW;
|
||||
gcs[0].pending_status.severity = (uint8_t)MAV_SEVERITY_WARNING;
|
||||
va_start(arg_list, fmt);
|
||||
hal.util->vsnprintf_P((char *)gcs[0].pending_status.text,
|
||||
sizeof(gcs[0].pending_status.text), fmt, arg_list);
|
||||
|
|
|
@ -153,9 +153,9 @@ int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
void Plane::do_erase_logs(void)
|
||||
{
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Erasing logs"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Erasing logs"));
|
||||
DataFlash.EraseAll();
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Log erase complete"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Log erase complete"));
|
||||
}
|
||||
|
||||
|
||||
|
@ -534,10 +534,10 @@ void Plane::log_init(void)
|
|||
{
|
||||
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
if (!DataFlash.CardInserted()) {
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("No dataflash card inserted"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("No dataflash card inserted"));
|
||||
g.log_bitmask.set(0);
|
||||
} else if (DataFlash.NeedErase()) {
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("ERASING LOGS"));
|
||||
do_erase_logs();
|
||||
for (uint8_t i=0; i<num_gcs; i++) {
|
||||
gcs[i].reset_cli_timeout();
|
||||
|
|
|
@ -653,7 +653,7 @@ private:
|
|||
|
||||
// Arming/Disarming mangement class
|
||||
AP_Arming_Plane arming {ahrs, barometer, compass, home_is_set,
|
||||
FUNCTOR_BIND_MEMBER(&Plane::gcs_send_text_P, void, gcs_severity, const prog_char_t *)};
|
||||
FUNCTOR_BIND_MEMBER(&Plane::gcs_send_text_P, void, MAV_SEVERITY, const prog_char_t *)};
|
||||
|
||||
AP_Param param_loader {var_info};
|
||||
|
||||
|
@ -695,7 +695,7 @@ private:
|
|||
void gcs_send_mission_item_reached_message(uint16_t mission_index);
|
||||
void gcs_data_stream_send(void);
|
||||
void gcs_update(void);
|
||||
void gcs_send_text_P(gcs_severity severity, const prog_char_t *str);
|
||||
void gcs_send_text_P(MAV_SEVERITY severity, const prog_char_t *str);
|
||||
void gcs_send_airspeed_calibration(const Vector3f &vg);
|
||||
void gcs_retry_deferred(void);
|
||||
void do_erase_logs(void);
|
||||
|
|
|
@ -14,21 +14,21 @@ bool AP_Arming_Plane::pre_arm_checks(bool report)
|
|||
|
||||
if (plane.g.roll_limit_cd < 300) {
|
||||
if (report) {
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: LIM_ROLL_CD too small"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: LIM_ROLL_CD too small"));
|
||||
}
|
||||
ret = false;
|
||||
}
|
||||
|
||||
if (plane.aparm.pitch_limit_max_cd < 300) {
|
||||
if (report) {
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: LIM_PITCH_MAX too small"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: LIM_PITCH_MAX too small"));
|
||||
}
|
||||
ret = false;
|
||||
}
|
||||
|
||||
if (plane.aparm.pitch_limit_min_cd > -300) {
|
||||
if (report) {
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: LIM_PITCH_MIN too large"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: LIM_PITCH_MIN too large"));
|
||||
}
|
||||
ret = false;
|
||||
}
|
||||
|
@ -38,7 +38,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool report)
|
|||
plane.g.throttle_fs_value <
|
||||
plane.channel_throttle->radio_max) {
|
||||
if (report) {
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: invalid THR_FS_VALUE for rev throttle"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: invalid THR_FS_VALUE for rev throttle"));
|
||||
}
|
||||
ret = false;
|
||||
}
|
||||
|
|
|
@ -52,7 +52,7 @@ void Plane::set_next_WP(const struct Location &loc)
|
|||
// location as the previous waypoint, to prevent immediately
|
||||
// considering the waypoint complete
|
||||
if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Resetting prev_WP"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Resetting prev_WP"));
|
||||
prev_WP_loc = current_loc;
|
||||
}
|
||||
|
||||
|
@ -100,7 +100,7 @@ void Plane::set_guided_WP(void)
|
|||
// -------------------------------
|
||||
void Plane::init_home()
|
||||
{
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("init home"));
|
||||
|
||||
ahrs.set_home(gps.location());
|
||||
home_is_set = HOME_SET_NOT_LOCKED;
|
||||
|
|
|
@ -278,9 +278,9 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
|
|||
default:
|
||||
// error message
|
||||
if (AP_Mission::is_nav_cmd(cmd)) {
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("verify_nav: Invalid or no current Nav cmd"));
|
||||
}else{
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("verify_conditon: Invalid or no current Condition cmd"));
|
||||
}
|
||||
// return true so that we do not get stuck at this command
|
||||
return true;
|
||||
|
@ -462,9 +462,9 @@ bool Plane::verify_takeoff()
|
|||
#if GEOFENCE_ENABLED == ENABLED
|
||||
if (g.fence_autoenable > 0) {
|
||||
if (! geofence_set_enabled(true, AUTO_TOGGLED)) {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Enable fence failed (cannot autoenable"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Enable fence failed (cannot autoenable"));
|
||||
} else {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence enabled. (autoenabled)"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Fence enabled. (autoenabled)"));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
@ -541,7 +541,7 @@ bool Plane::verify_loiter_time()
|
|||
loiter.start_time_ms = millis();
|
||||
}
|
||||
} else if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) {
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("verify_nav: LOITER time complete"));
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
@ -552,7 +552,7 @@ bool Plane::verify_loiter_turns()
|
|||
update_loiter();
|
||||
if (loiter.sum_cd > loiter.total_cd) {
|
||||
loiter.total_cd = 0;
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("verify_nav: LOITER orbits complete"));
|
||||
// clear the command queue;
|
||||
return true;
|
||||
}
|
||||
|
@ -636,7 +636,7 @@ bool Plane::verify_RTL()
|
|||
update_loiter();
|
||||
if (auto_state.wp_distance <= (uint32_t)max(g.waypoint_radius,0) ||
|
||||
nav_controller->reached_loiter_target()) {
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("Reached home"));
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
|
@ -678,7 +678,7 @@ bool Plane::verify_continue_and_change_alt()
|
|||
bool Plane::verify_altitude_wait(const AP_Mission::Mission_Command &cmd)
|
||||
{
|
||||
if (current_loc.alt > cmd.content.altitude_wait.altitude*100.0f) {
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("Reached altitude"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("Reached altitude"));
|
||||
return true;
|
||||
}
|
||||
if (auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) {
|
||||
|
|
|
@ -71,17 +71,17 @@ void Plane::read_control_switch()
|
|||
if (hal.util->get_soft_armed() || setup_failsafe_mixing()) {
|
||||
px4io_override_enabled = true;
|
||||
// disable output channels to force PX4IO override
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("PX4IO Override enabled"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("PX4IO Override enabled"));
|
||||
} else {
|
||||
// we'll try again next loop. The PX4IO code sometimes
|
||||
// rejects a mixer, probably due to it being busy in
|
||||
// some way?
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("PX4IO Override enable failed"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("PX4IO Override enable failed"));
|
||||
}
|
||||
} else if (!override && px4io_override_enabled) {
|
||||
px4io_override_enabled = false;
|
||||
RC_Channel_aux::enable_aux_servos();
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("PX4IO Override disabled"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("PX4IO Override disabled"));
|
||||
}
|
||||
if (px4io_override_enabled &&
|
||||
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED) {
|
||||
|
|
|
@ -7,7 +7,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
|
|||
// This is how to handle a short loss of control signal failsafe.
|
||||
failsafe.state = fstype;
|
||||
failsafe.ch3_timer_ms = millis();
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, "));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Failsafe - Short event on, "));
|
||||
switch(control_mode)
|
||||
{
|
||||
case MANUAL:
|
||||
|
@ -52,7 +52,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
|
|||
void Plane::failsafe_long_on_event(enum failsafe_state fstype)
|
||||
{
|
||||
// This is how to handle a long loss of control signal failsafe.
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, "));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Failsafe - Long event on, "));
|
||||
// If the GCS is locked up we allow control to revert to RC
|
||||
hal.rcin->clear_overrides();
|
||||
failsafe.state = fstype;
|
||||
|
@ -89,7 +89,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
|
|||
break;
|
||||
}
|
||||
if (fstype == FAILSAFE_GCS) {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("No GCS heartbeat."));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("No GCS heartbeat."));
|
||||
}
|
||||
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
|
||||
}
|
||||
|
@ -97,7 +97,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
|
|||
void Plane::failsafe_short_off_event()
|
||||
{
|
||||
// We're back in radio contact
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Failsafe - Short event off"));
|
||||
failsafe.state = FAILSAFE_NONE;
|
||||
|
||||
// re-read the switch so we can return to our preferred mode
|
||||
|
|
|
@ -137,13 +137,13 @@ void Plane::geofence_load(void)
|
|||
geofence_state->boundary_uptodate = true;
|
||||
geofence_state->fence_triggered = false;
|
||||
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence loaded"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("geo-fence loaded"));
|
||||
gcs_send_message(MSG_FENCE_STATUS);
|
||||
return;
|
||||
|
||||
failed:
|
||||
g.fence_action.set(FENCE_ACTION_NONE);
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("geo-fence setup error"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("geo-fence setup error"));
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -335,7 +335,7 @@ void Plane::geofence_check(bool altitude_check_only)
|
|||
if (geofence_state->fence_triggered && !altitude_check_only) {
|
||||
// we have moved back inside the fence
|
||||
geofence_state->fence_triggered = false;
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence OK"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("geo-fence OK"));
|
||||
#if FENCE_TRIGGERED_PIN > 0
|
||||
hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT);
|
||||
hal.gpio->write(FENCE_TRIGGERED_PIN, 0);
|
||||
|
@ -365,7 +365,7 @@ void Plane::geofence_check(bool altitude_check_only)
|
|||
hal.gpio->write(FENCE_TRIGGERED_PIN, 1);
|
||||
#endif
|
||||
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence triggered"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("geo-fence triggered"));
|
||||
gcs_send_message(MSG_FENCE_STATUS);
|
||||
|
||||
// see what action the user wants
|
||||
|
|
|
@ -217,9 +217,9 @@ void Plane::crash_detection_update(void)
|
|||
|
||||
if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) {
|
||||
if (crashed_near_land_waypoint) {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Hard Landing Detected - no action taken"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected - no action taken"));
|
||||
} else {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Crash Detected - no action taken"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected - no action taken"));
|
||||
}
|
||||
}
|
||||
else {
|
||||
|
@ -228,9 +228,9 @@ void Plane::crash_detection_update(void)
|
|||
}
|
||||
auto_state.land_complete = true;
|
||||
if (crashed_near_land_waypoint) {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Hard Landing Detected"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected"));
|
||||
} else {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Crash Detected"));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected"));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -113,7 +113,7 @@ void Plane::disarm_if_autoland_complete()
|
|||
/* we have auto disarm enabled. See if enough time has passed */
|
||||
if (millis() - auto_state.last_flying_ms >= g.land_disarm_delay*1000UL) {
|
||||
if (disarm_motors()) {
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("Auto-Disarmed"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("Auto-Disarmed"));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -228,12 +228,12 @@ bool Plane::jump_to_landing_sequence(void)
|
|||
mission.resume();
|
||||
}
|
||||
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Landing sequence begun."));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Landing sequence begun."));
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Unable to start landing sequence."));
|
||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Unable to start landing sequence."));
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
@ -4,10 +4,10 @@
|
|||
|
||||
void Plane::init_barometer(void)
|
||||
{
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Calibrating barometer"));
|
||||
barometer.calibrate();
|
||||
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("barometer calibration complete"));
|
||||
}
|
||||
|
||||
void Plane::init_rangefinder(void)
|
||||
|
@ -65,7 +65,7 @@ void Plane::zero_airspeed(bool in_startup)
|
|||
read_airspeed();
|
||||
// update barometric calibration with new airspeed supplied temperature
|
||||
barometer.update_calibration();
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("zero airspeed calibrated"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("zero airspeed calibrated"));
|
||||
}
|
||||
|
||||
// read_battery - reads battery voltage and current and invokes failsafe
|
||||
|
|
|
@ -267,10 +267,10 @@ void Plane::startup_ground(void)
|
|||
{
|
||||
set_mode(INITIALISING);
|
||||
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> GROUND START"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("<startup_ground> GROUND START"));
|
||||
|
||||
#if (GROUND_START_DELAY > 0)
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> With Delay"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("<startup_ground> With Delay"));
|
||||
delay(GROUND_START_DELAY * 1000);
|
||||
#endif
|
||||
|
||||
|
@ -317,7 +317,7 @@ void Plane::startup_ground(void)
|
|||
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
|
||||
ins.set_dataflash(&DataFlash);
|
||||
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("\n\n Ready to FLY."));
|
||||
}
|
||||
|
||||
enum FlightMode Plane::get_previous_mode() {
|
||||
|
@ -556,7 +556,7 @@ void Plane::startup_INS_ground(void)
|
|||
while (barometer.get_last_update() == 0) {
|
||||
// the barometer begins updating when we get the first
|
||||
// HIL_STATE message
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Waiting for first HIL_STATE message"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Waiting for first HIL_STATE message"));
|
||||
hal.scheduler->delay(1000);
|
||||
}
|
||||
}
|
||||
|
@ -571,7 +571,7 @@ void Plane::startup_INS_ground(void)
|
|||
}
|
||||
|
||||
if (style == AP_InertialSensor::COLD_START) {
|
||||
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move plane"));
|
||||
gcs_send_text_P(MAV_SEVERITY_ALERT, PSTR("Beginning INS calibration; do not move plane"));
|
||||
hal.scheduler->delay(100);
|
||||
}
|
||||
|
||||
|
@ -592,7 +592,7 @@ void Plane::startup_INS_ground(void)
|
|||
// --------------------------
|
||||
zero_airspeed(true);
|
||||
} else {
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("NO airspeed"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("NO airspeed"));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -166,7 +166,7 @@ int8_t Plane::takeoff_tail_hold(void)
|
|||
|
||||
return_zero:
|
||||
if (auto_state.fbwa_tdrag_takeoff_mode) {
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("FBWA tdrag off"));
|
||||
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("FBWA tdrag off"));
|
||||
auto_state.fbwa_tdrag_takeoff_mode = false;
|
||||
}
|
||||
return 0;
|
||||
|
|
Loading…
Reference in New Issue