这部分内容相当简单,基本上读懂推进器电机模型文件就很容易能读懂程序。可以查阅之前撰写的关于电机模型文件的文章(链接)。程序对应cabin_controllers/pwm_controller。实际上就是编程实现模型文件中推进器推力与pwm波间的关系的函数。 一、输入输出
监听topic: (1)/command/thrust,格式:cabin_msgs::ThrustStamped,为推力分配模块发布的各推进器推力,重点关注; (2)/state/switches,格式:cabin_msgs::SwitchState,置为false时,锁定pwm波为固定值,其不会随推力的改变而改变,置为true时,模块正常工作,算是一个保险小措施,不需要关注; (3)/controls/reset,格式:cabin_msgs::ResetControls,pwm波重置,状态改变时,发布重置的初始pwm值,程序中为1500,同时推力记录清零,小功能,不需要关注; 发布topic: /command/pwm ,格式:cabin_msgs::PwmStamped,各推进器pwm波。 yaml文件: (1)机械模型文件,主要确定推进器为正桨(cw)还是反桨(ccw); (2)电机模型文件,推进器推力与pwm波关系曲线参数。 二、几个宏定义
#define MIN_PWM 1230
#define MAX_PWM 1770
#define NEUTRAL_PWM 1500
输出PWM波的范围被限定在区间[1230,1770]上,电机初始状态PWM为1500。 三、载入文件参数
PWMController::LoadParam<std::string>("properties_file", properties_file);
properties = YAML::LoadFile(properties_file);
PWMController::LoadThrusterProperties();
需要配合pwm_controller.launch阅读。 重点关注PWMController::LoadThrusterProperties()函数。
//Load thruster types
int numThrusters = properties["properties"]["thrusters"].size();
for(int i = 0; i < numThrusters; i++){
int type = properties["properties"]["thrusters"][i]["type"].as<int>();
thrusterType[i] = type;
}
上述代码段为LoadThrusterProperties()函数读取机械模型参数文件中推进器桨叶为正桨(CW)还是反桨(CCW)信息。 LoadThrusterProperties()函数剩余部分内容为读取电机模型文件参数,对应launch文件中的<rosparam command=”load” …>。 四、PWMController::ThrustCB()函数 每收到一个推进器推力矩阵信息,通过PWMController::Thrust2pwm()函数计算每个推机器需要的PWM波信息,然后发布。 五、PWMController::Thrust2pwm()函数 函数参数如下: Thrust2pwm(推进器应提供的推力,桨叶类型,推进器名称)。 函数输出为当前推进器提供推力所需的pwm波值,int型。 以demo中为例,电机模型文件thruster_force_vs_pwm.yaml中推进器推力与pwm波间关系在不同的推力区间有不同的映射关系,每个区段是线性的。Thrust2pwm()函数就是实现了该分段函数。