Copter: update send text severities

This commit is contained in:
squilter 2015-08-11 16:50:34 -07:00 committed by Randy Mackay
parent f1d9b3570c
commit 767b4da5b6
14 changed files with 108 additions and 108 deletions

View File

@ -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);

View File

@ -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; i<num_gcs; i++) {
if (gcs[i].initialised) {
@ -1898,7 +1898,7 @@ void Copter::gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
void Copter::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);

View File

@ -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();
}

View File

@ -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; i<compass.get_count(); i++) {
if (!compass.healthy(i)) {
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("check compass"));
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, PSTR("check compass"));
ap.compass_mot = false;
return 1;
}
@ -55,7 +55,7 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
// check if radio is calibrated
pre_arm_rc_checks();
if (!ap.pre_arm_rc_check) {
gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("RC not calibrated"));
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, PSTR("RC not calibrated"));
ap.compass_mot = false;
return 1;
}
@ -63,14 +63,14 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
// check throttle is at zero
read_radio();
if (channel_throttle->control_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);
}

View File

@ -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;
}
}

View File

@ -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;

View File

@ -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();

View File

@ -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);

View File

@ -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);
}

View File

@ -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;
}

View File

@ -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++;

View File

@ -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

View File

@ -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

View File

@ -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();