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

增加ZR_APP 

Coper增加脚本控制相关
增加串口状态输出和飞行控制API
增加zr_app 一些参数
zr-sdk-4.1.16
那个Zeng 3 years ago
parent
52edbca0d6
commit
feb3e52407
17 changed files with 1768 additions and 1 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. 21
      ArduCopter/Copter.h
  2. 1
      ArduCopter/Parameters.cpp
  3. 2
      ArduCopter/Parameters.h
  4. 2
      ArduCopter/UserCode.cpp
  5. 3
      ArduCopter/version.h
  6. 1
      ArduCopter/wscript
  7. 165
      ArduCopter/zr_app.cpp
  8. 345
      Tools/environment_install/ubuntu.sh
  9. 253
      libraries/AC_ZR_APP/AC_ZR_App.cpp
  10. 71
      libraries/AC_ZR_APP/AC_ZR_App.h
  11. 340
      libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp
  12. 203
      libraries/AC_ZR_APP/AC_ZR_Serial_API.h
  13. 328
      libraries/AC_ZR_APP/ZR_Des.cpp
  14. 22
      libraries/AC_ZR_APP/ZR_Des.h
  15. 7
      libraries/AP_SerialManager/AP_SerialManager.cpp
  16. 3
      libraries/AP_SerialManager/AP_SerialManager.h
  17. 2
      modules/mavlink

21
ArduCopter/Copter.h
Unescape View File

@ -68,6 +68,9 @@ @@ -68,6 +68,9 @@
#include <AC_AutoTune/AC_AutoTune.h>
#include <AP_Common/AP_FWVersion.h>
#include <AC_ZR_APP/AC_ZR_Serial_API.h>
#include <AC_ZR_APP/AC_ZR_App.h>
// Configuration
#include "defines.h"
#include "config.h"
@ -675,6 +678,19 @@ private: @@ -675,6 +678,19 @@ private:
void read_AHRS(void);
void update_altitude();
// zr_app.cpp
bool start_takeoff(float alt);
bool set_target_location(const Location& target_loc);
bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt);
bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel);
bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative);
bool set_target_velocity_NED(const Vector3f& vel_ned);
bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool relative_yaw);
bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs);
bool get_circle_radius(float &radius_m);
bool set_circle_rate(float rate_dps);
// Attitude.cpp
float get_pilot_desired_yaw_rate(int16_t stick_angle);
void update_throttle_hover();
@ -1033,6 +1049,8 @@ private: @@ -1033,6 +1049,8 @@ private:
Proxy_Area proxy_area;
Mode::Number before_mode;
AC_ZR_Serial_API zr_serial_api;
AC_ZR_App zr_app;
public:
void mavlink_delay_cb(); // GCS_Mavlink.cpp
void failsafe_check(); // failsafe.cpp
@ -1065,6 +1083,9 @@ public: @@ -1065,6 +1083,9 @@ public:
uint16_t mission_nav_index;
void takeoff_crash_detect();
void zr_app_50hz();
void zr_app_10hz();
void zr_app_1hz();
};
extern Copter copter;

1
ArduCopter/Parameters.cpp
Unescape View File

@ -782,6 +782,7 @@ const AP_Param::Info Copter::var_info[] = { @@ -782,6 +782,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Path: Parameters.cpp
GOBJECT(g2, "", ParametersG2),
GOBJECT(zr_app, "ZR", AC_ZR_App),
AP_VAREND
};

2
ArduCopter/Parameters.h
Unescape View File

@ -402,6 +402,8 @@ public: @@ -402,6 +402,8 @@ public:
k_param_zr_takeoff_prt_deg,
k_param_zr_takeoff_prt_ps,
k_param_zr_app = 322, // 322 zr param
// the k_param_* space is 9-bits in size
// 511: reserved
};

2
ArduCopter/UserCode.cpp
Unescape View File

@ -33,6 +33,7 @@ void Copter::userhook_FastLoop() @@ -33,6 +33,7 @@ void Copter::userhook_FastLoop()
{
// put your 100Hz code here
takeoff_crash_detect();
zr_app_10hz();
}
#endif
@ -40,6 +41,7 @@ void Copter::userhook_FastLoop() @@ -40,6 +41,7 @@ void Copter::userhook_FastLoop()
void Copter::userhook_50Hz()
{
// put your 50Hz code here
zr_app_50hz();
}
#endif

3
ArduCopter/version.h
Unescape View File

@ -5,7 +5,10 @@ @@ -5,7 +5,10 @@
#endif
#include "ap_version.h"
#ifdef GIT_VERSION
#undef GIT_VERSION
#define GIT_VERSION "3"
#endif
#define THISFIRMWARE "ZRUAV v4.1.16" //"ArduCopter V4.0.0"
// the following line is parsed by the autotest scripts

1
ArduCopter/wscript
Unescape View File

@ -31,6 +31,7 @@ def build(bld): @@ -31,6 +31,7 @@ def build(bld):
'AP_OSD',
'AC_AutoTune',
'AP_KDECAN',
'AC_ZR_APP',
],
)

165
ArduCopter/zr_app.cpp
Unescape View File

@ -0,0 +1,165 @@ @@ -0,0 +1,165 @@
#include "Copter.h"
// start takeoff to given altitude (for use by scripting)
bool Copter::start_takeoff(float alt)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!flightmode->in_guided_mode()) {
return false;
}
if (mode_guided.do_user_takeoff_start(alt * 100.0f)) {
copter.set_auto_armed(true);
return true;
}
return false;
}
// set target location (for use by scripting)
bool Copter::set_target_location(const Location& target_loc)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!flightmode->in_guided_mode()) {
return false;
}
return mode_guided.set_destination(target_loc);
}
// set target position (for use by scripting)
bool Copter::set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!flightmode->in_guided_mode()) {
return false;
}
const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f);
return mode_guided.set_destination(pos_neu_cm, use_yaw, yaw_deg * 100.0, use_yaw_rate, yaw_rate_degs * 100.0, yaw_relative);
}
bool Copter::set_target_velocity_NED(const Vector3f& vel_ned)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!flightmode->in_guided_mode()) {
return false;
}
// convert vector to neu in cm
const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f);
mode_guided.set_velocity(vel_neu_cms);
return true;
}
void Copter::zr_app_10hz()
{
if(zr_serial_api.data_trans_init){
zr_serial_api.update();
}else{
static uint32_t last_5s;
uint32_t tnow = AP_HAL::millis();
if (tnow - last_5s > 5000) {
last_5s = tnow;
zr_serial_api.init(serial_manager);
gcs().send_text(MAV_SEVERITY_INFO, "data_trans init");
}
}
if(zr_serial_api.data_trans_init){
// 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)
uint16_t now_volt = uint16_t(battery.voltage() * 100);
zr_serial_api.get_vehicle_status((uint8_t)control_mode,(uint8_t)flightmode->is_landing(),home_distance(),now_volt,battery.capacity_remaining_pct());
}
}
void Copter::zr_app_50hz(){
if(zr_serial_api.data_trans_init){
Vector3l pos_neu_cm;
// if (current_loc.get_vector_from_origin_NEU(pos_neu_cm)) {
Location my_loc;
if (ahrs.get_position(my_loc)) {
pos_neu_cm.x = my_loc.lat;
pos_neu_cm.y = my_loc.lng;
pos_neu_cm.z = my_loc.alt;
// zr_serial_api.get_vehicle_NEU_pos(pos_neu_cm);
// gcs().send_text(MAV_SEVERITY_INFO,"get loc:%.2f,%.2f,%.2f",pos_neu_cm.x,pos_neu_cm.y,pos_neu_cm.z);
}
// }
Vector3f euler,euler_deg;
if(ahrs.get_secondary_attitude(euler)){
euler_deg.x = degrees(euler.x);
euler_deg.y = degrees(euler.y);
euler_deg.z = wrap_360_cd(degrees(euler.z));
// zr_serial_api.get_vehicle_euler_angles(euler_deg);
// gcs().send_text(MAV_SEVERITY_INFO,"get euler:%.2f,%.2f,%.2f",euler_deg.x,euler_deg.y,euler_deg.z);
}
Vector3f vel_ned_m;
Vector3l vel_ned_cm;
if (ahrs.get_velocity_NED(vel_ned_m)) {
vel_ned_cm.x = vel_ned_m.x * 100;
vel_ned_cm.y = vel_ned_m.y * 100;
vel_ned_cm.z = vel_ned_m.z * 100;
}
// zr_serial_api.get_vehicle_flight_data(pos_neu_cm,vel_ned_cm,euler_deg,(uint8_t)flightmode->mode_number(),(uint8_t)flightmode->is_landing());
zr_serial_api.get_vehicle_flight_data(pos_neu_cm,vel_ned_cm,euler_deg);
///////////
AC_ZR_Serial_API::ZR_Msg_Type msg_type;
Vector3l data;
float yaw_deg;
bool new_data = zr_serial_api.get_control_data((uint8_t)control_mode ,msg_type,data,yaw_deg);
if(new_data){
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:
{
if(!motors->armed()){
arming.arm(AP_Arming::Method::SCRIPTING);
}
if(control_mode != Mode::Number::GUIDED){
set_mode(Mode::Number::GUIDED, ModeReason::SCRIPTING);
}
float tk_alt = (data.z)/100.0;
start_takeoff(tk_alt);
gcs().send_text(MAV_SEVERITY_INFO,"do takeoff,alt %.2f",tk_alt);
}
break;
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_POS:
if(motors->armed()){
Location target_loc;
target_loc.lat = data.x + 100;
target_loc.lng = data.y;
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 %ld,%ld,%ld",target_loc.lat,target_loc.lng,target_loc.alt);
}
break;
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL:
if(motors->armed()){
Vector3f target_vel;
target_vel.x = data.x / 100.0;
target_vel.y = data.y / 100.0;
target_vel.z = data.z / 100.0;
// target_loc.alt = (data.z + 500)/100.0;
set_target_velocity_NED(target_vel);
gcs().send_text(MAV_SEVERITY_INFO,"set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
}
break;
default:
break;
}
}
}
}

