Seeking advice on drone's tranfer function using Mavros Ros & PX4 Native Stack

  1. Recently, I am finding the transfer function of drone. Its input is roll pitch and yaw rate from topic mavros/imu_raw_data of mavros and its output is PWM signals of roll pitch yaw and throttle from mavros/actuator control that are coded on ROS, mavros, Odroid xu4 and px4 native stack firmware. The control signal of drone is from radio controller and sometimes this signal will be combined with chirping signals on roll pitch and yaw so that it will make my drone move around x y z axis and collect the necessary input and output data.
  2. To achieve this, I used topic mavros/rc/in which reads the roll pitch yaw and throttle signal from radio controller and scaled them to the range of -1 to 1. After that, I produce directly to the output of actuator using topic mavros/ actuator control (roll pitch yaw throttle channel of group 0). This code is fine! I can change my Pixhawk into Offboard Mode, then arm motor and control drone through radio signal, add chirping noise using switch on radio controller ( topic mavros/rc/in) as well as save the input and output data in text file.
  3. However, when running this code in my experiment, my drone oscillates a lot in take-off process and flip itself occasionally. I guess that the major reason is that the control signal from radio alone to control drone directly is not good.
  4. After that, I tried to turn it into Manual mode and used QRoundControl software to adjust PID parameters for roll pitch and yaw rate. Fortunately, my drone operates well with less fluctuation. However, with this flight mode, I cannot run my code as in section1 and 2 proposed because it is clear that my code only runs with Offboard Mode.
    Therefore, can anybody here that have experience with controlling drone with mavros and px4 native stack give me some ideas to solve my problem. I will run manual mode first and then switch it to offboard mode when my drone is stable? but to do that, what i have to add in my code or a process? or should I convert my code to program PX4 native stack firmware?
    Thank you too much for any help and discussion.