zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

串口读取调整

zr-v5.1
Brown.Z 3 years ago
parent
96bfd6904a
commit
2d67228afe
5 changed files with 155 additions and 6 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      ArduCopter/zr_app.cpp
  2. 2
      just_build.sh
  3. 136
      libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp
  4. 16
      libraries/AC_ZR_APP/AC_ZR_Serial_API.h
  5. 3
      zr-v5.sh

4
ArduCopter/zr_app.cpp
Unescape View File

@ -64,7 +64,7 @@ void Copter::zr_app_50hz(){ @@ -64,7 +64,7 @@ void Copter::zr_app_50hz(){
float yaw_deg;
bool new_data = zr_serial_api.get_control_data((uint8_t)flightmode->mode_number() ,msg_type,data,yaw_deg);
if(new_data){
gcs().send_text(MAV_SEVERITY_INFO,"get msg_type:%d, data:%d,%d,%d, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg);
gcs().send_text(MAV_SEVERITY_INFO,"get msg_type:%d, data:%ld,%ld,%ld, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg);
switch (msg_type)
{
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_TKOFF:
@ -89,7 +89,7 @@ void Copter::zr_app_50hz(){ @@ -89,7 +89,7 @@ void Copter::zr_app_50hz(){
target_loc.alt = (data.z + 500);
// target_loc.alt = (data.z + 500)/100.0;
set_target_location(target_loc);
gcs().send_text(MAV_SEVERITY_INFO,"set location,lat %d,%d,%d",target_loc.lat,target_loc.lng,target_loc.alt);
gcs().send_text(MAV_SEVERITY_INFO,"set location,lat %ld,%ld,%ld",target_loc.lat,target_loc.lng,target_loc.alt);
}
break;
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL:

2
just_build.sh
Unescape View File

@ -1,2 +1,2 @@ @@ -1,2 +1,2 @@
./waf copter
cp build/Pixhawk4/bin/arducopter.apj /mnt/hgfs/firmware/编译/zr-415.px4
cp build/zr-v5/bin/arducopter.apj /mnt/hgfs/firmware/编译/zrv5-415.px4

136
libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp
Unescape View File

@ -41,6 +41,12 @@ AC_ZR_Serial_API::AC_ZR_Serial_API() @@ -41,6 +41,12 @@ AC_ZR_Serial_API::AC_ZR_Serial_API()
if (_singleton != nullptr) {
AP_HAL::panic("AC_ZR_Serial_API must be singleton");
}
bufsize = MAX(CONTROL_DATA_LEN, 50);
pktbuf = new uint8_t[bufsize];
if (!pktbuf ) {
AP_HAL::panic("Failed to allocate AC_ZR_Serial_API");
}
_singleton = this;
}
@ -111,7 +117,129 @@ void AC_ZR_Serial_API::read_data_from_port(AP_HAL::UARTDriver *_port,uint8_t *da @@ -111,7 +117,129 @@ void AC_ZR_Serial_API::read_data_from_port(AP_HAL::UARTDriver *_port,uint8_t *da
*len += 1;
}
}
/*
check the UART for more data
returns true if the function should be called again straight away
*/
bool AC_ZR_Serial_API::check_uart(AP_HAL::UARTDriver *_port)
{
if (_port == nullptr) {
return false;
}
uint16_t crc;
WITH_SEMAPHORE(control_data.sem);
uint32_t n = _port->available();
if (n == 0) {
return false;
}
if (pktoffset < bufsize) {
ssize_t nread = _port->read(&pktbuf[pktoffset], MIN(n, unsigned(bufsize-pktoffset)));
serial_last_data_time = AP_HAL::millis();
if (nread <= 0) {
return false;
}
pktoffset += nread;
}
if (pktbuf[0] != 0xFE) {
// goto reset;
}
uint8_t *p = (uint8_t *)memchr(&pktbuf[0], (char)0xFE, pktoffset-1);
if (p) {
uint8_t newlen = pktoffset - (p - pktbuf);
memmove(&pktbuf[0], p, newlen);
pktoffset = newlen;
Debug("move msg:%d",newlen);
} else {
// pktoffset = 0;
goto reset;
}
if (pktbuf[0] != 0xFE) {
Debug("no head:%d",pktoffset);
return false;
}
crc = crc_crc8(pktbuf, CONTROL_DATA_LEN-1);
if (crc == pktbuf[CONTROL_DATA_LEN-1]) {
// got pkt1
memcpy(flight_control_parser.data,pktbuf,CONTROL_DATA_LEN); // 收到一帧数据
control_data_last_time = AP_HAL::millis();
Debug(" %ld-get msg,ofs:%d ",control_data_last_time,pktoffset);
// process_packet(pktbuf);
memmove(&pktbuf[0], &pktbuf[CONTROL_DATA_LEN], pktoffset-CONTROL_DATA_LEN);
pktoffset -= CONTROL_DATA_LEN;
} else {
Debug("crc error clc: %02x ,rece: %02x",crc,pktbuf[CONTROL_DATA_LEN-1]);
set_control_ask(pktbuf[3],flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRCRC); // 应答CRC错误
goto reset;
}
return true;
reset:
uint8_t *p2 = (uint8_t *)memchr(&pktbuf[1], (char)0xFE, pktoffset-1);
if (p2) {
uint8_t newlen = pktoffset - (p2 - pktbuf);
memmove(&pktbuf[0], p2, newlen);
pktoffset = newlen;
Debug("move msg:%d",newlen);
} else {
pktoffset = 0;
}
return true;
}
void AC_ZR_Serial_API::process_packet(const uint8_t *b)
{
static uint32_t last_time = 0;
uint8_t crc_sum = 0;
WITH_SEMAPHORE(control_data.sem);
memcpy(flight_control_parser.data,b,CONTROL_DATA_LEN);
if(control_data_last_time == last_time){ // 数据没更新，直接退出
return ;
}
last_time = control_data_last_time;
if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头
Debug("head error: %02x",flight_control_parser.msg.head );
return ;
}
if(crc_sum != flight_control_parser.data[CONTROL_DATA_LEN - 1]){
Debug("crc error clc: %02x ,rece: %02x",crc_sum,flight_control_parser.msg.crc);
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRCRC); // 应答CRC错误
return ; // 校验失败
}
if(flight_control_parser.msg.type < ZR_Msg_Type::MSG_CONTROL_TKOFF || flight_control_parser.msg.type > ZR_Msg_Type::MSG_CONTROL_VEL ){
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答控制类型错误
return ; // 控制类型错误
}
if(flight_control_parser.msg.type != ZR_Msg_Type::MSG_CONTROL_TKOFF && flight_mode != 4){
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRMODE);
return ; // 飞行模式错误
}
control_data.time_stamp = AP_HAL::millis();
control_data_last_time = AP_HAL::millis();
control_data.type = flight_control_parser.msg.type;
control_data.data.x = flight_control_parser.msg.x;
control_data.data.y = flight_control_parser.msg.y;
control_data.data.z = flight_control_parser.msg.z;
control_data.yaw_deg = flight_control_parser.msg.yaw;
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_OK); // 应答成功
}
void AC_ZR_Serial_API::get_vehicle_NEU_pos(Vector3f vec_neu)
{
static char buf[50];
@ -138,6 +266,7 @@ void AC_ZR_Serial_API::get_vehicle_euler_angles(Vector3f euler) @@ -138,6 +266,7 @@ void AC_ZR_Serial_API::get_vehicle_euler_angles(Vector3f euler)
void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t home_distance,uint16_t volt_mv,uint8_t bat_remaining)
{
flight_mode = mode;
static uint8_t delay_cnt;
delay_cnt += 1;
@ -227,6 +356,7 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l @@ -227,6 +356,7 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l
return false;
}
last_time = control_data_last_time;
WITH_SEMAPHORE(control_data.sem);
if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头
Debug("head error: %02x",flight_control_parser.msg.head );
return false;
@ -283,8 +413,10 @@ void AC_ZR_Serial_API::update(void) @@ -283,8 +413,10 @@ void AC_ZR_Serial_API::update(void)
if(!data_trans_init || _zr_api_port == nullptr){
return;
}
#if 1
check_uart(_zr_api_port);
#else
read_data_from_port(_zr_api_port,serial_data_from_device, &serial_data_len); // 读取主设备串口数据
// Debug("receive host data:%d",mav_data_len);
uint32_t now_time = AP_HAL::millis();
if(serial_data_len > 0 && (now_time - serial_last_data_time > 50)){ // 接收可能被中断，延时一段
@ -297,7 +429,7 @@ void AC_ZR_Serial_API::update(void) @@ -297,7 +429,7 @@ void AC_ZR_Serial_API::update(void)
serial_data_len = 0;
// memset(serial_data_from_device,0,CONTROL_DATA_LEN);
}
#endif
// if(get_mav_data){
// Debug("%d,host data len:%d",now_time,mav_data_len);
// write_data_to_port(_zr_api_port,mav_data_from_host, mav_data_len);

16
libraries/AC_ZR_APP/AC_ZR_Serial_API.h
Unescape View File

@ -82,6 +82,9 @@ public: @@ -82,6 +82,9 @@ public:
private:
bool check_uart(AP_HAL::UARTDriver *_port);
void process_packet(const uint8_t *b);
static AC_ZR_Serial_API *_singleton;
AP_HAL::UARTDriver *_zr_api_port;
uint32_t serial_last_data_time;
@ -182,6 +185,19 @@ private: @@ -182,6 +185,19 @@ private:
uint8_t data[VEHICLE_STATUS_LEN];
}vehicle_status;
struct control_t {
HAL_Semaphore sem;
uint32_t time_stamp;
ZR_Msg_Type type;
Vector3l data;
float yaw_deg;
} control_data;
uint8_t *pktbuf;
uint16_t pktoffset;
uint16_t bufsize;
uint8_t flight_mode;
};

3
zr-v5.sh
Unescape View File

@ -1,2 +1,3 @@ @@ -1,2 +1,3 @@
./waf configure --board zr-v5
./waf copter
cp build/zr-v5/bin/arducopter.apj /mnt/hgfs/firmware/编译/zr-415-01.px4
cp build/zr-v5/bin/arducopter.apj /mnt/hgfs/firmware/编译/zr-v5-415.px4
Write Preview
Loading…
Cancel
Save