|
@@ -114,7 +114,7 @@ void setup()
|
|
|
WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
|
|
WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
|
|
|
|
|
|
|
|
g.init();
|
|
g.init();
|
|
|
-
|
|
|
|
|
|
|
+ // g.protocol = 2;
|
|
|
|
|
|
|
|
led.set_state(Led::LedState::INIT);
|
|
led.set_state(Led::LedState::INIT);
|
|
|
led.update();
|
|
led.update();
|
|
@@ -310,10 +310,16 @@ int odid_to_gb2025_safe(const ODID_UAS_Data *odid, UavIdentificationData *gb) {
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/* ========== 高度类处理(013/014/007/011) ========== */
|
|
/* ========== 高度类处理(013/014/007/011) ========== */
|
|
|
|
|
+
|
|
|
if (odid->LocationValid) {
|
|
if (odid->LocationValid) {
|
|
|
|
|
+ // Serial.printf("Geo altitude %f \r\n", odid->Location.AltitudeGeo);
|
|
|
|
|
+ // Serial.printf("Rel Height %f \r\n", odid->Location.Height);
|
|
|
|
|
+ // Serial.printf("Baro altitude %f \r\n", odid->Location.AltitudeBaro);
|
|
|
|
|
+ // Serial.printf("time stamp %f \r\n", odid->Location.TimeStamp);
|
|
|
// 大地高度
|
|
// 大地高度
|
|
|
if (odid->Location.AltitudeGeo > MIN_ALT &&
|
|
if (odid->Location.AltitudeGeo > MIN_ALT &&
|
|
|
odid->Location.AltitudeGeo <= MAX_ALT) {
|
|
odid->Location.AltitudeGeo <= MAX_ALT) {
|
|
|
|
|
+
|
|
|
gb->uav_geo_alt = odid->Location.AltitudeGeo;
|
|
gb->uav_geo_alt = odid->Location.AltitudeGeo;
|
|
|
gb->is_uav_geo_alt_valid = true;
|
|
gb->is_uav_geo_alt_valid = true;
|
|
|
} else {
|
|
} else {
|
|
@@ -322,8 +328,10 @@ int odid_to_gb2025_safe(const ODID_UAS_Data *odid, UavIdentificationData *gb) {
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// 气压高度
|
|
// 气压高度
|
|
|
|
|
+
|
|
|
if (odid->Location.AltitudeBaro > MIN_ALT &&
|
|
if (odid->Location.AltitudeBaro > MIN_ALT &&
|
|
|
odid->Location.AltitudeBaro <= MAX_ALT) {
|
|
odid->Location.AltitudeBaro <= MAX_ALT) {
|
|
|
|
|
+
|
|
|
gb->uav_press_alt = odid->Location.AltitudeBaro;
|
|
gb->uav_press_alt = odid->Location.AltitudeBaro;
|
|
|
gb->is_press_alt_valid = true;
|
|
gb->is_press_alt_valid = true;
|
|
|
gb->flag_press_alt = true;
|
|
gb->flag_press_alt = true;
|
|
@@ -334,9 +342,11 @@ int odid_to_gb2025_safe(const ODID_UAS_Data *odid, UavIdentificationData *gb) {
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// 相对高度
|
|
// 相对高度
|
|
|
|
|
+
|
|
|
if (odid->Location.Height > MIN_ALT &&
|
|
if (odid->Location.Height > MIN_ALT &&
|
|
|
odid->Location.Height <= MAX_ALT) {
|
|
odid->Location.Height <= MAX_ALT) {
|
|
|
gb->uav_rel_alt = odid->Location.Height;
|
|
gb->uav_rel_alt = odid->Location.Height;
|
|
|
|
|
+
|
|
|
gb->is_rel_alt_valid = true;
|
|
gb->is_rel_alt_valid = true;
|
|
|
gb->flag_rel_alt = true;
|
|
gb->flag_rel_alt = true;
|
|
|
} else {
|
|
} else {
|
|
@@ -375,19 +385,7 @@ int odid_to_gb2025_safe(const ODID_UAS_Data *odid, UavIdentificationData *gb) {
|
|
|
(UavTimestampAccuracy_t)odid->Location.TSAccuracy : UAV_TIME_ACC_UNKNOWN;
|
|
(UavTimestampAccuracy_t)odid->Location.TSAccuracy : UAV_TIME_ACC_UNKNOWN;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- /* ========== 020-时间戳 ========== */
|
|
|
|
|
- if (odid->LocationValid) {
|
|
|
|
|
- if (odid->Location.TimeStamp != INV_TIMESTAMP &&
|
|
|
|
|
- odid->Location.TimeStamp <= MAX_TIMESTAMP) {
|
|
|
|
|
- gb->timestamp = (uint64_t)(odid->Location.TimeStamp * 1000);
|
|
|
|
|
- gb->is_timestamp_valid = true;
|
|
|
|
|
- } else {
|
|
|
|
|
- gb->timestamp = 0;
|
|
|
|
|
- gb->is_timestamp_valid = false;
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
-
|
|
|
|
|
- /* ========== 遥控站位置 ========== */
|
|
|
|
|
|
|
+ /* ========== 遥控站位置 时间戳========== */
|
|
|
if (odid->SystemValid) {
|
|
if (odid->SystemValid) {
|
|
|
// 检查遥控站经纬度
|
|
// 检查遥控站经纬度
|
|
|
bool sta_lat_valid = (odid->System.OperatorLatitude >= -90 &&
|
|
bool sta_lat_valid = (odid->System.OperatorLatitude >= -90 &&
|
|
@@ -415,6 +413,17 @@ int odid_to_gb2025_safe(const ODID_UAS_Data *odid, UavIdentificationData *gb) {
|
|
|
gb->station_geo_alt = 0;
|
|
gb->station_geo_alt = 0;
|
|
|
gb->is_station_alt_valid = false;
|
|
gb->is_station_alt_valid = false;
|
|
|
}
|
|
}
|
|
|
|
|
+
|
|
|
|
|
+ if (odid->System.Timestamp < 0)
|
|
|
|
|
+ {
|
|
|
|
|
+ gb->timestamp = 0;
|
|
|
|
|
+ gb->is_timestamp_valid = false;
|
|
|
|
|
+
|
|
|
|
|
+ } else {
|
|
|
|
|
+ gb->timestamp = (uint64_t)(odid->System.Timestamp * 1000); // 使用unix时间 单位ms
|
|
|
|
|
+ gb->is_timestamp_valid = true;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
return 0;
|
|
return 0;
|
|
@@ -430,118 +439,45 @@ static const char *check_parse(void)
|
|
|
{
|
|
{
|
|
|
String ret = "";
|
|
String ret = "";
|
|
|
|
|
|
|
|
- if(g.protocol == PROTOCOL_EU)
|
|
|
|
|
{
|
|
{
|
|
|
- {
|
|
|
|
|
- ODID_Location_encoded encoded {};
|
|
|
|
|
- if (encodeLocationMessage(&encoded, &UAS_data.Location) != ODID_SUCCESS) {
|
|
|
|
|
- ret += "LOC ";
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- {
|
|
|
|
|
- ODID_System_encoded encoded {};
|
|
|
|
|
- if (encodeSystemMessage(&encoded, &UAS_data.System) != ODID_SUCCESS) {
|
|
|
|
|
- ret += "SYS ";
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- {
|
|
|
|
|
- ODID_BasicID_encoded encoded {};
|
|
|
|
|
- if (UAS_data.BasicIDValid[0] == 1) {
|
|
|
|
|
- if (encodeBasicIDMessage(&encoded, &UAS_data.BasicID[0]) != ODID_SUCCESS) {
|
|
|
|
|
- ret += "ID_1 ";
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- memset(&encoded, 0, sizeof(encoded));
|
|
|
|
|
- if (UAS_data.BasicIDValid[1] == 1) {
|
|
|
|
|
- if (encodeBasicIDMessage(&encoded, &UAS_data.BasicID[1]) != ODID_SUCCESS) {
|
|
|
|
|
- ret += "ID_2 ";
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- {
|
|
|
|
|
- ODID_SelfID_encoded encoded {};
|
|
|
|
|
- if (encodeSelfIDMessage(&encoded, &UAS_data.SelfID) != ODID_SUCCESS) {
|
|
|
|
|
- ret += "SELF_ID ";
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- {
|
|
|
|
|
- ODID_OperatorID_encoded encoded {};
|
|
|
|
|
- if (encodeOperatorIDMessage(&encoded, &UAS_data.OperatorID) != ODID_SUCCESS) {
|
|
|
|
|
- ret += "OP_ID ";
|
|
|
|
|
- }
|
|
|
|
|
|
|
+ ODID_Location_encoded encoded {};
|
|
|
|
|
+ if (encodeLocationMessage(&encoded, &UAS_data.Location) != ODID_SUCCESS) {
|
|
|
|
|
+ ret += "LOC ";
|
|
|
}
|
|
}
|
|
|
-
|
|
|
|
|
- }else if(g.protocol == PROTOCOL_CN2023) // 国标状态处理
|
|
|
|
|
|
|
+ }
|
|
|
{
|
|
{
|
|
|
- {
|
|
|
|
|
- CNDID_Location_encoded encoded {};
|
|
|
|
|
- if (encodeCNLocationMessage(&encoded, &CN_UAS_data.Location) != CNDID_SUCCESS) {
|
|
|
|
|
- ret += "LOC ";
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- {
|
|
|
|
|
- CNDID_System_encoded encoded {};
|
|
|
|
|
- if (encodeCNSystemMessage(&encoded, &CN_UAS_data.System) != CNDID_SUCCESS) {
|
|
|
|
|
- ret += "SYS ";
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- {
|
|
|
|
|
- CNDID_BasicID_encoded encoded {};
|
|
|
|
|
- if (CN_UAS_data.BasicIDValid[0] == 1) {
|
|
|
|
|
- if (encodeCNBasicIDMessage(&encoded, &CN_UAS_data.BasicID[0]) != CNDID_SUCCESS) {
|
|
|
|
|
- ret += "ID_1 ";
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- memset(&encoded, 0, sizeof(encoded));
|
|
|
|
|
- if (CN_UAS_data.BasicIDValid[1] == 1) {
|
|
|
|
|
- if (encodeCNBasicIDMessage(&encoded, &CN_UAS_data.BasicID[1]) != CNDID_SUCCESS) {
|
|
|
|
|
- ret += "ID_2 ";
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- {
|
|
|
|
|
- CNDID_SelfID_encoded encoded {};
|
|
|
|
|
- if (encodeCNSelfIDMessage(&encoded, &CN_UAS_data.SelfID) != CNDID_SUCCESS) {
|
|
|
|
|
- ret += "SELF_ID ";
|
|
|
|
|
- }
|
|
|
|
|
|
|
+ ODID_System_encoded encoded {};
|
|
|
|
|
+ if (encodeSystemMessage(&encoded, &UAS_data.System) != ODID_SUCCESS) {
|
|
|
|
|
+ ret += "SYS ";
|
|
|
}
|
|
}
|
|
|
-
|
|
|
|
|
}
|
|
}
|
|
|
- else if(g.protocol == PROTOCOL_GB2025)
|
|
|
|
|
{
|
|
{
|
|
|
- {
|
|
|
|
|
- CNDID_Location_encoded encoded {};
|
|
|
|
|
- if (encodeCNLocationMessage(&encoded, &CN_UAS_data.Location) != CNDID_SUCCESS) {
|
|
|
|
|
- ret += "LOC ";
|
|
|
|
|
|
|
+ ODID_BasicID_encoded encoded {};
|
|
|
|
|
+ if (UAS_data.BasicIDValid[0] == 1) {
|
|
|
|
|
+ if (encodeBasicIDMessage(&encoded, &UAS_data.BasicID[0]) != ODID_SUCCESS) {
|
|
|
|
|
+ ret += "ID_1 ";
|
|
|
}
|
|
}
|
|
|
}
|
|
}
|
|
|
- {
|
|
|
|
|
- CNDID_System_encoded encoded {};
|
|
|
|
|
- if (encodeCNSystemMessage(&encoded, &CN_UAS_data.System) != CNDID_SUCCESS) {
|
|
|
|
|
- ret += "SYS ";
|
|
|
|
|
|
|
+ memset(&encoded, 0, sizeof(encoded));
|
|
|
|
|
+ if (UAS_data.BasicIDValid[1] == 1) {
|
|
|
|
|
+ if (encodeBasicIDMessage(&encoded, &UAS_data.BasicID[1]) != ODID_SUCCESS) {
|
|
|
|
|
+ ret += "ID_2 ";
|
|
|
}
|
|
}
|
|
|
}
|
|
}
|
|
|
- {
|
|
|
|
|
- CNDID_BasicID_encoded encoded {};
|
|
|
|
|
- if (CN_UAS_data.BasicIDValid[0] == 1) {
|
|
|
|
|
- if (encodeCNBasicIDMessage(&encoded, &CN_UAS_data.BasicID[0]) != CNDID_SUCCESS) {
|
|
|
|
|
- ret += "ID_1 ";
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- memset(&encoded, 0, sizeof(encoded));
|
|
|
|
|
- if (CN_UAS_data.BasicIDValid[1] == 1) {
|
|
|
|
|
- if (encodeCNBasicIDMessage(&encoded, &CN_UAS_data.BasicID[1]) != CNDID_SUCCESS) {
|
|
|
|
|
- ret += "ID_2 ";
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
|
|
+ }
|
|
|
|
|
+ {
|
|
|
|
|
+ ODID_SelfID_encoded encoded {};
|
|
|
|
|
+ if (encodeSelfIDMessage(&encoded, &UAS_data.SelfID) != ODID_SUCCESS) {
|
|
|
|
|
+ ret += "SELF_ID ";
|
|
|
}
|
|
}
|
|
|
- {
|
|
|
|
|
- CNDID_SelfID_encoded encoded {};
|
|
|
|
|
- if (encodeCNSelfIDMessage(&encoded, &CN_UAS_data.SelfID) != CNDID_SUCCESS) {
|
|
|
|
|
- ret += "SELF_ID ";
|
|
|
|
|
- }
|
|
|
|
|
|
|
+ }
|
|
|
|
|
+ {
|
|
|
|
|
+ ODID_OperatorID_encoded encoded {};
|
|
|
|
|
+ if (encodeOperatorIDMessage(&encoded, &UAS_data.OperatorID) != ODID_SUCCESS) {
|
|
|
|
|
+ ret += "OP_ID ";
|
|
|
}
|
|
}
|
|
|
}
|
|
}
|
|
|
|
|
+
|
|
|
if (ret.length() > 0) {
|
|
if (ret.length() > 0) {
|
|
|
// if all errors would occur in this function, it will fit in
|
|
// if all errors would occur in this function, it will fit in
|
|
|
// 50 chars that is also the max for the arm status message
|
|
// 50 chars that is also the max for the arm status message
|
|
@@ -808,7 +744,6 @@ static void set_data(Transport &t) // 两种类型都获取 广播按照需要
|
|
|
if (g.options & OPTIONS_FORCE_ARM_OK) { // 设置强制绿灯?
|
|
if (g.options & OPTIONS_FORCE_ARM_OK) { // 设置强制绿灯?
|
|
|
arm_check_ok = true;
|
|
arm_check_ok = true;
|
|
|
}
|
|
}
|
|
|
-
|
|
|
|
|
led.set_state(pfst_check_ok && arm_check_ok? Led::LedState::ARM_OK : Led::LedState::ARM_FAIL);
|
|
led.set_state(pfst_check_ok && arm_check_ok? Led::LedState::ARM_OK : Led::LedState::ARM_FAIL);
|
|
|
|
|
|
|
|
uint32_t now_ms = millis();
|
|
uint32_t now_ms = millis();
|
|
@@ -843,7 +778,11 @@ void loop()
|
|
|
#else
|
|
#else
|
|
|
#error "Must enable DroneCAN or MAVLink"
|
|
#error "Must enable DroneCAN or MAVLink"
|
|
|
#endif
|
|
#endif
|
|
|
-
|
|
|
|
|
|
|
+ if(g.com_port == CAN_PORT){
|
|
|
|
|
+ auto &transport = dronecan;
|
|
|
|
|
+ }else if(g.com_port == UART_PORT){
|
|
|
|
|
+ auto &transport = mavlink1;
|
|
|
|
|
+ }
|
|
|
bool have_location = false;
|
|
bool have_location = false;
|
|
|
// - last_location_ms:最后一次收到有效位置数据的时间戳(毫秒)
|
|
// - last_location_ms:最后一次收到有效位置数据的时间戳(毫秒)
|
|
|
// - last_system_ms:最后一次收到飞控系统消息的时间戳(毫秒)
|
|
// - last_system_ms:最后一次收到飞控系统消息的时间戳(毫秒)
|
|
@@ -856,18 +795,14 @@ void loop()
|
|
|
|
|
|
|
|
if (last_location_ms == 0 ||
|
|
if (last_location_ms == 0 ||
|
|
|
now_ms - last_location_ms > 5000) {
|
|
now_ms - last_location_ms > 5000) {
|
|
|
- // 判定条件1:位置数据异常(超时/未收到)
|
|
|
|
|
- // - last_location_ms == 0:从未收到过位置数据
|
|
|
|
|
- // - 现在时间 - 最后一次位置时间 > 5000ms:位置数据超时5秒
|
|
|
|
|
|
|
+
|
|
|
UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE; // 相同的故障ID 4 // web页面可视
|
|
UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE; // 相同的故障ID 4 // web页面可视
|
|
|
CN_UAS_data.Location.Status = CNDID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
|
|
CN_UAS_data.Location.Status = CNDID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
if (last_system_ms == 0 ||
|
|
if (last_system_ms == 0 ||
|
|
|
now_ms - last_system_ms > 5000) {
|
|
now_ms - last_system_ms > 5000) {
|
|
|
- //判定条件2:系统通信异常(飞控消息超时/未收到)
|
|
|
|
|
- // - last_system_ms == 0:从未收到过飞控系统消息
|
|
|
|
|
- // - 超时5秒:飞控和RemoteID模块断连超过5秒
|
|
|
|
|
|
|
+
|
|
|
UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
|
|
UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
|
|
|
CN_UAS_data.Location.Status = CNDID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
|
|
CN_UAS_data.Location.Status = CNDID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
|
|
|
}
|
|
}
|
|
@@ -875,15 +810,16 @@ void loop()
|
|
|
if (transport.get_parse_fail() != nullptr) {
|
|
if (transport.get_parse_fail() != nullptr) {
|
|
|
UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
|
|
UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
|
|
|
CN_UAS_data.Location.Status = CNDID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
|
|
CN_UAS_data.Location.Status = CNDID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
|
|
|
- // 记录解析失败的具体原因(如“GPS数据格式错误”)
|
|
|
|
|
|
|
+
|
|
|
status_reason = String(transport.get_parse_fail()); // 状态原因上传web页面
|
|
status_reason = String(transport.get_parse_fail()); // 状态原因上传web页面
|
|
|
- } // 状态原因还是沿用opendroneid 的 国标没有要求反馈解析失败原因
|
|
|
|
|
|
|
+ }
|
|
|
|
|
|
|
|
// web update has to happen after we update Status above
|
|
// web update has to happen after we update Status above
|
|
|
// 网络更新必须在我们更新上述状态之后进行
|
|
// 网络更新必须在我们更新上述状态之后进行
|
|
|
if (g.webserver_enable) {
|
|
if (g.webserver_enable) {
|
|
|
webif.update();
|
|
webif.update();
|
|
|
}
|
|
}
|
|
|
|
|
+
|
|
|
if (g.bcast_powerup) { // true 上电即广播"模式
|
|
if (g.bcast_powerup) { // true 上电即广播"模式
|
|
|
// if we are broadcasting on powerup we always mark location valid
|
|
// if we are broadcasting on powerup we always mark location valid
|
|
|
// so the location with default data is sent
|
|
// so the location with default data is sent
|
|
@@ -947,7 +883,7 @@ void loop()
|
|
|
ble.transmit_cn_longrange(CN_UAS_data);
|
|
ble.transmit_cn_longrange(CN_UAS_data);
|
|
|
else if(g.protocol == PROTOCOL_GB2025)
|
|
else if(g.protocol == PROTOCOL_GB2025)
|
|
|
ble.transmit_GB2025_longrange(GB2025_UAS_data); // 国标只要求bt5
|
|
ble.transmit_GB2025_longrange(GB2025_UAS_data); // 国标只要求bt5
|
|
|
- // Serial.printf("update_bt5\n");
|
|
|
|
|
|
|
+ Serial.printf("update_bt5\n");
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
static uint32_t last_update_bt4_ms; // BLE 近端蓝牙发送频率 bt4_rate 达到对应的时间了就启动RemoteID BLE4发送
|
|
static uint32_t last_update_bt4_ms; // BLE 近端蓝牙发送频率 bt4_rate 达到对应的时间了就启动RemoteID BLE4发送
|
|
@@ -967,7 +903,7 @@ void loop()
|
|
|
ble.transmit_cn_legacy(CN_UAS_data);
|
|
ble.transmit_cn_legacy(CN_UAS_data);
|
|
|
// Serial.printf("update_bt4\n");
|
|
// Serial.printf("update_bt4\n");
|
|
|
}
|
|
}
|
|
|
-
|
|
|
|
|
|
|
+
|
|
|
// sleep for a bit for power saving 节能
|
|
// sleep for a bit for power saving 节能
|
|
|
delay(1);
|
|
delay(1);
|
|
|
}
|
|
}
|