345
Tools/environment_install/ubuntu.sh
Unescape View File

@ -0,0 +1,345 @@ @@ -0,0 +1,345 @@
#!/bin/bash
echo "---------- $0 start ----------"
set -e
set -x
if [ $EUID == 0 ]; then
echo "Please do not run this script as root; don't sudo it!"
exit 1
fi
OPT="/opt"
# Ardupilot Tools
ARDUPILOT_TOOLS="Tools/autotest"
ASSUME_YES=false
QUIET=false
sep="##############################################"
OPTIND=1 # Reset in case getopts has been used previously in the shell.
while getopts "yq" opt; do
case "$opt" in
\?)
exit 1
;;
y) ASSUME_YES=true
;;
q) QUIET=true
;;
esac
done
APT_GET="sudo apt-get"
if $ASSUME_YES; then
APT_GET="$APT_GET --assume-yes"
fi
if $QUIET; then
APT_GET="$APT_GET -qq"
fi
# update apt package list
$APT_GET update
function package_is_installed() {
dpkg-query -W -f='${Status}' "$1" 2>/dev/null | grep -c "ok installed"
}
function heading() {
echo "$sep"
echo $*
echo "$sep"
}
# Install lsb-release as it is needed to check Ubuntu version
if ! package_is_installed "lsb-release"; then
heading "Installing lsb-release"
$APT_GET install lsb-release
echo "Done!"
fi
# Checking Ubuntu release to adapt software version to install
RELEASE_CODENAME=$(lsb_release -c -s)
PYTHON_V="python" # starting from ubuntu 20.04, python isn't symlink to default python interpreter
PIP=pip2
if [ ${RELEASE_CODENAME} == 'xenial' ]; then
SITLFML_VERSION="2.3v5"
SITLCFML_VERSION="2.3"
elif [ ${RELEASE_CODENAME} == 'disco' ]; then
SITLFML_VERSION="2.5"
SITLCFML_VERSION="2.5"
elif [ ${RELEASE_CODENAME} == 'eoan' ]; then
SITLFML_VERSION="2.5"
SITLCFML_VERSION="2.5"
elif [ ${RELEASE_CODENAME} == 'focal' ] || [ ${RELEASE_CODENAME} == 'ulyssa' ]; then
SITLFML_VERSION="2.5"
SITLCFML_VERSION="2.5"
PYTHON_V="python3"
PIP=pip3
elif [ ${RELEASE_CODENAME} == 'groovy' ] ||
[ ${RELEASE_CODENAME} == 'hirsute' ] ||
[ ${RELEASE_CODENAME} == 'bullseye' ] ||
[ ${RELEASE_CODENAME} == 'impish' ]; then
SITLFML_VERSION="2.5"
SITLCFML_VERSION="2.5"
PYTHON_V="python3"
PIP=pip3
elif [ ${RELEASE_CODENAME} == 'trusty' ]; then
SITLFML_VERSION="2"
SITLCFML_VERSION="2"
else
# We assume APT based system, so let's try with apt-cache first.
SITLCFML_VERSION=$(apt-cache search -n '^libcsfml-audio' | cut -d" " -f1 | head -1 | grep -Eo '[+-]?[0-9]+([.][0-9]+)?')
SITLFML_VERSION=$(apt-cache search -n '^libsfml-audio' | cut -d" " -f1 | head -1 | grep -Eo '[+-]?[0-9]+([.][0-9]+)?')
# If we cannot retrieve the number with apt-cache, try a last time with dpkg-query
re='^[+-]?[0-9]+([.][0-9]+)?$'
if ! [[ $SITLCFML_VERSION =~ $re ]] || ! [[ $SITLFML_VERSION =~ $re ]] ; then
# Extract the floating point number that is the version of the libcsfml package.
SITLCFML_VERSION=$(dpkg-query --search libcsfml-audio | cut -d":" -f1 | grep libcsfml-audio | head -1 | grep -Eo '[+-]?[0-9]+([.][0-9]+)?')
# And same for libsfml-audio.
SITLFML_VERSION=$(dpkg-query --search libsfml-audio | cut -d":" -f1 | grep libsfml-audio | head -1 | grep -Eo '[+-]?[0-9]+([.][0-9]+)?')
fi
fi
# Check whether the specific ARM pkg-config package is available or whether we should emulate the effect of installing it.
# Check if we need to manually install libtool-bin
ARM_PKG_CONFIG_NOT_PRESENT=0
if [ -z "$(apt-cache search -n '^pkg-config-arm-linux-gnueabihf')" ]; then
ARM_PKG_CONFIG_NOT_PRESENT=$(dpkg-query --search pkg-config-arm-linux-gnueabihf |& grep -c "dpkg-query:")
fi
if [ "$ARM_PKG_CONFIG_NOT_PRESENT" -eq 1 ]; then
INSTALL_PKG_CONFIG=""
# No need to install Ubuntu's pkg-config-arm-linux-gnueabihf, instead install the base pkg-config.
$APT_GET install pkg-config
if [ -f /usr/share/pkg-config-crosswrapper ]; then
# We are on non-Ubuntu so simulate effect of installing pkg-config-arm-linux-gnueabihf.
sudo ln -s /usr/share/pkg-config-crosswrapper /usr/bin/arm-linux-gnueabihf-pkg-config
else
echo "Warning: unable to link to pkg-config-crosswrapper"
fi
else
# Package is available so install it later.
INSTALL_PKG_CONFIG="pkg-config-arm-linux-gnueabihf"
fi
# Lists of packages to install
BASE_PKGS="build-essential ccache g++ gawk git make wget"
if [ ${RELEASE_CODENAME} == 'xenial' ] || [ ${RELEASE_CODENAME} == 'disco' ] || [ ${RELEASE_CODENAME} == 'eoan' ]; then
# use fixed version for package that drop python2 support
PYTHON_PKGS="future lxml pymavlink MAVProxy pexpect flake8==3.7.9 geocoder empy configparser==5.0.0 click==7.1.2 decorator==4.4.2"
else
PYTHON_PKGS="future lxml pymavlink MAVProxy pexpect flake8 geocoder empy"
fi
# add some Python packages required for commonly-used MAVProxy modules and hex file generation:
if [[ $SKIP_AP_EXT_ENV -ne 1 ]]; then
PYTHON_PKGS="$PYTHON_PKGS pygame intelhex"
fi
ARM_LINUX_PKGS="g++-arm-linux-gnueabihf $INSTALL_PKG_CONFIG"
# python-wxgtk packages are added to SITL_PKGS below
SITL_PKGS="libtool libxml2-dev libxslt1-dev ${PYTHON_V}-dev ${PYTHON_V}-pip ${PYTHON_V}-setuptools ${PYTHON_V}-numpy ${PYTHON_V}-pyparsing ${PYTHON_V}-psutil"
# add some packages required for commonly-used MAVProxy modules:
if [[ $SKIP_AP_GRAPHIC_ENV -ne 1 ]]; then
SITL_PKGS="$SITL_PKGS xterm ${PYTHON_V}-matplotlib ${PYTHON_V}-serial ${PYTHON_V}-scipy ${PYTHON_V}-opencv libcsfml-dev libcsfml-audio${SITLCFML_VERSION} libcsfml-dev libcsfml-graphics${SITLCFML_VERSION} libcsfml-network${SITLCFML_VERSION} libcsfml-system${SITLCFML_VERSION} libcsfml-window${SITLCFML_VERSION} libsfml-audio${SITLFML_VERSION} libsfml-dev libsfml-graphics${SITLFML_VERSION} libsfml-network${SITLFML_VERSION} libsfml-system${SITLFML_VERSION} libsfml-window${SITLFML_VERSION} ${PYTHON_V}-yaml"
fi
if [[ $SKIP_AP_COV_ENV -ne 1 ]]; then
# Coverage utilities
COVERAGE_PKGS="lcov gcovr"
fi
# ArduPilot official Toolchain for STM32 boards
function install_arm_none_eabi_toolchain() {
# GNU Tools for ARM Embedded Processors
# (see https://launchpad.net/gcc-arm-embedded/)
ARM_ROOT="gcc-arm-none-eabi-10-2020-q4-major"
ARM_TARBALL="$ARM_ROOT-x86_64-linux.tar.bz2"
ARM_TARBALL_URL="https://firmware.ardupilot.org/Tools/STM32-tools/$ARM_TARBALL"
if [ ! -d $OPT/$ARM_ROOT ]; then
(
cd $OPT;
heading "Installing toolchain for STM32 Boards"
echo "Downloading from ArduPilot server"
sudo wget $ARM_TARBALL_URL
echo "Installing..."
sudo tar xjf ${ARM_TARBALL}
echo "... Cleaning"
sudo rm ${ARM_TARBALL};
)
fi
echo "Registering STM32 Toolchain for ccache"
sudo ln -s -f $CCACHE_PATH /usr/lib/ccache/arm-none-eabi-g++
sudo ln -s -f $CCACHE_PATH /usr/lib/ccache/arm-none-eabi-gcc
echo "Done!"
}
function maybe_prompt_user() {
if $ASSUME_YES; then
return 0
else
read -p "$1"
if [[ $REPLY =~ ^[Yy]$ ]]; then
return 0
else
return 1
fi
fi
}
heading "Add user to dialout group to allow managing serial ports"
sudo usermod -a -G dialout $USER
echo "Done!"
# Add back python symlink to python interpreter on Ubuntu >= 20.04
if [ ${RELEASE_CODENAME} == 'focal' ] ||
[ ${RELEASE_CODENAME} == 'ulyssa' ];
then
BASE_PKGS+=" python-is-python3"
SITL_PKGS+=" libpython3-stdlib" # for argparse
elif [ ${RELEASE_CODENAME} == 'groovy' ] ||
[ ${RELEASE_CODENAME} == 'hirsute' ] ||
[ ${RELEASE_CODENAME} == 'bullseye' ] ||
[ ${RELEASE_CODENAME} == 'impish' ]; then
BASE_PKGS+=" python-is-python3"
SITL_PKGS+=" libpython3-stdlib" # for argparse
else
SITL_PKGS+=" python-argparse"
fi
# Check for graphical package for MAVProxy
if [[ $SKIP_AP_GRAPHIC_ENV -ne 1 ]]; then
if [ ${RELEASE_CODENAME} == 'bullseye' ]; then
SITL_PKGS+=" libjpeg62-turbo-dev"
elif [ ${RELEASE_CODENAME} == 'groovy' ] ||
[ ${RELEASE_CODENAME} == 'hirsute' ] ||
[ ${RELEASE_CODENAME} == 'focal' ] ||
[ ${RELEASE_CODENAME} == 'ulyssa' ]; then
SITL_PKGS+=" libjpeg8-dev"
elif apt-cache search python-wxgtk3.0 | grep wx; then
SITL_PKGS+=" python-wxgtk3.0"
elif apt-cache search python3-wxgtk4.0 | grep wx; then
# see below
:
else
# we only support back to trusty:
SITL_PKGS+=" python-wxgtk2.8"
SITL_PKGS+=" fonts-freefont-ttf libfreetype6-dev libjpeg8-dev libpng12-0 libportmidi-dev libsdl-image1.2-dev libsdl-mixer1.2-dev libsdl-ttf2.0-dev libsdl1.2-dev" # for pygame
fi
if [ ${RELEASE_CODENAME} == 'bullseye' ] ||
[ ${RELEASE_CODENAME} == 'groovy' ] ||
[ ${RELEASE_CODENAME} == 'hirsute' ] ||
[ ${RELEASE_CODENAME} == 'focal' ] ||
[ ${RELEASE_CODENAME} == 'ulyssa' ] ||
[ ${RELEASE_CODENAME} == 'impish' ]; then
SITL_PKGS+=" python3-wxgtk4.0"
SITL_PKGS+=" fonts-freefont-ttf libfreetype6-dev libpng16-16 libportmidi-dev libsdl-image1.2-dev libsdl-mixer1.2-dev libsdl-ttf2.0-dev libsdl1.2-dev" # for pygame
fi
fi
# Check if we need to manually install realpath
RP=$(apt-cache search -n '^realpath$')
if [ -n "$RP" ]; then
BASE_PKGS+=" realpath"
fi
# Check if we need to manually install libtool-bin
LBTBIN=$(apt-cache search -n '^libtool-bin')
if [ -n "$LBTBIN" ]; then
SITL_PKGS+=" libtool-bin"
fi
# Install all packages
$APT_GET install $BASE_PKGS $SITL_PKGS $PX4_PKGS $ARM_LINUX_PKGS $COVERAGE_PKGS
# Update Pip and Setuptools on old distro
if [ ${RELEASE_CODENAME} == 'xenial' ] || [ ${RELEASE_CODENAME} == 'disco' ] || [ ${RELEASE_CODENAME} == 'eoan' ]; then
# use fixed version for package that drop python2 support
$PIP install --user -U pip==20.3 setuptools==44.0.0
fi
$PIP install --user -U $PYTHON_PKGS
if [[ -z "${DO_AP_STM_ENV}" ]] && maybe_prompt_user "Install ArduPilot STM32 toolchain [N/y]?" ; then
DO_AP_STM_ENV=1
fi
heading "Removing modemmanager package that could conflict with firmware uploading"
if package_is_installed "modemmanager"; then
$APT_GET remove modemmanager
fi
echo "Done!"
CCACHE_PATH=$(which ccache)
if [[ $DO_AP_STM_ENV -eq 1 ]]; then
install_arm_none_eabi_toolchain
fi
heading "Check if we are inside docker environment..."
IS_DOCKER=false
if [[ -f /.dockerenv ]] || grep -Eq '(lxc|docker)' /proc/1/cgroup ; then
IS_DOCKER=true
fi
echo "Done!"
SHELL_LOGIN=".profile"
if $IS_DOCKER; then
echo "Inside docker, we add the tools path into .bashrc directly"
SHELL_LOGIN=".bashrc"
fi
heading "Adding ArduPilot Tools to environment"
SCRIPT_DIR=$(dirname $(realpath ${BASH_SOURCE[0]}))
ARDUPILOT_ROOT=$(realpath "$SCRIPT_DIR/../../")
if [[ $DO_AP_STM_ENV -eq 1 ]]; then
exportline="export PATH=$OPT/$ARM_ROOT/bin:\$PATH";
grep -Fxq "$exportline" ~/$SHELL_LOGIN 2>/dev/null || {
if maybe_prompt_user "Add $OPT/$ARM_ROOT/bin to your PATH [N/y]?" ; then
echo $exportline >> ~/$SHELL_LOGIN
eval $exportline
else
echo "Skipping adding $OPT/$ARM_ROOT/bin to PATH."
fi
}
fi
exportline2="export PATH=$ARDUPILOT_ROOT/$ARDUPILOT_TOOLS:\$PATH";
grep -Fxq "$exportline2" ~/$SHELL_LOGIN 2>/dev/null || {
if maybe_prompt_user "Add $ARDUPILOT_ROOT/$ARDUPILOT_TOOLS to your PATH [N/y]?" ; then
echo $exportline2 >> ~/$SHELL_LOGIN
eval $exportline2
else
echo "Skipping adding $ARDUPILOT_ROOT/$ARDUPILOT_TOOLS to PATH."
fi
}
if [[ $SKIP_AP_COMPLETION_ENV -ne 1 ]]; then
exportline3="source $ARDUPILOT_ROOT/Tools/completion/completion.bash";
grep -Fxq "$exportline3" ~/$SHELL_LOGIN 2>/dev/null || {
if maybe_prompt_user "Add ArduPilot Bash Completion to your bash shell [N/y]?" ; then
echo $exportline3 >> ~/.bashrc
eval $exportline3
else
echo "Skipping adding ArduPilot Bash Completion."
fi
}
fi
exportline4="export PATH=/usr/lib/ccache:\$PATH";
grep -Fxq "$exportline4" ~/$SHELL_LOGIN 2>/dev/null || {
if maybe_prompt_user "Append CCache to your PATH [N/y]?" ; then
echo $exportline4 >> ~/$SHELL_LOGIN
eval $exportline4
else
echo "Skipping appending CCache to PATH."
fi
}
echo "Done!"
if [[ $SKIP_AP_GIT_CHECK -ne 1 ]]; then
if [ -d ".git" ]; then
heading "Update git submodules"
cd $ARDUPILOT_ROOT
git submodule update --init --recursive
echo "Done!"
fi
fi
echo "---------- $0 end ----------"

