2020/3/24
We have two paths to move forward. (1) we need to track our own sim_pos. or, (2) we need to figure if we send the GPS correctly.
(1) Track out sim_pos. Wenxin, we have discussed this before. We have to create a parallel estimation, similar to sim_pos, but we add a new set of parameters and estimation to keep our_sim_pos. We can log them using their logging functions, or, we can also make out_sim_pos readable from the MAV_proxy side. I think it is better to revise in the simulator CPP side, instead of the Python proxy side. I guess you may have some thoughts on this already since our discussion before the Spring break.
(2) Dylan, you may work in this direction to verify if we send the GPS data correctly. The sim_GPS and the real GPS device both pass the data via the serial port. They use UBLOX as the default format. However, when we use GPS_TYPE_MAV, we directly process the MAVLINK Msg into the (AP_GPS::GPS_State state). We basically copy the examples from these sites.
https://www.ardusub.com/developers/gps-positioning.html
https://dronekit-python.readthedocs.io/en/latest/guide/copter/guided_mode.html
https://github.com/Zoneshi/Iris_Navigate/blob/master/Indoor_gps.py
When we use MAVProxy, the code send the GPS msg with mav_send_gps() call. While, they use vehicle.message_factory.gps_input_send() at https://github.com/Zoneshi/Iris_Navigate/blob/master/Indoor_gps.py
Dylan, could you check on their differences?
=====================================================
Wenxin, please also check the mode_guided.cpp. Line 55 set the "target_loc = copter.current_loc"; line 56, target_loc.set_alt_cm(takeoff_alt_cm, Location::AltFrame::ABOVE_HOME); set the altittue. Line 66 set the YAW before it takes off. It used a constant.
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
So, the XY set to the copter current location; the only change is supposed to be the altitude. Not sure why we see the sim_pos start to drift during the take-off.
to Wenxin, https://github.com/wenxinc1990/ardupilot https://github.com/wenxinc1990/ardupilot For your code, please document what you have changed. So, we can pay attentions to follow your setup.
2020/1/28
1. please check the the indoor GPS code
https://github.com/Zoneshi/Iris_Navigate/blob/master/Indoor_gps.py
- we can try use their GPS parameters.
- they did not send YAW. You set YAW =4500.
2. Dronekit developer guide
https://dronekit-python.readthedocs.io/en/latest/guide/copter/guided_mode.html
3. Another example https://www.ardusub.com/developers/gps-positioning.html
3. You code "if count % 10==0: " send fake GPS data every 10*0.1 = 1 second.
4. The drone seems not moving, until the point (near waypoint?), the velocity is changed and we then feed back it with GPS_INPUT. That causes the GPS velocity keep increasing
5. when add GPS_INPUT_IGNORE_FLAGS 8 to disable the GPS velocity feeding, the drone never moves. It seems the simple_goto( airspeed=speed) does not matter. If we do not feed back GPS velocity, the drone does not move.
Four questions to be answered
1. if the round-trip delay of read/revise/sendback cause too much error. Let estimate the location by add velocity*duration to update the location (lat/lon). Otherwise, the drone receives the position from the previous interval.
2. "map set showsimpos 1" shows the simpos is changed before it reach 99 meters, it seems we send the speed via GPS_INPUT to make it move. Please verify
3. Because we have a dead-loop, and feed back the GPS readings, the velocity keeps increasing. We need to define a end-condition, e.g., simpos is near our target position or the EKF position near the waypoint. It seems the EKF position is never close to the waypoint1.
The following description is contradict to what we see. We need to stop when near the waypoint1
"Vehicle.simple_goto() can be interrupted by a later command, and does not provide any functionality to indicate when the vehicle has reached its destination. Developers can use either a time delay or measure proximity to the target to give the vehicle an opportunity to reach its destination. The Example: Guided Mode Movement and Commands (Copter) shows both approaches.
You can optionally set the target movement speed using the function’s airspeed or groundspeed parameters (this is equivalent to setting Vehicle.airspeed or Vehicle.groundspeed). The speed setting will then be used for all positional movement commands until it is set to another value."
"From Copter 3.3 the vehicle will stop moving if a new message is not received in approximately 3 seconds. Prior to Copter 3.3 the message only needs to be sent once, and the velocity remains active until the next movement command is received. The example code works for both cases!"
4. let us find out when the sudden change happens.
Callback function can be used to monitor changes??
Callbacks are added using Vehicle.add_attribute_listener() or the Vehicle.on_attribute() decorator method.
Or,