Pozyx 3D option

RobGei · October 1, 2020, 8:02am

Dear all,

I am hoping that I could get some support regarding my POZYX integration into a multicopter. I am trying to use POZYX as a non-GPS indoor navigation system. I have the basic setup as described here:

https://ardupilot.org/copter/docs/common-pozyx.html

I am using Pozyx firmware 2.2 and the latest libs.

I placed the anchors at different heights (12cm to 165cm) making sure not to create an even plane. The IDs of the anchors are ascending in the “N-shape”. In order to get a correct movement in the map, this results in the x-axis pointing up (from the lowest ID to the second lowest) and the y-axis pointing right. For a right hand coordinate system, this leads to a z-axis pointing down. That means if the copter ascents, the numbers will get smaller. Is that accounted for in the controller?

Obviously I adapted the Arduino sketch to avoid the assertation of the too high TX power, and made sure that the z-component is transmitted.

If I configure the copter system to use POZYX 2D position data only, everything looks pretty well and acts as it is supposed to. But when I enable the z component (i.e. set the alt source of the EKF to 3), everything goes sideways. The IMUs stop aiding and pitch and roll angles are indicated even if the copter stands still.

I am using the POZYX_POS_ALG_TRACKING algorithm on Arduino and looking at the output data directly the z component looks rather good to me:

The xy position is fine from my point of view:

The spikes are likely caused by myself walking to the copter.

I am thankful for any hints! Thank you very much!!!

RobGei · October 2, 2020, 1:39pm

Ok, looking at this:

Arducopter Code

github.com

ArduPilot/ardupilot/blob/93fb5354daf104c37c83bbb46b90315987bdb593/libraries/AP_Beacon/AP_Beacon_Pozyx.cpp

/*
   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 <AP_HAL/AP_HAL.h>
#include "AP_Beacon_Pozyx.h"
#include <ctype.h>
#include <stdio.h>

This file has been truncated. show original

The z-position is indeed inverted. So that riddle is solved. Any ideas for the rest?

RobGei · October 2, 2020, 1:42pm

By the way:

The scale in the log browser needs factor 100.

Poh_Ling · March 27, 2021, 9:28am

Hi, I am working with the similar project. I realize that the position x and y received by ardupilot are different with what I sent, but pretty well for position z. e.g. I set a constant value 1000 for both x and y, but ardupilot received x = -1070, y = 930. Anyone can share some experience/ advise for me? Thank you.