rover_guidance_steering.h

Go to the documentation of this file.

/*
 * Copyright (C) 2021 Jesús Bautista <jesusbautistavillar@gmail.com>
 *                    Hector García  <noeth3r@gmail.com>
 *               2025 Gautier Hattenberger <gautier.hattenberger@gmail.com>
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 */
 
#ifndef ROVER_GUIDANCE_STEERING_H
#define ROVER_GUIDANCE_STEERING_H
 
#include "std.h"
#include <math.h>
#include "math/pprz_geodetic_float.h"
 
#include "generated/airframe.h"
 
// Check critical global definitiones
#ifndef SERVO_MOTOR_THROTTLE_IDX
#error "Steering rover firmware requires the servo MOTOR_THROTTLE"
#endif
 
#ifndef SERVO_MOTOR_STEERING_IDX
#error "Steering rover firmware requires the servo MOTOR_STEERING"
#endif
 
#ifndef COMMAND_THROTTLE
#error "Steering rover firmware requires the command COMMAND_THROTTLE"
#endif
 
#ifndef COMMAND_STEERING
#error "Steering rover firmware requires the command COMMAND_STEERING"
#endif
 
 
// MIN_DELTA, MAX_DELTA: Min and max wheels turning angle (deg)
// You should measure this angle if you want to have an
// efficient control in your steering
#ifndef MAX_DELTA
#define MAX_DELTA 15.0
#endif
#ifndef MIN_DELTA
#define MIN_DELTA MAX_DELTA
#endif
 
// This variables allow you to configurate de max and min
// steering actuator signal. There is a mecanic limitation
// for the actuator in the steering of our rover, so we have
// to limit the actuator travel.
#ifndef MAX_CMD_SHUT
#define MAX_CMD_SHUT 0
#endif
#ifndef MIN_CMD_SHUT
#define MIN_CMD_SHUT 0
#endif
 
// MIN_SPEED, MAX_SPEED: Min and max state speed (m/s)
#ifndef MAX_SPEED
#define MAX_SPEED 999.0 //We don't really use that variable
#endif
#ifndef MIN_SPEED
#define MIN_SPEED 0.2 //But this one is mandatory because we have
#endif                //to deal with GPS noise (and 1/v in guidance control).
 
// DRIVE_SHAFT_DISTANCE: Distance between front and rear wheels (m)
#ifndef DRIVE_SHAFT_DISTANCE
#define DRIVE_SHAFT_DISTANCE 0.25
#warning "Construction variable DRIVE_SHAFT_DISTANCE for steering wheel rover not defined"
#endif
 
// SR_MEASURED_KF: Lineal feed forward control constant (have to be measured in new servos)
#ifndef SR_MEASURED_KF
#define SR_MEASURED_KF 10
#warning "Construction constant SR_MEASURED_KF for steering wheel rover not defined"
#endif
 
 
// High level commands
typedef struct {
  float speed;
  float delta;
} sr_cmd_t;
 
// Main structure
typedef struct {
  sr_cmd_t cmd;
  float throttle;
  float heading_sp;            
  float omega_sp;              
 
  float pos_kp;
  float heading_kp;
  float speed_error;
  float kf;
  float kp;
  float ki;
} rover_ctrl;
 
extern rover_ctrl guidance_control;
 
extern void rover_guidance_steering_init(void);
extern void rover_guidance_steering_periodic(void); // call state machine
extern void rover_guidance_steering_heading_ctrl(float omega);
extern void rover_guidance_steering_speed_ctrl(void);
extern void rover_guidance_steering_setpoints(struct EnuCoor_f pos_sp, float *heading_sp);
extern void rover_guidance_steering_pid_reset(void);
extern void rover_guidance_steering_kill(void);
 
extern void rover_guidance_steering_set_speed_pgain(float pgain);
extern void rover_guidance_steering_set_speed_igain(float igain);
 
// Bound delta
#define BoundDelta(delta) (delta < -MIN_DELTA ? -MIN_DELTA : \
                          (delta >  MAX_DELTA ?  MAX_DELTA : \
                           delta))
 
// Bound speed
#define BoundSpeed(speed) (speed <  MIN_SPEED ? MIN_SPEED : \
                          (speed >  MAX_SPEED ? MAX_SPEED : \
                           speed))
 
// Bound throttle
#define BoundThrottle(throttle) TRIM_PPRZ((int)throttle)
 
// Set low level commands from high level commands
#define GetCmdFromDelta(delta) (delta >= 0 ? delta/MAX_DELTA * (MAX_PPRZ - (int)MAX_CMD_SHUT) : \
                                             delta/MIN_DELTA * (MAX_PPRZ - (int)MIN_CMD_SHUT))
 
// This macro is for NAV state
#define GetCmdFromThrottle(throttle) (autopilot_throttle_killed() ? 0 : TRIM_PPRZ((int)throttle))
 
// Set AP throttle value
#define SetAPThrottleFromCommands(void) { \
    autopilot.throttle = commands[COMMAND_THROTTLE]; \
  }
 
#endif // ROVER_GUIDANCE_STEERING_H