mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
Copter: update send text severities
This commit is contained in:
parent
f1d9b3570c
commit
767b4da5b6
@ -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);
|
||||
|
@ -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);
|
||||
|
@ -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();
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
|
||||
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
@ -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;
|
||||
|
@ -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();
|
||||
|
@ -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);
|
||||
|
@ -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);
|
||||
|
||||
}
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
@ -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++;
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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();
|
||||
|
Loading…
Reference in New Issue
Block a user