253
libraries/AC_ZR_APP/AC_ZR_App.cpp
Unescape View File

@ -0,0 +1,253 @@ @@ -0,0 +1,253 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AC_ZR_App.h"
AC_ZR_App *AC_ZR_App::_singleton;
// table of user settable parameters
const AP_Param::GroupInfo AC_ZR_App::var_info[] = {
// @Param: _TYPE
// @DisplayName: user type
// @Description: parameters test
// @Values: 0:None,1:other
// @RebootRequired: True
// @User: zrzk
AP_GROUPINFO("_TYPE", 0, AC_ZR_App, _type, 1),
AP_GROUPINFO("_SYS_TYPE", 1, AC_ZR_App, sysid_type, 0 ),
AP_GROUPINFO("_SYS_ID", 2, AC_ZR_App, sysid_board_id, 98765432U),
AP_GROUPINFO("_SYS_DL1", 3, AC_ZR_App, sysid_dl1, 0x8175),
AP_GROUPINFO("_SYS_DL2", 4, AC_ZR_App, sysid_dl2, 0x6fda),
AP_GROUPINFO("_SYS_DL3", 5, AC_ZR_App, sysid_dl3, 0xf38f),
AP_GROUPINFO("_SYS_DL4", 6, AC_ZR_App, sysid_dl4, 0xbf48),
// @Param: _SL_*
// @DisplayName: ZR_SL_*
// @Description: 降落减速设置，3个梯度
// @Values: 值单位cm和cm/s
// @RebootRequired: True
// @User: zrzk
AP_GROUPINFO("_SL_ALT_HI", 7, AC_ZR_App, land_slow_alt_high, 3000),
AP_GROUPINFO("_SL_ALT_MI", 8, AC_ZR_App, land_slow_alt_mid, 1500),
AP_GROUPINFO("_SL_ALT_LO", 9, AC_ZR_App, land_slow_alt_low, 150),
AP_GROUPINFO("_SL_SPD_HI", 10, AC_ZR_App, land_slow_speed_dn_high, 150),
AP_GROUPINFO("_SL_SPD_MI", 11, AC_ZR_App, land_slow_speed_dn_mid, 50),
AP_GROUPINFO("_SL_SPD_LO", 12, AC_ZR_App, land_slow_speed_dn_low, 30),
AP_GROUPEND
};
// constructor
AC_ZR_App::AC_ZR_App()
{
AP_Param::setup_object_defaults(this, var_info);
if (_singleton != nullptr) {
AP_HAL::panic("AC_ZR_App must be singleton");
}
_singleton = this;
}
/**
* @brief 从参数中解析注册到期时间
*
* @param deadline 返回到期时间
*/
void AC_ZR_App::get_deadline_params(uint32_t &deadline)
{
uint8_t tempbuf[8];
tempbuf[1] = sysid_dl1 & 0xff;
tempbuf[0] = (sysid_dl1 & 0xff00) >> 8;
// gcs().send_text(MAV_SEVERITY_INFO, "tempbuf0: %d ,tempbuf1:%d",tempbuf[0] ,tempbuf[1]);
tempbuf[3] = sysid_dl2 & 0xff;
tempbuf[2] = (sysid_dl2 & 0xff00) >> 8;
//gcs().send_text(MAV_SEVERITY_INFO, "tempbuf2: %d ,tempbuf3:%d",tempbuf[2] ,tempbuf[3]);
tempbuf[5] = sysid_dl3 & 0xff;
tempbuf[4] = (sysid_dl3 & 0xff00) >> 8;
//gcs().send_text(MAV_SEVERITY_INFO, "tempbuf4: %d ,tempbuf5:%d",tempbuf[4] ,tempbuf[5]);
tempbuf[7] = sysid_dl4 & 0xff;
tempbuf[6] = (sysid_dl4 & 0xff00) >> 8;
//gcs().send_text(MAV_SEVERITY_INFO, "tempbuf6: %d ,tempbuf7:%d",tempbuf[6] ,tempbuf[7]);
uint8_t out_data_temp[8];
ZR_Des des;
uint8_t deskey_zr[8]= {90, 114, 90, 107, 85, 97, 86, 103}; //{ "ZrZkUaVg" };
uint8_t deskey_zhd[8]= {90, 114, 90, 104, 68, 85, 97, 86}; //{ "ZrZhDUaV" };
int8_t type = (int)ZR_FRAME_TYPE;
if(type == 3 ){ // 中海达加密模式
des.des(&tempbuf[0], &deskey_zhd[0], 1, &out_data_temp[0]);
}else{
des.des(&tempbuf[0], &deskey_zr[0], 1, &out_data_temp[0]);
}
// des.des(&tempbuf[0], &des.deskey[0], 1, &out_data_temp[0]);
//gcs().send_text(MAV_SEVERITY_INFO, "out_data_temp6: %d ,out_data_temp7:%d",out_data_temp[6] ,out_data_temp[7]);
if (out_data_temp[6] == 122 && out_data_temp[7] == 114) //'zr'
{
deadline = 20000000 + ((out_data_temp[0] - 0x30) * 10 + (out_data_temp[1] - 0x30)) * 10000 +
((out_data_temp[2] - 0x30) * 10 + (out_data_temp[3] - 0x30)) * 100 +
((out_data_temp[4] - 0x30) * 10 + (out_data_temp[5] - 0x30));
}
else
{
deadline = 20200101;
gcs().send_text(MAV_SEVERITY_INFO, "D_Error: The registration code is wrong!");
}
}
/**
* @brief 检查是否注册时间是否正常
*
* @param date 返回到期时间
* @return true 没到期
* @return false 到期
*/
bool AC_ZR_App::check_deadline(uint32_t &date)
{
uint32_t deadline =0;
const AP_GPS &gps = AP::gps();
if(gps.status() < 2){ // 室内测试没卫星的情况，允许解锁测试
return true;
}
get_deadline_params(deadline);
date = deadline;
uint32_t gps_date =0;
if((gps.time_week() ==0)||(gps.time_week_ms() ==0)){
return false;
}
uint64_t timestamp = gps.time_epoch_convert(gps.time_week(),gps.time_week_ms())/1000;
//tick -timestamp - uint:s
time_t tick = (time_t)timestamp;
tm gpstime = *localtime(&tick);
uint16_t year = gpstime.tm_year+1900;
uint8_t month = gpstime.tm_mon+1;
uint8_t day = gpstime.tm_mday;
gps_date = year*10000+month*100+day;
if (deadline>= gps_date)
{
return true;
}
else
{
return false;
}
}
uint32_t AC_ZR_App::get_zr_sysid()
{
return (uint32_t)sysid_board_id;
}
/**
* @brief 降落根据高度梯度减速
*
* @param alt_cm 当前飞机高度
* @param sys_speed_dn 系统设置的降落速度
* @return uint16_t 减速后速度
*/
uint16_t AC_ZR_App::get_land_deceleration(int32_t alt_cm,int16_t sys_speed_dn)
{
int16_t speed_dn_now = sys_speed_dn; // 没到减速阶段则使用系统降落速度
if(alt_cm < land_slow_alt_low){ // 最低处减速
speed_dn_now = land_slow_speed_dn_low;
}
else if(alt_cm < land_slow_alt_mid){ // 中高处减速
speed_dn_now = land_slow_speed_dn_mid;
}
else if(alt_cm < land_slow_alt_high){ // 高处减速
speed_dn_now = land_slow_speed_dn_high;
}
return abs(speed_dn_now);
}
uint8_t AC_ZR_App::BinarySearch2f(float a[], float value, int low, int high)
{
static uint8_t cacl_volt_pst = 0;
if (low > high)
return cacl_volt_pst;
cacl_volt_pst = (low + high) / 2;
if (fabs(a[cacl_volt_pst] - value)<0.01)
//if (abs(a[mid] - value) < 30)
return cacl_volt_pst;
else if (a[cacl_volt_pst] > value)
return BinarySearch2f(a, value, low, cacl_volt_pst - 1);
else
return BinarySearch2f(a, value, cacl_volt_pst + 1, high);
}
uint8_t AC_ZR_App::get_battery_capacity(uint8_t type,float volt)
{
uint8_t bat_cnt = 100;
// float cycle_pst = (1 - bat_cycled * 0.0002);
switch (type)
{
case 0:
/* code */
break;
case 1:
static float batt_mah_teb[] =
{
16.80, 17.40, 18.00, 18.51, 18.80, 19.00, 19.19, 19.34, 19.53, 19.73,
19.88, 20.04, 20.16, 20.29, 20.47, 20.56, 20.63, 20.69, 20.75, 20.80,
20.85, 20.90, 20.96, 21.02, 21.07, 21.13, 21.18, 21.24, 21.29, 21.34,
21.40, 21.45, 21.49, 21.54, 21.59, 21.64, 21.69, 21.72, 21.76, 21.79,
21.84, 21.88, 21.92, 21.96, 22.01, 22.06, 22.11, 22.16, 22.21, 22.26,
22.31, 22.37, 22.43, 22.48, 22.54, 22.59, 22.65, 22.70, 22.76, 22.81,
22.86, 22.91, 22.95, 23.01, 23.07, 23.13, 23.19, 23.24, 23.30, 23.36,
23.41, 23.46, 23.51, 23.58, 23.65, 23.71, 23.79, 23.88, 23.94, 24.00,
24.07, 24.14, 24.21, 24.26, 24.32, 24.36, 24.40, 24.43, 24.45, 24.48,
24.50, 24.52, 24.54, 24.57, 24.60, 24.64, 24.69, 24.76, 24.84, 24.90,
25.20
};
bat_cnt = BinarySearch2f(batt_mah_teb,volt,0,100);
// gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
break;
case 2:
static float batt_mah_teb_7s[] =
{
21.60, 21.80, 22.04, 22.28, 22.52, 22.74, 22.95, 23.19, 23.32, 23.52,
23.65, 23.81, 23.94, 24.02, 24.10, 24.18, 24.22, 24.25, 24.30, 24.39,
24.46, 24.52, 24.60, 24.67, 24.74, 24.80, 24.86, 24.93, 24.97, 25.03,
25.08, 25.13, 25.18, 25.23, 25.27, 25.32, 25.36, 25.41, 25.45, 25.50,
25.54, 25.58, 25.63, 25.69, 25.74, 25.79, 25.85, 25.91, 25.98, 26.05,
26.10, 26.16, 26.21, 26.28, 26.35, 26.40, 26.47, 26.54, 26.61, 26.67,
26.74, 26.79, 26.85, 26.93, 27.00, 27.07, 27.13, 27.19, 27.26, 27.32,
27.38, 27.46, 27.52, 27.59, 27.66, 27.75, 27.80, 27.87, 27.94, 28.03,
28.10, 28.16, 28.21, 28.28, 28.33, 28.38, 28.43, 28.46, 28.50, 28.53,
28.56, 28.59, 28.61, 28.64, 28.67, 28.71, 28.77, 28.86, 28.97, 29.13,
29.30
};
bat_cnt = BinarySearch2f(batt_mah_teb_7s,volt,0,100);
// gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
break;
default:
break;
}
return bat_cnt;
}

