4/14
Wenxin revised the simstate message format. So, we can read the simstate velocity correctly and send to the SITL via the python interface. Now, we need to rethink the overall issues related with how to build GPS inputs to affect EKF such that the EKF states affect the flight paths in AutoPilot.
Point (1) Fake GPS--> Point (2) EKF states --> Point (3) affect AutoPilot algorithm (fly-along-track) WP_NAV
IMU reading at 100Hz; GPS is at 8Hz; AutoPilot algorithm is even more slower;
Task 1: Please try to add a little shift in the Python code into the simstate readings, and see if it is the same as we changed the CPP code in the very beginning. So, it helps us to confirm that we did “move” the GPS updates based simstate to our Python inputs.
Question1: To spoof a dst D to D'. Assume we can do it with a vector D_delta. Now, we need to divide D_delta into multiple WP_NAV cycles. (how WP_NAV cycle is defined?). In each cycle, we need to use EKF states to make autopilot decisions. We need to reversely find out what EKF states will results in the WP_NAV cycle decisions. Because they are updated in different frequencies, how to achieve this?
Furthermore, after we identify the EKF states we need to fool the WP_NAV al, we need to find out what GPS signals can result in these EKF states.
NOTE 0: GPS data is from simstate. The estimation of simstate will be determined by body acceleration/rotaton from IMU sensors.
Note 1: turn off DRIFT_DELAY and DRIFT_TIME in SITL.cpp to stop the initial drifting before taking off.
Note 2: Side note about the different update frequency IMU/GPS/WAV
about the AP_scheduler in copter.cpp. The three items: 1st is the name, such as update_GPS, the 2nd is 50 ticks, not Hz. Each tick is 1/400s for copter. So, GPS updates 8 times per second. You may want to change your Python code delay to 0.125; so 8 time per second. The comments in the code are confusing. Please read the Learning ArduPilot doc at
https://ardupilot.org/dev/docs/learning-ardupilot-threading.html?highlight=ap_scheduler
“If you look inside that file you will see a small table with a set of 3 scheduling tasks. Associated with each task are two numbers. The table looks like this:
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ ins_update, 1, 1000 },
{ one_hz_print, 50, 1000 },
{ five_second_call, 250, 1800 },
};
The first number after each function name is the call frequency, in units controlled by the ins.init() call. For this example sketch the ins.init() uses RATE_50HZ, so each scheduling step is 20ms. That means the ins_update() call is made every 20ms, the one_hz_print() function is called every 50 times (ie. once a second) and the five_second_call() is called every 250 times (ie. once every 5 seconds).
The third number is the maximum time that the function is expected to take. This is used to avoid making the call unless there is enough time left in this scheduling run to run the function. When scheduler.run() is called it is passed the amount of time (in microseconds) available for running tasks, and if the worst case time for this task would mean it wouldn’t fit before that time runs out then it won’t be called.”
A related doc: In the AP_Scheduler.cpp, the ticks are defined 400Hz or 50Hz, line 34 to line 38. Copter.cpp use 400.
3/10/20
We need consider Copter/EKF/AHRS as the firmware and SITL as the hardware.
AP_HAL_SITL folder defines the "hardware board" for SITL. Ardupilot/EKF read/control the "board".
SITL folder simulates the hardware. So, it uses UART to "read" GPS inputs such as UBLOX format, and passes the output to AHRS as "hardware" output via the gps_pipe.
AP_GPS.cpp, GPS_TYPE_MAV, line 474, shows that the GPS_TYPE_MAV, it is different from all other SITL known GPS types, (because it is not from "hardware" but from software MAVLink input (??). here new_gps object is created. AP_GPS_MAV.cpp defines the new object.
update_dynamic use accel_body and rot_accel to figure out velocity, etc.
AP_HAL_Empty define a template of boards.
AP_SerialManager::init() state[3].uart=hal.uartB; define the first GPS
AP_GPS has two inputs: (1) from UART via SITL simulate (2) from MAV, in AP_GPS.cpp
The question remains: why simpos is wrong? Where did they made it wrong? If we track it ourself, how we do it correctly?
Set DRIFT_TIME from 5 to 0. in the test code, make the initial simpos drift slower.
In the test scripts, limit the speed to use velN and velE to control the heading. Enable GPS HGD to 1.
Now we still have issues with the estimated state of simpos. WHen we set the delat in the script to a smaller value for GPS estimates, close to 0, plus limit the speed some time we can control the simpso, but we are not sure if it si correct. We need to figure how simpos is update in SITL_state.cpp
2/19/20
*** The following notes will be updated as we are more clear about the code.*** Some sentences may not be accurate at this time.***
Prof. Duan tried with the estimated GPS position in this test code in the following. It still shows GPS glitches and then the copter landed. But because the script is still sending the GPS message, the GPS location is still showing, but the simpos and the drone state have stopped. As the speed is only 2m/s, it should not have glitches with the estimated position. We have to dig into this. The simpos is starting to move during the takeoff period (about 40 meters). It seems always at the same time. So, maybe some value accumulated up to a threshold. Wenxin mentioned the IMU noisy is between 0 and 1. The sim position seems updated only based on the gyro vector. It is not clear if the gyro info has included the GPS data. in /libraries/sitl/sim_Aircraft.cpp
/*
update the simulation attitude and relative position
*/
Aircraft::update_dynamics(const Vector3f &rot_accel)
As we discussed Friday, there are two GPS sources: (1) one is the simulated GPS, (2) another is the GPS msg we sent via MAVLink. We
need to check which GPS source affects the glitches.
in SITL.h, GPSType does not include GPS_INPUT or tyep 14. It seems the GPS_INPUT may be only works for the real ardupilot code, not the SILT.
enum GPSType {
GPS_TYPE_NONE = 0,
GPS_TYPE_UBLOX = 1,
GPS_TYPE_MTK = 2,
GPS_TYPE_MTK16 = 3,
GPS_TYPE_MTK19 = 4,
GPS_TYPE_NMEA = 5,
GPS_TYPE_SBP = 6,
GPS_TYPE_FILE = 7,
GPS_TYPE_NOVA = 8,
GPS_TYPE_SBP2 = 9,
};
in SITL.cpp, GPS_TYPE is default GPS_TYPE_UBLOX (?). But this is a user settable parameter. Also, GPS_DIABLE 0 and GPS_DELAY 1 are defined here. and a lot of other GPS parameter defined
// table of user settable parameters
const AP_Param::GroupInfo SITL::var_info[] = {
AP_GROUPINFO("GPS_DISABLE",4, SITL, gps_disable, 0),
AP_GROUPINFO("GPS_TYPE", 12, SITL, gps_type[0], SITL::GPS_TYPE_UBLOX),
AP_GROUPINFO("GPS_BYTELOSS", 13, SITL, gps_byteloss, 0),
AP_GROUPINFO("GPS_NUMSATS", 14, SITL, gps_numsats, 10),
AP_GROUPINFO("GPS_GLITCH", 17, SITL, gps_glitch[0], 0),
AP_GROUPINFO("GPS_HZ", 18, SITL, gps_hertz, 5),
AP_GROUPINFO("GPS2_ENABLE", 26, SITL, gps2_enable, 0),
AP_GROUPINFO("GPS_DRIFTALT", 37, SITL, gps_drift_alt, 0),
AP_GROUPINFO("GPS_POS", 54, SITL, gps_pos_offset[0], 0),
AP_GROUPINFO("GPS_NOISE", 58, SITL, gps_noise, 0),
AP_GROUPINFO("GP2_GLITCH", 59, SITL, gps_glitch[1], 0),
AP_GROUPINFO("GPS2_TYPE", 61, SITL, gps_type[1], SITL::GPS_TYPE_UBLOX),
AP_GROUPEND
};
// second table of user settable parameters for SITL.
const AP_Param::GroupInfo SITL::var_info2[] = {
AP_GROUPINFO("GPS_HDG", 43, SITL, gps_hdg_enabled, 0),
AP_GROUPEND
};
// third table of user settable parameters for SITL.
const AP_Param::GroupInfo SITL::var_info3[] = {
AP_GROUPINFO("GPS2_POS", 2, SITL, gps_pos_offset[1], 0),
AP_GROUPEND
};
gps_glitch[], defined in SITL.h
AP_Vector3f gps_glitch[2]; // glitch offsets in lat, lon and altitude
updated in sitl_gps.cpp
void SITL_State::_update_gps()
1267
The simulated position starts drifting away from the home during the
takeoff. It seems something accumulated and then GPS glitches is
detected.
sim_vehicle.py -w -A "--uartB=udpclient:127.0.0.1:14550" -v ArduCopter -L UH --console --map