Line data    Source code

       1             : #include<stdio.h>
       2             : #include "Rover.h"
       3             : 
       4             : // Function to set a desired pitch angle according to throttle
       5       54262 : void Rover::balancebot_pitch_control(float &throttle)
       6             : {
       7             :     // calculate desired pitch angle
       8       54262 :     const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max) + radians(g2.bal_pitch_trim);
       9             : 
      10             :     // calculate required throttle using PID controller
      11       54262 :     throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, radians(g2.bal_pitch_max), g2.motors.limit.throttle_lower || g2.motors.limit.throttle_upper, G_Dt) * 100.0f;
      12       54262 : }
      13             : 
      14             : // returns true if vehicle is a balance bot
      15             : // called in AP_MotorsUGV::output()
      16             : // this affects whether the vehicle tries to control its pitch with throttle output
      17     1241905 : bool Rover::is_balancebot() const
      18             : {
      19     1241905 :     return ((enum frame_class)g2.frame_class.get() == FRAME_BALANCEBOT);
      20             : }