71
libraries/AC_ZR_APP/AC_ZR_App.h
Unescape View File

@ -0,0 +1,71 @@ @@ -0,0 +1,71 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <GCS_MAVLink/GCS.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_GPS/AP_GPS.h>
#include <sys/time.h>
#include <time.h>
#include "ZR_Des.h"
#ifndef ZR_FRAME_TYPE
#define ZR_FRAME_TYPE 0
#endif
class AC_ZR_App
{
public:
AC_ZR_App();
/* Do not allow copies */
AC_ZR_App(const AC_ZR_App &other) = delete;
AC_ZR_App &operator=(const AC_ZR_App&) = delete;
static AC_ZR_App *get_singleton() {
return _singleton;
}
void get_deadline_params(uint32_t &deadline);
bool check_deadline(uint32_t &date);
uint32_t get_zr_sysid();
uint16_t get_land_deceleration(int32_t alt_cm,int16_t sys_speed_dn);
uint8_t get_battery_capacity(uint8_t type,float volt);
static const struct AP_Param::GroupInfo var_info[];
protected:
AP_Int8 _type;
AP_Int8 sysid_type; // modify by @Brown
AP_Int32 sysid_board_id;
AP_Int32 sysid_dl1;
AP_Int32 sysid_dl2;
AP_Int32 sysid_dl3;
AP_Int32 sysid_dl4;
AP_Int16 land_slow_alt_high;
AP_Int16 land_slow_alt_mid;
AP_Int16 land_slow_alt_low;
AP_Int16 land_slow_speed_dn_high;
AP_Int16 land_slow_speed_dn_mid;
AP_Int16 land_slow_speed_dn_low;
private:
uint8_t BinarySearch2f(float a[], float value, int low, int high);
static AC_ZR_App *_singleton;
};

