From 767b4da5b6852a2ba700ee1f97d83129618b04f1 Mon Sep 17 00:00:00 2001 From: squilter Date: Tue, 11 Aug 2015 16:50:34 -0700 Subject: [PATCH] Copter: update send text severities --- ArduCopter/Copter.h | 2 +- ArduCopter/GCS_Mavlink.cpp | 32 +++++----- ArduCopter/Log.cpp | 8 +-- ArduCopter/compassmot.cpp | 20 +++--- ArduCopter/control_autotune.cpp | 10 +-- ArduCopter/crash_check.cpp | 8 +-- ArduCopter/ekf_check.cpp | 2 +- ArduCopter/esc_calibration.cpp | 8 +-- ArduCopter/events.cpp | 2 +- ArduCopter/motor_test.cpp | 6 +- ArduCopter/motors.cpp | 108 ++++++++++++++++---------------- ArduCopter/sensors.cpp | 4 +- ArduCopter/switches.cpp | 2 +- ArduCopter/system.cpp | 4 +- 14 files changed, 108 insertions(+), 108 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 2efa425fd0..2248824424 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -595,7 +595,7 @@ private: void gcs_send_mission_item_reached_message(uint16_t mission_index); void gcs_data_stream_send(void); void gcs_check_input(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 do_erase_logs(void); void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt); void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index d9f102678b..3dd214f5b6 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1012,12 +1012,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: // MAV ID: 21 { // 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 - send_text_P(SEVERITY_LOW, PSTR("Frame: " FRAME_CONFIG_STRING)); + send_text_P(MAV_SEVERITY_WARNING, PSTR("Frame: " FRAME_CONFIG_STRING)); handle_param_request_list(msg); break; } @@ -1720,12 +1720,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.idx >= copter.rally.get_rally_total() || packet.idx >= copter.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 != copter.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; } @@ -1738,7 +1738,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) rally_point.flags = packet.flags; if (!copter.rally.set_rally_point_with_index(packet.idx, rally_point)) { - send_text_P(SEVERITY_HIGH, PSTR("error setting rally point")); + send_text_P(MAV_SEVERITY_CRITICAL, PSTR("error setting rally point")); } break; @@ -1746,27 +1746,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) //send a rally point to the GCS case MAVLINK_MSG_ID_RALLY_FETCH_POINT: { - //send_text_P(SEVERITY_HIGH, PSTR("## getting rally point in GCS_Mavlink.cpp 1")); // #### TEMP + //send_text_P(MAV_SEVERITY_CRITICAL, PSTR("## getting rally point in GCS_Mavlink.cpp 1")); // #### TEMP mavlink_rally_fetch_point_t packet; mavlink_msg_rally_fetch_point_decode(msg, &packet); - //send_text_P(SEVERITY_HIGH, PSTR("## getting rally point in GCS_Mavlink.cpp 2")); // #### TEMP + //send_text_P(MAV_SEVERITY_CRITICAL, PSTR("## getting rally point in GCS_Mavlink.cpp 2")); // #### TEMP if (packet.idx > copter.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; } - //send_text_P(SEVERITY_HIGH, PSTR("## getting rally point in GCS_Mavlink.cpp 3")); // #### TEMP + //send_text_P(MAV_SEVERITY_CRITICAL, PSTR("## getting rally point in GCS_Mavlink.cpp 3")); // #### TEMP RallyLocation rally_point; if (!copter.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; } - //send_text_P(SEVERITY_HIGH, PSTR("## getting rally point in GCS_Mavlink.cpp 4")); // #### TEMP + //send_text_P(MAV_SEVERITY_CRITICAL, PSTR("## getting rally point in GCS_Mavlink.cpp 4")); // #### TEMP mavlink_msg_rally_point_send_buf(msg, chan, msg->sysid, msg->compid, packet.idx, @@ -1774,7 +1774,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) rally_point.alt, rally_point.break_alt, rally_point.land_dir, rally_point.flags); - //send_text_P(SEVERITY_HIGH, PSTR("## getting rally point in GCS_Mavlink.cpp 5")); // #### TEMP + //send_text_P(MAV_SEVERITY_CRITICAL, PSTR("## getting rally point in GCS_Mavlink.cpp 5")); // #### TEMP break; } @@ -1821,7 +1821,7 @@ void Copter::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(); @@ -1881,7 +1881,7 @@ void Copter::gcs_check_input(void) } } -void Copter::gcs_send_text_P(gcs_severity severity, const prog_char_t *str) +void Copter::gcs_send_text_P(MAV_SEVERITY severity, const prog_char_t *str) { for (uint8_t i=0; ivsnprintf_P((char *)gcs[0].pending_status.text, sizeof(gcs[0].pending_status.text), fmt, arg_list); diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 0757c2a6d1..536fd7e98d 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -149,9 +149,9 @@ int8_t Copter::process_logs(uint8_t argc, const Menu::arg *argv) void Copter::do_erase_logs(void) { - gcs_send_text_P(SEVERITY_HIGH, PSTR("Erasing logs\n")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Erasing logs\n")); DataFlash.EraseAll(); - gcs_send_text_P(SEVERITY_HIGH, PSTR("Log erase complete\n")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Log erase complete\n")); } #if AUTOTUNE_ENABLED == ENABLED @@ -772,10 +772,10 @@ void Copter::log_init(void) { DataFlash.Init(log_structure, ARRAY_SIZE(log_structure)); if (!DataFlash.CardInserted()) { - gcs_send_text_P(SEVERITY_HIGH, PSTR("No dataflash inserted")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("No dataflash inserted")); g.log_bitmask.set(0); } else if (DataFlash.NeedErase()) { - gcs_send_text_P(SEVERITY_HIGH, PSTR("ERASING LOGS")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("ERASING LOGS")); do_erase_logs(); gcs[0].reset_cli_timeout(); } diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index e06105a772..2b58fe5036 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -37,7 +37,7 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan) // check compass is enabled if (!g.compass_enabled) { - gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("compass disabled\n")); + gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, PSTR("compass disabled\n")); ap.compass_mot = false; return 1; } @@ -46,7 +46,7 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan) compass.read(); for (uint8_t i=0; icontrol_in != 0) { - gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("thr not zero")); + gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, PSTR("thr not zero")); ap.compass_mot = false; return 1; } // check we are landed if (!ap.land_complete) { - gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("Not landed")); + gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Not landed")); ap.compass_mot = false; return 1; } @@ -95,13 +95,13 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan) AP_Notify::flags.esc_calibration = true; // warn user we are starting calibration - gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("STARTING CALIBRATION")); + gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, PSTR("STARTING CALIBRATION")); // inform what type of compensation we are attempting if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) { - gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("CURRENT")); + gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, PSTR("CURRENT")); } else{ - gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("THROTTLE")); + gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, PSTR("THROTTLE")); } // disable throttle and battery failsafe @@ -245,10 +245,10 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan) } compass.save_motor_compensation(); // display success message - gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("Calibration Successful!")); + gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Calibration Successful!")); } else { // compensation vector never updated, report failure - gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("Failed!")); + gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Failed!")); compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); } diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index 7fe8555d99..d768d6becc 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -1006,19 +1006,19 @@ void Copter::autotune_update_gcs(uint8_t message_id) { switch (message_id) { case AUTOTUNE_MESSAGE_STARTED: - gcs_send_text_P(SEVERITY_HIGH,PSTR("AutoTune: Started")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("AutoTune: Started")); break; case AUTOTUNE_MESSAGE_STOPPED: - gcs_send_text_P(SEVERITY_HIGH,PSTR("AutoTune: Stopped")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("AutoTune: Stopped")); break; case AUTOTUNE_MESSAGE_SUCCESS: - gcs_send_text_P(SEVERITY_HIGH,PSTR("AutoTune: Success")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("AutoTune: Success")); break; case AUTOTUNE_MESSAGE_FAILED: - gcs_send_text_P(SEVERITY_HIGH,PSTR("AutoTune: Failed")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("AutoTune: Failed")); break; case AUTOTUNE_MESSAGE_SAVED_GAINS: - gcs_send_text_P(SEVERITY_HIGH,PSTR("AutoTune: Saved Gains")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("AutoTune: Saved Gains")); break; } } diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 3f352cfb61..cb3f178322 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -47,7 +47,7 @@ void Copter::crash_check() // log an error in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH); // send message to gcs - gcs_send_text_P(SEVERITY_HIGH,PSTR("Crash: Disarming")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Crash: Disarming")); // disarm motors init_disarm_motors(); } @@ -136,7 +136,7 @@ void Copter::parachute_check() void Copter::parachute_release() { // send message to gcs and dataflash - gcs_send_text_P(SEVERITY_HIGH,PSTR("Parachute: Released!")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Parachute: Released!")); Log_Write_Event(DATA_PARACHUTE_RELEASED); // disarm motors @@ -159,7 +159,7 @@ void Copter::parachute_manual_release() // do not release if we are landed or below the minimum altitude above home if (ap.land_complete) { // warn user of reason for failure - gcs_send_text_P(SEVERITY_HIGH,PSTR("Parachute: Landed")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Parachute: Landed")); // log an error in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_LANDED); return; @@ -168,7 +168,7 @@ void Copter::parachute_manual_release() // do not release if we are landed or below the minimum altitude above home if ((parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100))) { // warn user of reason for failure - gcs_send_text_P(SEVERITY_HIGH,PSTR("Parachute: Too Low")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Parachute: Too Low")); // log an error in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_TOO_LOW); return; diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 5ba9029dbf..8c6b575df0 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -60,7 +60,7 @@ void Copter::ekf_check() Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE); // send message to gcs if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("EKF variance")); ekf_check_state.last_warn_time = hal.scheduler->millis(); } failsafe_ekf_event(); diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index 7fb4969477..9aeccd7619 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -39,7 +39,7 @@ void Copter::esc_calibration_startup_check() // we will enter esc_calibrate mode on next reboot g.esc_calibrate.set_and_save(ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH); // send message to gcs - gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Calibration: restart board")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("ESC Calibration: restart board")); // turn on esc calibration notification AP_Notify::flags.esc_calibration = true; // block until we restart @@ -85,7 +85,7 @@ void Copter::esc_calibration_passthrough() motors.set_update_rate(50); // send message to GCS - gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Calibration: passing pilot throttle to ESCs")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("ESC Calibration: passing pilot throttle to ESCs")); while(1) { // arm motors @@ -115,7 +115,7 @@ void Copter::esc_calibration_auto() motors.set_update_rate(50); // send message to GCS - gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Calibration: auto calibration")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("ESC Calibration: auto calibration")); // arm and enable motors motors.armed(true); @@ -131,7 +131,7 @@ void Copter::esc_calibration_auto() // wait for safety switch to be pressed while (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { if (!printed_msg) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Calibration: push safety switch")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("ESC Calibration: push safety switch")); printed_msg = true; } delay(10); diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index b960e5467b..c575a4729d 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -159,7 +159,7 @@ void Copter::failsafe_battery_event(void) set_failsafe_battery(true); // warn the ground station and log to dataflash - gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Low Battery!")); Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED); } diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index ea7728fe53..0f536d8f3d 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -74,19 +74,19 @@ bool Copter::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc) // check rc has been calibrated pre_arm_rc_checks(); if(check_rc && !ap.pre_arm_rc_check) { - gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH,PSTR("Motor Test: RC not calibrated")); + gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Motor Test: RC not calibrated")); return false; } // ensure we are landed if (!ap.land_complete) { - gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH,PSTR("Motor Test: vehicle not landed")); + gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Motor Test: vehicle not landed")); return false; } // check if safety switch has been pushed if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { - gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH,PSTR("Motor Test: Safety Switch")); + gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Motor Test: Safety Switch")); return false; } diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 69b93afcfe..ad2b4f7eef 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -153,7 +153,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs) } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("ARMING MOTORS")); #endif // Remember Orientation @@ -176,7 +176,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs) startup_ground(true); // final check that gyros calibrated successfully if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) && !ins.gyro_calibrated_ok_all()) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Gyro calibration failed")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Gyro calibration failed")); AP_Notify::flags.armed = false; failsafe_enable(); in_arm_motors = false; @@ -190,7 +190,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs) // if we are using motor interlock switch and it's enabled, fail to arm if (ap.using_interlock && motors.get_interlock()){ - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Motor Interlock Enabled")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Motor Interlock Enabled")); AP_Notify::flags.armed = false; return false; } @@ -201,7 +201,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs) set_motor_emergency_stop(false); // if we are using motor Estop switch, it must not be in Estop position } else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){ - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Motor Emergency Stopped")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Motor Emergency Stopped")); AP_Notify::flags.armed = false; return false; } @@ -256,7 +256,7 @@ bool Copter::pre_arm_checks(bool display_failure) // at the same time. This cannot be allowed. if (check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Interlock/E-Stop Conflict")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Interlock/E-Stop Conflict")); } return false; } @@ -268,7 +268,7 @@ bool Copter::pre_arm_checks(bool display_failure) set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK)); if (ap.using_interlock && motors.get_interlock()){ if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Motor Interlock Enabled")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Motor Interlock Enabled")); } return false; } @@ -277,7 +277,7 @@ bool Copter::pre_arm_checks(bool display_failure) // and warn if it is if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){ if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Motor Emergency Stopped")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Motor Emergency Stopped")); } return false; } @@ -301,7 +301,7 @@ bool Copter::pre_arm_checks(bool display_failure) pre_arm_rc_checks(); if(!ap.pre_arm_rc_check) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: RC not calibrated")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: RC not calibrated")); } return false; } @@ -310,7 +310,7 @@ bool Copter::pre_arm_checks(bool display_failure) // barometer health check if(!barometer.all_healthy()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Barometer not healthy")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Barometer not healthy")); } return false; } @@ -322,7 +322,7 @@ bool Copter::pre_arm_checks(bool display_failure) if (using_baro_ref) { if (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Altitude disparity")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Altitude disparity")); } return false; } @@ -334,7 +334,7 @@ bool Copter::pre_arm_checks(bool display_failure) // check the primary compass is healthy if(!compass.healthy()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not healthy")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Compass not healthy")); } return false; } @@ -342,7 +342,7 @@ bool Copter::pre_arm_checks(bool display_failure) // check compass learning is on or offsets have been set if(!compass.configured()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not calibrated")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Compass not calibrated")); } return false; } @@ -351,7 +351,7 @@ bool Copter::pre_arm_checks(bool display_failure) Vector3f offsets = compass.get_offsets(); if(offsets.length() > COMPASS_OFFSETS_MAX) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass offsets too high")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Compass offsets too high")); } return false; } @@ -360,7 +360,7 @@ bool Copter::pre_arm_checks(bool display_failure) float mag_field = compass.get_field().length(); if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65f || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35f) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check mag field")); } return false; } @@ -377,7 +377,7 @@ bool Copter::pre_arm_checks(bool display_failure) Vector3f vec_diff = mag_vec - prime_mag_vec; if (compass.use_for_yaw(i) && vec_diff.length() > COMPASS_ACCEPTABLE_VECTOR_DIFF) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent compasses")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent compasses")); } return false; } @@ -396,7 +396,7 @@ bool Copter::pre_arm_checks(bool display_failure) // check fence is initialised if(!fence.pre_arm_check()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: check fence")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: check fence")); } return false; } @@ -407,7 +407,7 @@ bool Copter::pre_arm_checks(bool display_failure) // check accelerometers have been calibrated if(!ins.accel_calibrated_ok_all()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Accels not calibrated")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Accels not calibrated")); } return false; } @@ -415,7 +415,7 @@ bool Copter::pre_arm_checks(bool display_failure) // check accels are healthy if(!ins.get_accel_health_all()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Accelerometers not healthy")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Accelerometers not healthy")); } return false; } @@ -439,7 +439,7 @@ bool Copter::pre_arm_checks(bool display_failure) } if (vec_diff.length() > threshold) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent Accelerometers")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent Accelerometers")); } return false; } @@ -450,7 +450,7 @@ bool Copter::pre_arm_checks(bool display_failure) // check gyros are healthy if(!ins.get_gyro_health_all()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Gyros not healthy")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Gyros not healthy")); } return false; } @@ -463,7 +463,7 @@ bool Copter::pre_arm_checks(bool display_failure) Vector3f vec_diff = ins.get_gyro(i) - ins.get_gyro(); if (vec_diff.length() > PREARM_MAX_GYRO_VECTOR_DIFF) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent Gyros")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent Gyros")); } return false; } @@ -477,7 +477,7 @@ bool Copter::pre_arm_checks(bool display_failure) if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { if(hal.analogin->board_voltage() < BOARD_VOLTAGE_MIN || hal.analogin->board_voltage() > BOARD_VOLTAGE_MAX) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check Board Voltage")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check Board Voltage")); } return false; } @@ -489,7 +489,7 @@ bool Copter::pre_arm_checks(bool display_failure) if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check Battery")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check Battery")); } return false; } @@ -501,7 +501,7 @@ bool Copter::pre_arm_checks(bool display_failure) // ensure ch7 and ch8 have different functions if (check_duplicate_auxsw()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Duplicate Aux Switch Options")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Duplicate Aux Switch Options")); } return false; } @@ -511,7 +511,7 @@ bool Copter::pre_arm_checks(bool display_failure) // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 if (channel_throttle->radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check FS_THR_VALUE")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check FS_THR_VALUE")); } return false; } @@ -520,7 +520,7 @@ bool Copter::pre_arm_checks(bool display_failure) // lean angle parameter check if (aparm.angle_max < 1000 || aparm.angle_max > 8000) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check ANGLE_MAX")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check ANGLE_MAX")); } return false; } @@ -528,7 +528,7 @@ bool Copter::pre_arm_checks(bool display_failure) // acro balance parameter check if ((g.acro_balance_roll > g.p_stabilize_roll.kP()) || (g.acro_balance_pitch > g.p_stabilize_pitch.kP())) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: ACRO_BAL_ROLL/PITCH")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: ACRO_BAL_ROLL/PITCH")); } return false; } @@ -537,7 +537,7 @@ bool Copter::pre_arm_checks(bool display_failure) // check range finder if optflow enabled if (optflow.enabled() && !sonar.pre_arm_check()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: check range finder")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: check range finder")); } return false; } @@ -546,7 +546,7 @@ bool Copter::pre_arm_checks(bool display_failure) // check helicopter parameters if (!motors.parameter_check()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check Heli Parameters")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check Heli Parameters")); } return false; } @@ -559,9 +559,9 @@ bool Copter::pre_arm_checks(bool display_failure) if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Collective below Failsafe")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Collective below Failsafe")); #else - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Throttle below Failsafe")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Throttle below Failsafe")); #endif } return false; @@ -622,7 +622,7 @@ bool Copter::pre_arm_gps_checks(bool display_failure) // always check if inertial nav has started and is ready if(!ahrs.get_NavEKF().healthy()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Waiting for Nav Checks")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Waiting for Nav Checks")); } return false; } @@ -652,7 +652,7 @@ bool Copter::pre_arm_gps_checks(bool display_failure) // ensure GPS is ok if (!position_ok()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Need 3D Fix")); } AP_Notify::flags.pre_arm_gps_check = false; return false; @@ -661,7 +661,7 @@ bool Copter::pre_arm_gps_checks(bool display_failure) // check home and EKF origin are not too far if (far_from_EKF_origin(ahrs.get_home())) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: EKF-home variance")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: EKF-home variance")); } AP_Notify::flags.pre_arm_gps_check = false; return false; @@ -670,7 +670,7 @@ bool Copter::pre_arm_gps_checks(bool display_failure) // warn about hdop separately - to prevent user confusion with no gps lock if (gps.get_hdop() > g.gps_hdop_good) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: High GPS HDOP")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: High GPS HDOP")); } AP_Notify::flags.pre_arm_gps_check = false; return false; @@ -695,13 +695,13 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { if(!ins.get_accel_health_all()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Accelerometers not healthy")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Accelerometers not healthy")); } return false; } if(!ins.get_gyro_health_all()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Gyros not healthy")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Gyros not healthy")); } return false; } @@ -710,7 +710,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) // always check if inertial nav has started and is ready if(!ahrs.healthy()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Waiting for Nav Checks")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Waiting for Nav Checks")); } return false; } @@ -718,7 +718,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) // always check if the current mode allows arming if (!mode_allows_arming(control_mode, arming_from_gcs)) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Mode not armable")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Mode not armable")); } return false; } @@ -728,7 +728,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) // heli specific arming check if ((rsc_control_deglitched > 0) || !motors.allow_arming()){ if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Rotor Control Engaged")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Rotor Control Engaged")); } return false; } @@ -744,7 +744,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) // baro health check if (!barometer.all_healthy()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Barometer not healthy")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Barometer not healthy")); } return false; } @@ -755,7 +755,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); if (using_baro_ref && (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Altitude disparity")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Altitude disparity")); } return false; } @@ -770,7 +770,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) // check vehicle is within fence if(!fence.pre_arm_check()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: check fence")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: check fence")); } return false; } @@ -780,7 +780,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > aparm.angle_max) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Leaning")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Leaning")); } return false; } @@ -790,7 +790,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Check Battery")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Check Battery")); } return false; } @@ -802,9 +802,9 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Collective below Failsafe")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Collective below Failsafe")); #else - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Throttle below Failsafe")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Throttle below Failsafe")); #endif } return false; @@ -816,9 +816,9 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) if (channel_throttle->control_in > get_takeoff_trigger_throttle()) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Collective too high")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Collective too high")); #else - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Throttle too high")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Throttle too high")); #endif } return false; @@ -827,9 +827,9 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && channel_throttle->control_in > 0) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Collective too high")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Collective too high")); #else - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Throttle too high")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Throttle too high")); #endif } return false; @@ -840,7 +840,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) // check if safety switch has been pushed if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Safety Switch")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Safety Switch")); } return false; } @@ -858,7 +858,7 @@ void Copter::init_disarm_motors() } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL - gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("DISARMING MOTORS")); #endif // save compass offsets learned by the EKF @@ -928,7 +928,7 @@ void Copter::lost_vehicle_check() if (soundalarm_counter >= LOST_VEHICLE_DELAY) { if (AP_Notify::flags.vehicle_lost == false) { AP_Notify::flags.vehicle_lost = true; - gcs_send_text_P(SEVERITY_HIGH,PSTR("Locate Copter Alarm!")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Locate Copter Alarm!")); } } else { soundalarm_counter++; diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index fa0cf38c5a..5c3d6791f9 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -4,13 +4,13 @@ void Copter::init_barometer(bool full_calibration) { - gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer")); + gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Calibrating barometer")); if (full_calibration) { barometer.calibrate(); }else{ barometer.update_calibration(); } - gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete")); + gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("barometer calibration complete")); } // return barometric altitude in centimeters diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 22cf660d96..0a5fdc331d 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -597,7 +597,7 @@ void Copter::save_trim() float pitch_trim = ToRad((float)channel_pitch->control_in/100.0f); ahrs.add_trim(roll_trim, pitch_trim); Log_Write_Event(DATA_SAVE_TRIM); - gcs_send_text_P(SEVERITY_HIGH, PSTR("Trim saved")); + gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Trim saved")); } // auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 69c60a7119..a325ad98de 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -223,7 +223,7 @@ void Copter::init_ardupilot() 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")); delay(1000); } @@ -284,7 +284,7 @@ void Copter::init_ardupilot() //****************************************************************************** void Copter::startup_ground(bool force_gyro_cal) { - gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START")); + gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("GROUND START")); // initialise ahrs (may push imu calibration into the mpu6000 if using that device). ahrs.init();