Copter: Follow mode should allow arming with lead vehicle

Follow mode has recently been added to Copter but one issue we often see is that when a vehicle is switch to Follow mode it immediately climbs or descends a few meters because of differences in the estimated absolute altitude of the vehicle. If the vehicle climbs it is not too bad, but when the vehicle descends it often hits the ground.

We could make it much easier on the pilot if the pilot could just place the vehicles on the ground while they are disarmed, switch the follower vehicles into Follow mode and then arm and take-off with the lead vehicle and have all the followers arm and then follow.

Some more details to help with the implementation:

• AP_Follow should have a new parameter added called something like "FOLL_ARM_BEHAVE". Users would set this to "1" to indicate they want the follower to arm with the leader.
• AP_Follow::handle_msg() should be enhanced to consume the mavlink HEARTBEAT message from the leader. In particular it should record the system_status field which will be MAV_STATE_ACTIVE when the lead vehicle is armed, MAV_STATE_STANDBY when the lead vehicle is disarmed or landed, and some other value if it is initialising or in a failsafe.
• AP_Follow should get a new method called perhaps "bool arm_with_leader()" which becomes true if the lead vehicle's system_status has transitioned from MAV_STATE_STANDBY to MAV_STATE_ACTIVE within the last second. Otherwise it should always return false.
• create a new method in the Copter::ModeFollow class called check_arm_with_leader(). The definition should go at the bottom of the mode.h and the implementation should go in mode_follow.cpp. Inside this new method we should call init_arm_motors(true) and set_auto_armed(true) so this vehicle attempts to arm but only if these three things are true:
  • this vehicle is in Follow mode
  • this vehicle is disarmed
  • if AP_Follow::arm_with_leader() returns true.
• the above Copter::ModeFollow::check_arm_with_leader() should be called at 10hz from the scheduler.

Start with libraries/AP_Follow/AP_Follow.cpp, especially AP_Follow::handle_msg(), then read ArduCopter/mode.h and ArduCopter/mode_follow.cpp and the scheduler setup. Implement the requested leader-arm parameter, state transition check, and 10 Hz ModeFollow hook. Done means a disarmed follower in Follow mode arms only after the leader transitions to active, while other states do not trigger arming.