340
libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp
Unescape View File

@ -0,0 +1,340 @@ @@ -0,0 +1,340 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AC_ZR_Serial_API.h"
#include <AP_RTC/AP_RTC.h>
#include <ctype.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <AP_Math/crc.h>
#define ZR_API_DEBUG 1
#if ZR_API_DEBUG
# define Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_INFO, "{%d}" fmt , __LINE__, ## args)
// # define Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_INFO, "NOTICE:" fmt , ## args)
#else
# define Debug(fmt, args ...)
#endif
AC_ZR_Serial_API *AC_ZR_Serial_API::_singleton;
// constructor
AC_ZR_Serial_API::AC_ZR_Serial_API()
{
// AP_Param::setup_object_defaults(this, var_info);
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;
}
/// Startup initialisation.
void AC_ZR_Serial_API::init(const AP_SerialManager& serial_manager)
{
// search for serial ports with gps protocol
// 接收主设备的串口数据
if(_zr_api_port == nullptr )
_zr_api_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_ZR_Serial_API, 0);
bool init_host;
if(_zr_api_port != nullptr ){
init_host = true;
}else{
init_host = false;
}
data_trans_init = init_host;
}
/**
* @brief 数据写入串口
*
* @param _port 选择要写入的串口，_port_to_host，_zr_api_port
* @param data 写入的数据
* @param len 数据长度
*/
void AC_ZR_Serial_API::write_data_to_port(AP_HAL::UARTDriver *_port, const uint8_t *data, uint8_t len)
{
// not all backends have valid ports
if (_port != nullptr) {
if (_port->txspace() > len) {
_port->write(data, len);
} else {
// Debug("Data_Trans Not enough TXSPACE,%d < %d", _port->txspace(),len);
}
}
}
/**
* @brief 读取串口数据
*
* @param _port 选择要读取的串口,_zr_api_port
* @param data serial_data_from_device[CONTROL_DATA_LEN]
* @param len mav_data_len,device_data_len
*/
void AC_ZR_Serial_API::read_data_from_port(AP_HAL::UARTDriver *_port,uint8_t *data, uint8_t *len)
{
// exit immediately if no uart for this instance
if (_port == nullptr) {
return;
}
WITH_SEMAPHORE(sem);
uint32_t nbytes = _port->available();
// if(nbytes > 0){
// if (*len < bufsize) { // 判断数据是否存满
// serial_last_data_time = AP_HAL::millis();
// ssize_t nread = _port->read(pktbuf, MIN(nbytes, unsigned(CONTROL_DATA_LEN)));
// if (nread > 0) {
// *len = nread;
// memcpy(data,pktbuf,*len); // 收到一帧数据
// }
// Debug("read:%ld,len:%d",nbytes,*len);
// }else{
// Debug("out size:%d",*len);
// *len = 0;
// }
// }
if(nbytes > 0){
uint8_t max_len = unsigned(CONTROL_DATA_LEN);
if (*len < max_len) { // 判断数据是否存满
uint16_t count = MIN(nbytes, unsigned(max_len-*len));
// Debug("t:%ld,read:%ld,len:%d,cnt:%d",AP_HAL::millis(),nbytes,*len,count);
while (count--) {
const int16_t temp = _port->read();
if (temp == -1) {
break;
}
data[*len] = (uint8_t)temp;
*len+=1;
}
serial_last_data_time = AP_HAL::millis();
}else{
// Debug("t:%ld,out size:%d",AP_HAL::millis(),*len);
*len = 0;
}
}
}
void AC_ZR_Serial_API::get_vehicle_NEU_pos(Vector3f vec_neu)
{
static char buf[50];
snprintf(buf, sizeof(buf), "pos:%.2f,%.2f,%.2f\n\r",vec_neu.x,vec_neu.y,vec_neu.z);
mav_data_len = 50;
memset(mav_data_from_host,0,FLIGHT_DATA_LEN);
memcpy(mav_data_from_host,buf,50);
get_mav_data = true;
write_data_to_port(_zr_api_port,mav_data_from_host, mav_data_len);
}
void AC_ZR_Serial_API::get_vehicle_euler_angles(Vector3f euler)
{
static char buf[50];
snprintf(buf, sizeof(buf), "rpy:%.2f,%.2f,%.2f\n\r",euler.x,euler.y,euler.z);
mav_data_len = 50;
// memset(mav_data_from_host,0,FLIGHT_DATA_LEN);
// memcpy(mav_data_from_host,buf,50);
get_mav_data = true;
write_data_to_port(_zr_api_port,(const uint8_t *)buf, mav_data_len);
}
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)
{
return; // for test ,don't send
flight_mode = mode;
static uint8_t delay_cnt;
delay_cnt += 1;
if(delay_cnt < 50){
return;
}else{
delay_cnt = 0;
}
uint8_t crc_sum = 0;
vehicle_status.msg.msg_head = MSG_HEAD;
vehicle_status.msg.msg_type = ZR_Msg_Type::MSG_VEHICLE_STATUS;
vehicle_status.msg.length = VEHICLE_STATUS_LEN - 4;
vehicle_status.msg.fly_mode = mode;
vehicle_status.msg.fly_status = in_air;
vehicle_status.msg.home_distance = home_distance;
vehicle_status.msg.volt_mv = volt_mv;
vehicle_status.msg.bat_remaining = bat_remaining;
crc_sum = crc_crc8(vehicle_status.data,VEHICLE_STATUS_LEN-1);
vehicle_status.msg.crc = crc_sum;
write_data_to_port(_zr_api_port,vehicle_status.data, VEHICLE_STATUS_LEN);
}
void AC_ZR_Serial_API::get_vehicle_flight_data(Vector3l pos,Vector3l vel,Vector3f euler)
{
static uint8_t delay_cnt;
delay_cnt += 1;
if(delay_cnt < 10){
return;
}else{
delay_cnt = 0;
}
uint8_t crc_sum = 0;
flight_data_parser.msg.msg_head = MSG_HEAD;
flight_data_parser.msg.msg_type = ZR_Msg_Type::MSG_FLIGHT_DATA;
flight_data_parser.msg.length = FLIGHT_DATA_LEN - 4;
flight_data_parser.msg.lat = pos.x;
flight_data_parser.msg.lng = pos.y;
flight_data_parser.msg.alt = pos.z;
flight_data_parser.msg.vx = vel.x;
flight_data_parser.msg.vy = vel.y;
flight_data_parser.msg.vz = vel.z;
flight_data_parser.msg.roll = euler.x;
flight_data_parser.msg.pitch = euler.y;
flight_data_parser.msg.yaw = euler.z;
crc_sum = crc_crc8(flight_data_parser.data,FLIGHT_DATA_LEN-1);
flight_data_parser.msg.crc = crc_sum;
write_data_to_port(_zr_api_port,flight_data_parser.data, FLIGHT_DATA_LEN);
}
/**
* @brief 获取串口接收到的控制数据
*
* @param mode 飞行模式，如果不是guide则false
* @param type 控制模式，速度模式，位置模式
* @param data 控制的数据，根据type决定是速度还是位置
* @param yaw_deg 偏航角
* @return true
* @return false
*/
bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l &data,float &yaw_deg)
{
static uint32_t last_time = 0;
uint8_t crc_sum = 0;
if(control_data_last_time == last_time){ // 数据没更新，直接退出
return false;
}
last_time = control_data_last_time;
WITH_SEMAPHORE(sem);
if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头
Debug("head error: %02x",flight_control_parser.msg.head );
return false;
}
// crc 校验
crc_sum = crc_crc8(flight_control_parser.data,CONTROL_DATA_LEN-1);
if(crc_sum != flight_control_parser.data[CONTROL_DATA_LEN - 1]){
Debug("crc err: %02x ,clc: %02x",flight_control_parser.msg.crc,crc_sum);
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRCRC); // 应答CRC错误
return false; // 校验失败
}
type = flight_control_parser.msg.type;
if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_VEL_P ){
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答控制类型错误
Debug("type error: %d",(int)flight_control_parser.msg.type);
return false; // 控制类型错误
}
if(type != ZR_Msg_Type::MSG_CONTROL_TKOFF && mode != 4){
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRMODE);
Debug("mode error: %d",mode);
return false; // 飞行模式错误
}
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_OK); // 应答成功
// 速度或者位置，3个int32
// memcpy(&data,flight_control_parser.data + 4,12); // 第4位开始是控制数据，长度3*4
data.x = flight_control_parser.msg.x;
data.y = flight_control_parser.msg.y;
data.z = flight_control_parser.msg.z;
yaw_deg = flight_control_parser.msg.yaw;
return true;
}
void AC_ZR_Serial_API::set_control_ask(uint8_t msg_id,ZR_Msg_Type receive_type,ZR_Msg_ASK ask)
{
uint8_t crc_sum = 0;
flight_control_ask.msg.msg_head = MSG_HEAD;
flight_control_ask.msg.msg_type = ZR_Msg_Type::MSG_ASK;
flight_control_ask.msg.length = CONTROL_ASK_LEN - 4;
flight_control_ask.msg.msg_id = msg_id;
flight_control_ask.msg.receive_type = receive_type;
flight_control_ask.msg.msg_ask = ask;
crc_sum = crc_crc8(flight_control_ask.data,CONTROL_ASK_LEN-1);
flight_control_ask.msg.crc = crc_sum;
// Debug("ack:%d, ok:%d",(int)flight_control_ask.msg.receive_type,(int)flight_control_ask.msg.msg_ask);
write_data_to_port(_zr_api_port,flight_control_ask.data, CONTROL_ASK_LEN);
}
void AC_ZR_Serial_API::update(void)
{
if(!data_trans_init || _zr_api_port == nullptr){
return;
}
#if 0
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)) || serial_data_len >= CONTROL_DATA_LEN){ // 接收可能被中断，延时一段
// Debug("%d,receive len:%d",now_time,serial_data_len);
if(serial_data_len == CONTROL_DATA_LEN){ // 正常控制指令都是 CONTROL_DATA_LEN 长度
WITH_SEMAPHORE(sem);
memcpy(flight_control_parser.data,serial_data_from_device,serial_data_len);
control_data_last_time = AP_HAL::millis();
}
serial_data_len = 0;
// memset(serial_data_from_device,0,CONTROL_DATA_LEN);
}
#endif
}
namespace AP {
AC_ZR_Serial_API *zr_data_trans()
{
return AC_ZR_Serial_API::get_singleton();
}
}

