[swarm] add formation control for rotorcraft #2246
Conversation
Based on Ewoud Smeur (INDI) and Hector Garcia de Marina (Formation control) work. See https://blog.paparazziuav.org/2017/12/02/pilot-a-super-rotorcraft - several UAV can be control from the ground (using a joystick for instance) by sending proper acceleration setpoint to the INDI guidance controller - configuration of the formation is done from a JSON file (2 example files provided) Some extra changes: - update accel from IMU in GPS passthrough INS - add accel setpoint to ABI messages - by to accel setpoint in INDI guidance - send ground reference from natnet2ivy - possibility to have a joystick labeled 'GCS' with input2ivy
| sp_accel.z = (speed_sp_z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain; | ||
| float dt = get_sys_time_float() - time_of_accel_sp_2d; | ||
| if (dt > 0.5) { | ||
| indi_accel_sp_set_2d = false; |
There was a problem hiding this comment.
This seems strange...
I am not sure where else 'sp_accel' used but is overwritten somewhere else? Else it should be sufficient to immediately set indi_accel_sp_set_2d = false; without the dt check.
There was a problem hiding this comment.
if the guidance control loop is faster than the setpoint update, you want to avoid to switch the 2D/3D control and the derivate from speed until a timeout is reached
There was a problem hiding this comment.
I remember that when we wrote this condition for dt was also for safety porpuses. In particular, if the commands from the ground stop, then the rotorcraft resumes the flight plan (for example, hovering in a position with the guidance from INDI). Indeed, if the setpoint update is slower than the guidance, you do not want to have a "misbehavior" by switching between two set points from different systems (formation control or guidance).
I agree with kirk that this part could be more clear... in fact, I do not even remember how we wrote it. Although the behavior was as expected (including the safety).
There was a problem hiding this comment.
Indeed, the setpoint is updated over telemetry, which will update much slower than the periodic frequency.
There was a problem hiding this comment.
merging with an extra comment
| */ | ||
| static void accel_sp_cb(uint8_t sender_id, struct FloatVect3 *accel_sp) | ||
| { | ||
| if (sender_id == 1) { |
There was a problem hiding this comment.
I guess these should check for ACCEL_SP_3D_ID and ACCEL_SP_2D_ID
There was a problem hiding this comment.
actually, I didn't pay enough attention at how the message and ID wax used, it is now done correctly with a flag instead of abusing the sender ID
* [swarm] add formation control for rotorcraft Based on Ewoud Smeur (INDI) and Hector Garcia de Marina (Formation control) work. See https://blog.paparazziuav.org/2017/12/02/pilot-a-super-rotorcraft - several UAV can be control from the ground (using a joystick for instance) by sending proper acceleration setpoint to the INDI guidance controller - configuration of the formation is done from a JSON file (2 example files provided) Some extra changes: - update accel from IMU in GPS passthrough INS - add accel setpoint to ABI messages - by to accel setpoint in INDI guidance - send ground reference from natnet2ivy - possibility to have a joystick labeled 'GCS' with input2ivy
Based on Ewoud Smeur (INDI) and Hector Garcia de Marina (Formation
control) work. See
https://blog.paparazziuav.org/2017/12/02/pilot-a-super-rotorcraft
instance) by sending proper acceleration setpoint to the INDI guidance
controller
files provided)
Some extra changes: