From e4c6c0ad1799317f34319595562ba300acaaeee9 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 15 Sep 2021 12:18:19 +0530 Subject: [PATCH] AP_UAVCAN:add support for sending Vehicle Notify state --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 105 +++++++++++++++++++++++++++++- libraries/AP_UAVCAN/AP_UAVCAN.h | 7 ++ 2 files changed, 111 insertions(+), 1 deletion(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index da3a6b01c7..1ee7ff42f6 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -106,6 +107,14 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { // @User: Advanced AP_GROUPINFO("OPTION", 5, AP_UAVCAN, _options, 0), + // @Param: NTF_RT + // @DisplayName: Notify State rate + // @Description: Maximum transmit rate for Notify State Message + // @Range: 1 200 + // @Units: Hz + // @User: Advanced + AP_GROUPINFO("NTF_RT", 6, AP_UAVCAN, _notify_state_hz, 20), + AP_GROUPEND }; @@ -121,6 +130,7 @@ static uavcan::Publisher* buzzer[HAL static uavcan::Publisher* safety_state[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static uavcan::Publisher* arming_status[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static uavcan::Publisher* rtcm_stream[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* notify_state[HAL_MAX_CAN_PROTOCOL_DRIVERS]; // Clients UC_CLIENT_CALL_REGISTRY_BINDER(ParamGetSetCb, uavcan::protocol::param::GetSet); @@ -310,7 +320,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) rtcm_stream[driver_index] = new uavcan::Publisher(*_node); rtcm_stream[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); rtcm_stream[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); - + + notify_state[driver_index] = new uavcan::Publisher(*_node); + notify_state[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); + notify_state[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + param_get_set_client[driver_index] = new uavcan::ServiceClient(*_node, ParamGetSetCb(this, &AP_UAVCAN::handle_param_get_set_response)); param_execute_opcode_client[driver_index] = new uavcan::ServiceClient(*_node, ParamExecuteOpcodeCb(this, &AP_UAVCAN::handle_param_save_response)); @@ -413,6 +427,7 @@ void AP_UAVCAN::loop(void) buzzer_send(); rtcm_stream_send(); safety_state_send(); + notify_state_send(); send_parameter_request(); send_parameter_save_request(); AP::uavcan_dna_server().verify_nodes(this); @@ -623,6 +638,94 @@ void AP_UAVCAN::set_buzzer_tone(float frequency, float duration_s) _buzzer.pending_mask = 0xFF; } +// notify state send +void AP_UAVCAN::notify_state_send() +{ + uint32_t now = AP_HAL::native_millis(); + + if (_notify_state_hz == 0 || (now - _last_notify_state_ms) < uint32_t(1000 / _notify_state_hz)) { + return; + } + + ardupilot::indication::NotifyState msg; + msg.vehicle_state = 0; + if (AP_Notify::flags.initialising) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_INITIALISING; + } + if (AP_Notify::flags.armed) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_ARMED; + } + if (AP_Notify::flags.flying) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FLYING; + } + if (AP_Notify::flags.compass_cal_running) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_MAGCAL_RUN; + } + if (AP_Notify::flags.ekf_bad) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_EKF_BAD; + } + if (AP_Notify::flags.esc_calibration) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_ESC_CALIBRATION; + } + if (AP_Notify::flags.failsafe_battery) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FAILSAFE_BATT; + } + if (AP_Notify::flags.failsafe_gcs) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FAILSAFE_GCS; + } + if (AP_Notify::flags.failsafe_radio) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FAILSAFE_RADIO; + } + if (AP_Notify::flags.firmware_update) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FW_UPDATE; + } + if (AP_Notify::flags.gps_fusion) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_GPS_FUSION; + } + if (AP_Notify::flags.gps_glitching) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_GPS_GLITCH; + } + if (AP_Notify::flags.have_pos_abs) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_POS_ABS_AVAIL; + } + if (AP_Notify::flags.leak_detected) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_LEAK_DET; + } + if (AP_Notify::flags.parachute_release) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_CHUTE_RELEASED; + } + if (AP_Notify::flags.powering_off) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_POWERING_OFF; + } + if (AP_Notify::flags.pre_arm_check) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_PREARM; + } + if (AP_Notify::flags.pre_arm_gps_check) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_PREARM_GPS; + } + if (AP_Notify::flags.save_trim) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_SAVE_TRIM; + } + if (AP_Notify::flags.vehicle_lost) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_LOST; + } + if (AP_Notify::flags.video_recording) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_VIDEO_RECORDING; + } + if (AP_Notify::flags.waiting_for_throw) { + msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_THROW_READY; + } + + msg.aux_data_type = ardupilot::indication::NotifyState::VEHICLE_YAW_EARTH_CENTIDEGREES; + uint16_t yaw_cd = (uint16_t)(360.0f - degrees(AP::ahrs().get_yaw()))*100.0f; + const uint8_t *data = (uint8_t *)&yaw_cd; + for (uint8_t i=0; i<2; i++) { + msg.aux_data.push_back(data[i]); + } + notify_state[_driver_index]->broadcast(msg); + _last_notify_state_ms = AP_HAL::native_millis(); +} + void AP_UAVCAN::rtcm_stream_send() { WITH_SEMAPHORE(_rtcm_stream.sem); diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index dfa85e63d8..31ab881f06 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -232,6 +232,9 @@ private: // SafetyState void safety_state_send(); + // send notify vehicle state + void notify_state_send(); + // send GNSS injection void rtcm_stream_send(); @@ -262,6 +265,7 @@ private: AP_Int32 _esc_bm; AP_Int16 _servo_rate_hz; AP_Int16 _options; + AP_Int16 _notify_state_hz; uavcan::Node<0> *_node; @@ -319,6 +323,9 @@ private: // safety status send state uint32_t _last_safety_state_ms; + // notify vehicle state + uint32_t _last_notify_state_ms; + // incoming button handling static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb); static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);