203
libraries/AC_ZR_APP/AC_ZR_Serial_API.h
Unescape View File

@ -0,0 +1,203 @@ @@ -0,0 +1,203 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <GCS_MAVLink/GCS.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#define MSG_HEAD 0xFE
#define FLIGHT_DATA_LEN 40
#define CONTROL_DATA_LEN 21
#define VEHICLE_STATUS_LEN 13
#define CONTROL_ASK_LEN 7
class AC_ZR_Serial_API
{
public:
AC_ZR_Serial_API();
/* Do not allow copies */
AC_ZR_Serial_API(const AC_ZR_Serial_API &other) = delete;
AC_ZR_Serial_API &operator=(const AC_ZR_Serial_API&) = delete;
static AC_ZR_Serial_API *get_singleton() {
return _singleton;
}
// 消息类型
enum class ZR_Msg_Type : uint8_t {
MSG_ASK = 0, // 应答消息
MSG_FLIGHT_DATA , // 飞行数据
MSG_VEHICLE_STATUS, // 飞机状态
MSG_CONTROL_TKOFF , // 飞行控制：起飞
MSG_CONTROL_POS , // 飞行控制：位置控制模式
MSG_CONTROL_VEL , // 飞行控制：速度控制模式
MSG_CONTROL_VEL_P // 飞行控制：速度控制模式
};
enum class ZR_Msg_ASK : uint8_t{
MSG_ASK_OK = 0, // 成功
MSG_ASK_ERRCRC , // crc校验错误
MSG_ASK_ERRTYPE , // 消息类型错误
MSG_ASK_ERRMODE , // 飞行模式错误
MSG_ASK_OUTRANGE // 控制数据超出限制
};
/// Startup initialisation.
void init(const AP_SerialManager& serial_manager);
void update(void);
void write_data_to_port(AP_HAL::UARTDriver *_port,const uint8_t *data, uint8_t len);
void read_data_from_port(AP_HAL::UARTDriver *_port,uint8_t *data, uint8_t *len);
void get_api_data(uint8_t *data, uint8_t len);
void get_vehicle_NEU_pos(Vector3f vec_neu);
void get_vehicle_euler_angles(Vector3f euler);
void get_vehicle_flight_data(Vector3l pos,Vector3l vel,Vector3f euler);
void get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t home_distance,uint16_t volt_mv,uint8_t bat_remaining);
bool get_control_data(uint8_t mode,ZR_Msg_Type &type,Vector3l &data,float &yaw_deg);
// 接收控制指令的应答
void set_control_ask(uint8_t msg_id,ZR_Msg_Type receive_type,ZR_Msg_ASK ask);
bool data_trans_init;
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;
uint32_t control_data_last_time;
uint8_t serial_data_from_device[CONTROL_DATA_LEN];
uint8_t mav_data_from_host[FLIGHT_DATA_LEN];
uint8_t serial_data_len,mav_data_len,mav_len;
bool get_mav_data;
struct PACKED flight_data_parser_s
{
uint8_t msg_head;
ZR_Msg_Type msg_type;
uint8_t length;
// 3
int32_t lat;
int32_t lng;
int32_t alt;
// 15
int32_t vx;
int32_t vy;
int32_t vz;
// 27
float roll;
float pitch;
float yaw;
// 39
uint8_t crc;
};
union PACKED flight_data_parser_u
{
flight_data_parser_s msg;
uint8_t data[FLIGHT_DATA_LEN];
}flight_data_parser;
struct PACKED zr_flight_control_data_s
{ // 0
uint8_t head;
ZR_Msg_Type type;
uint8_t length;
uint8_t msg_id;
// 4
int32_t x;
int32_t y;
int32_t z;
// 16
float yaw;
// 10
uint8_t crc;
};
union PACKED flight_control_parser_u
{
zr_flight_control_data_s msg;
uint8_t data[CONTROL_DATA_LEN];
}flight_control_parser;
struct PACKED flight_control_ask_s
{
uint8_t msg_head;
ZR_Msg_Type msg_type;
uint8_t length;
uint8_t msg_id;
// 4
ZR_Msg_Type receive_type;
ZR_Msg_ASK msg_ask;
uint8_t crc;
};
union PACKED flight_control_ask_parser_u
{
flight_control_ask_s msg;
uint8_t data[CONTROL_ASK_LEN];
}flight_control_ask;
struct PACKED vehicle_status_parser_s
{
uint8_t msg_head;
ZR_Msg_Type msg_type;
uint8_t length;
// 3
uint8_t fly_mode;
uint8_t fly_status;
// 5
uint32_t home_distance;
// 9
uint16_t volt_mv;
uint8_t bat_remaining;
// 12
uint8_t crc;
};
union PACKED vehicle_status_parser_u
{
vehicle_status_parser_s msg;
uint8_t data[VEHICLE_STATUS_LEN];
}vehicle_status;
struct control_t {
uint32_t time_stamp;
ZR_Msg_Type type;
Vector3l data;
float yaw_deg;
} control_data;
HAL_Semaphore sem;
uint8_t *pktbuf;
uint16_t pktoffset;
uint16_t bufsize;
uint8_t flight_mode;
};

328
libraries/AC_ZR_APP/ZR_Des.cpp
Unescape View File

@ -0,0 +1,328 @@ @@ -0,0 +1,328 @@
#include "ZR_Des.h"
const uint8_t initial_tr[64] =
{
57, 49, 41, 33, 25, 17, 9, 1,
59, 51, 43, 35, 27, 19, 11, 3,
61, 53, 45, 37, 29, 21, 13, 5,
63, 55, 47, 39, 31, 23, 15, 7,
56, 48, 40, 32, 24, 16, 8, 0,
58, 50, 42, 34, 26, 18, 10, 2,
60, 52, 44, 36, 28, 20, 12, 4,
62, 54, 46, 38, 30, 22, 14, 6};
const uint8_t final_tr[64] =
{
39, 7, 47, 15, 55, 23, 63, 31,
38, 6, 46, 14, 54, 22, 62, 30,
37, 5, 45, 13, 53, 21, 61, 29,
36, 4, 44, 12, 52, 20, 60, 28,
35, 3, 43, 11, 51, 19, 59, 27,
34, 2, 42, 10, 50, 18, 58, 26,
33, 1, 41, 9, 49, 17, 57, 25,
32, 0, 40, 8, 48, 16, 56, 24};
const uint8_t swap[64] =
{
33, 34, 35, 36, 37, 38, 39, 40,
41, 42, 43, 44, 45, 46, 47, 48,
49, 50, 51, 52, 53, 54, 55, 56,
57, 58, 59, 60, 61, 62, 63, 64,
1, 2, 3, 4, 5, 6, 7, 8,
9, 10, 11, 12, 13, 14, 15, 16,
17, 18, 19, 20, 21, 22, 23, 24,
25, 26, 27, 28, 29, 30, 31, 32};
const uint8_t key_tr1[56] =
{
56, 48, 40, 32, 24, 16, 8,
0, 57, 49, 41, 33, 25, 17,
9, 1, 58, 50, 42, 34, 26,
18, 10, 2, 59, 51, 43, 35,
62, 54, 46, 38, 30, 22, 14,
6, 61, 53, 45, 37, 29, 21,
13, 5, 60, 52, 44, 36, 28,
20, 12, 4, 27, 19, 11, 3};
const uint8_t key_tr2[64] =
{
0, 0, 13, 4, 16, 10, 23, 0,
0, 0, 2, 9, 27, 14, 5, 20,
0, 0, 22, 7, 18, 11, 3, 25,
0, 0, 15, 1, 6, 26, 19, 12,
0, 0, 40, 54, 51, 30, 36, 46,
0, 0, 29, 47, 39, 50, 44, 32,
0, 0, 43, 52, 48, 38, 55, 33,
0, 0, 45, 31, 41, 49, 35, 28};
const uint8_t etr[64] =
{
0, 0, 31, 4, 0, 1, 2, 3,
0, 0, 3, 8, 4, 5, 6, 7,
0, 0, 7, 12, 8, 9, 10, 11,
0, 0, 11, 16, 12, 13, 14, 15,
0, 0, 15, 20, 16, 17, 18, 19,
0, 0, 19, 24, 20, 21, 22, 23,
0, 0, 23, 28, 24, 25, 26, 27,
0, 0, 27, 0, 28, 29, 30, 31};
const uint8_t ptr[32] =
{
31, 14, 39, 44, 60, 23, 55, 36,
4, 30, 46, 53, 12, 37, 62, 21,
5, 15, 47, 29, 63, 54, 6, 20,
38, 28, 61, 13, 45, 22, 7, 52};
const uint8_t s[8][64] =
{
{14, 4, 13, 1, 2, 15, 11, 8, 3, 10, 6, 12, 5, 9, 0, 7,
0, 15, 7, 4, 14, 2, 13, 1, 10, 6, 12, 11, 9, 5, 3, 8,
4, 1, 14, 8, 13, 6, 2, 11, 15, 12, 9, 7, 3, 10, 5, 0,
15, 12, 8, 2, 4, 9, 1, 7, 5, 11, 3, 14, 10, 0, 6, 13},
{15, 1, 8, 14, 6, 11, 3, 4, 9, 7, 2, 13, 12, 0, 5, 10,
3, 13, 4, 7, 15, 2, 8, 14, 12, 0, 1, 10, 6, 9, 11, 5,
0, 14, 7, 11, 10, 4, 13, 1, 5, 8, 12, 6, 9, 3, 2, 15,
13, 8, 10, 1, 3, 15, 4, 2, 11, 6, 7, 12, 0, 5, 14, 9},
{10, 0, 9, 14, 6, 3, 15, 5, 1, 13, 12, 7, 11, 4, 2, 8,
13, 7, 0, 9, 3, 4, 6, 10, 2, 8, 5, 14, 12, 11, 15, 1,
13, 6, 4, 9, 8, 15, 3, 0, 11, 1, 2, 12, 5, 10, 14, 7,
1, 10, 13, 0, 6, 9, 8, 7, 4, 15, 14, 3, 11, 5, 2, 12},
{7, 13, 14, 3, 0, 6, 9, 10, 1, 2, 8, 5, 11, 12, 4, 15,
13, 8, 11, 5, 6, 15, 0, 3, 4, 7, 2, 12, 1, 10, 14, 9,
10, 6, 9, 0, 12, 11, 7, 13, 15, 1, 3, 14, 5, 2, 8, 4,
3, 15, 0, 6, 10, 1, 13, 8, 9, 4, 5, 11, 12, 7, 2, 14},
{2, 12, 4, 1, 7, 10, 11, 6, 8, 5, 3, 15, 13, 0, 14, 9,
14, 11, 2, 12, 4, 7, 13, 1, 5, 0, 15, 10, 3, 9, 8, 6,
4, 2, 1, 11, 10, 13, 7, 8, 15, 9, 12, 5, 6, 3, 0, 14,
11, 8, 12, 7, 1, 14, 2, 13, 6, 15, 0, 9, 10, 4, 5, 3},
{12, 1, 10, 15, 9, 2, 6, 8, 0, 13, 3, 4, 14, 7, 5, 11,
10, 15, 4, 2, 7, 12, 9, 5, 6, 1, 13, 14, 0, 11, 3, 8,
9, 14, 15, 5, 2, 8, 12, 3, 7, 0, 4, 10, 1, 13, 11, 6,
4, 3, 2, 12, 9, 5, 15, 10, 11, 14, 1, 7, 6, 0, 8, 13},
{4, 11, 2, 14, 15, 0, 8, 13, 3, 12, 9, 7, 5, 10, 6, 1,
13, 0, 11, 7, 4, 9, 1, 10, 14, 3, 5, 12, 2, 15, 8, 6,
1, 4, 11, 13, 12, 3, 7, 14, 10, 15, 6, 8, 0, 5, 9, 2,
6, 11, 13, 8, 1, 4, 10, 7, 9, 5, 0, 15, 14, 2, 3, 12},
{13, 2, 8, 4, 6, 15, 11, 1, 10, 9, 3, 14, 5, 0, 12, 7,
1, 15, 13, 8, 10, 3, 7, 4, 12, 5, 6, 11, 0, 14, 9, 2,
7, 11, 4, 1, 9, 12, 14, 2, 0, 6, 10, 13, 15, 3, 5, 8,
2, 1, 14, 7, 4, 10, 8, 13, 15, 12, 9, 0, 3, 5, 6, 11}};
const uint8_t rots[16] =
{
1, 1, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2, 2, 2, 1};
const uint8_t bit_msk[8] =
{
0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01};
/************************************************************************/
/* */
/* Module title: des */
/* Module type: des mainrutine */
/* */
/* Author: YXH */
/* Date: 2012-07-13 */
/* */
/* Last changed by: YXH */
/* Date: 2012-07-13 */
/* */
/* Functional Description: Encipher and decipher 64 bits string */
/* according to a 64 bits key string */
/* The string format is shown below */
/* */
/* input parameter 1: pointer to 64 bits input string */
/* 2: pointer to 64 bits key string */
/* 3: boolean if false indicating enciphering */
/* if true dechiphering 1解密 */
/* 4: pointer to a 64 bits output string */
/************************************************************************/
/* */
/* msb lsb */
/* bit bit */
/* +-- -- -- -- -- -- -- --+ */
/* addr !1st 8th! */
/* +-- -- -- -- -- -- -- --+ */
/* addr+1 !9th 16th! */
/* +-- -- -- -- -- -- -- --+ */
/* : : */
/* : : */
/* +-- -- -- -- -- -- -- --+ */
/* addr+7 !57th 64th! */
/* +-- -- -- -- -- -- -- --+ */
/* */
/************************************************************************/
void ZR_Des::des(uint8_t* plain_strng, uint8_t* key, uint8_t d, uint8_t* ciph_strng)
{
uint8_t a_str[8], b_str[8], x_str[8];
uint8_t i, j, * pkey, temp;
for (i = 0; i < 8; ++i)
{
if (key[i] != main_key[i])
{
compute_subkeys(key);
i = 7;
}
}
transpose(plain_strng, a_str, initial_tr, 64);
for (i = 1; i < 17; ++i)
{
for (j = 0; j < 8; ++j) { b_str[j] = a_str[j]; }
if (!d) /*enchipher*/
pkey = &sub_keys[i - 1][0];
else /*dechipher*/
pkey = &sub_keys[16 - i][0];
for (j = 0; j < 4; ++j) { a_str[j] = b_str[j + 4]; }
f(pkey, a_str, x_str);
for (j = 0; j < 4; ++j) { a_str[j + 4] = b_str[j] ^ x_str[j]; }
}
temp = a_str[0]; a_str[0] = a_str[4]; a_str[4] = temp;
temp = a_str[1]; a_str[1] = a_str[5]; a_str[5] = temp;
temp = a_str[2]; a_str[2] = a_str[6]; a_str[6] = temp;
temp = a_str[3]; a_str[3] = a_str[7]; a_str[7] = temp;
transpose(a_str, ciph_strng, final_tr, 64);
}
/************************************************************************/
/* */
/* Module title: transpose */
/* Module type: des subrutine */
/* */
/* Author: YXH */
/* Date: 2012-07-13 */
/* */
/* Last changed by: YXH */
/* Date: 2012-07-13 */
/* */
/* Functional Description: Permute n bits in a string, according */
/* to a table describing the new order. */
/* 0 < n <= 64 */
/* */
/* input parameter 1: pointer to first byte in input string */
/* 2: pointer to first byte in output string */
/* 3: pointer to table describing new order */
/* 4: number of bits to be permuted */
/************************************************************************/
void ZR_Des:: transpose(uint8_t* idata, uint8_t* odata, const uint8_t* tbl, uint8_t n)
{
const uint8_t* tab_adr;
int i, bi_idx;
tab_adr = &bit_msk[0];
i = 0;
do
{
odata[i++] = 0;
} while (i < 8);
i = 0;
do
{
bi_idx = *tbl++;
if (idata[bi_idx >> 3] & tab_adr[bi_idx & 7])
{
odata[i >> 3] |= tab_adr[i & 7];
}
} while (++i < n);
}
/************************************************************************/
/* */
/* Module title: rotate_l */
/* Module type: des subrutine */
/* */
/* Author: YXH */
/* Date: 2012-07-13 */
/* */
/* Last changed by: YXH */
/* Date: 2012-07-13 */
/* */
/* Functional Description: rotate 2 concatenated strings of 28 */
/* bits one position to the left. */
/* */
/* input parameter 1: pointer to first byte in key string */
/* */
/************************************************************************/
void ZR_Des::rotate_l(uint8_t* key)
{
uint8_t str_x[8];
uint8_t i;
for (i = 0; i < 8; ++i) str_x[i] = key[i];
for (i = 0; i < 7; ++i)
{
key[i] = (key[i] << 1);
if ((i < 6) && ((str_x[i + 1] & 128) == 128))
key[i] |= 1;
}
if (str_x[0] & 0x80)
key[3] |= 0x10;
else
key[3] &= ~0x10;
if (str_x[3] & 0x08)
key[6] |= 0x01;
else
key[6] &= ~0x01;
}
/************************************************************************/
/* */
/* Module title: compute_subkeys */
/* Module type: des subrutine */
/* */
/* Author: YXH */
/* Date: 2012-07-13 */
/* */
/* Last changed by: YXH */
/* Date: 2012-07-13 */
/* */
/* Functional Description: Computes the 16 sub keys for use in the */
/* DES algorithm */
/* */
/* input parameter 1: pointer to first byte in key string */
/* output : fills the array sub_keys[16][8] with */
/* sub keys and stores the input key in */
/* main_key[8] */
/************************************************************************/
void ZR_Des::compute_subkeys(uint8_t* key)
{
uint8_t i, j, ikey[8], okey[8];
for (i = 0; i < 8; ++i)
{
main_key[i] = key[i];
}
transpose(key, ikey, key_tr1, 56);
for (i = 0; i < 16; ++i)
{
for (j = 0; j < rots[i]; ++j) { rotate_l(ikey); }
transpose(ikey, okey, key_tr2, 64);
for (j = 0; j < 8; ++j)
{
sub_keys[i][j] = okey[j];
}
}
}
/************************************************************************/
/* */
/* Module title: f */
/* Module type: des subrutine */
/* */
/* Author: YXH */
/* Date: 2012-07-13 */
/* */
/* Last changed by: YXH */
/* Date: 2012-07-13 */
/* */
/* Functional Description: The chipher function */
/* */
/* input parameter 1: pointer to first byte in key string */
/* 2: pointer to a 32 bit input string */
/* 3: pointer to a 32 bit output string */
/************************************************************************/
void ZR_Des::f(uint8_t* skey, uint8_t* a_str, uint8_t* x_str)
{
uint8_t e_str[8], y_str[8], z_str[8];
uint8_t k;
transpose(a_str, e_str, etr, 64);
for (k = 0; k < 8; ++k)
{
y_str[k] = (e_str[k] ^ skey[k]) & 63;
z_str[k] = s[k][y_str[k]];
}
transpose(z_str, x_str, ptr, 32);
}

22
libraries/AC_ZR_APP/ZR_Des.h
Unescape View File

@ -0,0 +1,22 @@ @@ -0,0 +1,22 @@
#pragma once
#include <stdint.h>
class ZR_Des
{
private:
/* data */
public:
// uint8_t deskey[8] = {90, 114, 90, 107, 85, 97, 86, 103}; //{ "ZrZkUaVg" };
// uint8_t deskey[8] = {90, 114, 90, 104, 68, 85, 97, 86}; //{ "ZrZhDUaV" };
uint8_t DES_Encrypt_key[8];
uint8_t DES_Decrypt_key[8];
uint8_t sub_keys[16][8]; //sub_keys[16][8]
uint8_t main_key[8];
void des(uint8_t *, uint8_t *, uint8_t, uint8_t *);
void FLASH_Read_KEYS(uint8_t key_index);
void transpose(uint8_t *, uint8_t *, const uint8_t *, uint8_t);
void rotate_l(uint8_t *);
void compute_subkeys(uint8_t *);
void f(uint8_t *, uint8_t *, uint8_t *);
};

7
libraries/AP_SerialManager/AP_SerialManager.cpp
Unescape View File

@ -469,6 +469,13 @@ void AP_SerialManager::init() @@ -469,6 +469,13 @@ void AP_SerialManager::init()
AP::RC().add_uart(state[i].uart);
break;
#endif
case SerialProtocol_ZR_Serial_API:
state[i].uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_ZR_API_BUFSIZE_RX,
AP_SERIALMANAGER_ZR_API_BUFSIZE_TX);
state[i].uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
break;
default:
state[i].uart->begin(map_baudrate(state[i].baud));

3
libraries/AP_SerialManager/AP_SerialManager.h
Unescape View File

@ -89,6 +89,8 @@ @@ -89,6 +89,8 @@
#define AP_SERIALMANAGER_SLCAN_BUFSIZE_RX 128
#define AP_SERIALMANAGER_SLCAN_BUFSIZE_TX 128
#define AP_SERIALMANAGER_ZR_API_BUFSIZE_RX 512
#define AP_SERIALMANAGER_ZR_API_BUFSIZE_TX 512
class AP_SerialManager {
public:
AP_SerialManager();
@ -125,6 +127,7 @@ public: @@ -125,6 +127,7 @@ public:
SerialProtocol_RCIN = 23,
SerialProtocol_Battery =24,
SerialProtocol_Camera_X30T = 25,
SerialProtocol_ZR_Serial_API = 39,
};
// get singleton instance

2
modules/mavlink

@ -1 +1 @@ @@ -1 +1 @@
Subproject commit 72b407db3991ed74631fa59a5e826615f569819f
Subproject commit 5fc095d17043557589ecc6a28d62fb234081fc87
Write Preview
Loading…
Cancel
Save